POSITION_TOLERANCE = 50


def is_manual_mode():
    """
    return if operating in local mode. TODO: should be based on IO, not on robot flag.
    """    
    return  robot.working_mode == "manual"

def release_safety():
    """
    Release sefety signals acording to working mode
    """
    if air_pressure_ok.take() != True:
        raise Exception("Cannot release safety: air preassure not ok") 
    if n2_pressure_ok.take() != True:
        raise Exception("Cannot release safety: n2 pressure not ok")         
    auto = not is_manual_mode()
    if auto:
        if feedback_psys_safety.read() == False:
            release_psys()  
            time.sleep(0.5)   
            if feedback_psys_safety.read() == False:
                raise Exception("Cannot enable power: check doors")

    if feedback_local_safety.read() == False:
        release_local()    
        time.sleep(0.5)    
        if feedback_local_safety.read() == False:
            raise Exception("Cannot enable power: check sample changer emergency stop button") 
            
def enable_motion():
    """
    Check safety and enable arm power if in remote mode
    """
    release_safety()  
    system_check(robot_move=True)  
    auto = not is_manual_mode()        
    if auto:
        if not robot.state.isNormal():
            raise Exception("Cannot enable power: robot state is " + str(robot.state))
        robot.enable()


def wait_end_of_move():
    robot.update()
    while (not robot.settled) or (not robot.empty) or (not robot.isReady()) :
        time.sleep(0.01)


def move_to_home():        
    #robot.reset_motion("jHome")
    robot.movej("pHome", robot.tool , DESC_SCAN)    
    wait_end_of_move()

def move_to_laser(from_point = "pHome"):
    robot.reset_motion()
    tool = robot.tool
    d = robot.get_distance_to_pnt("pLaser")
    if d<0:
        raise Exception ("Error calculating distance to laser: " + str(d))
    if d<POSITION_TOLERANCE:
        print "FROM LASER"
        robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
        return
    d = robot.get_distance_to_pnt("pLaserHome")
    if d<0:
        raise Exception ("Error calculating distance to laser appro: " + str(d))
    if d<POSITION_TOLERANCE:
        print "FROM APPRO"
        robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
        return
        
    d = robot.get_distance_to_pnt(from_point)
    if d<0:
        raise Exception ("Error calculating distance to " + from_point + ": " + str(d))
    if d<POSITION_TOLERANCE:
        print "FROM " + from_point
        robot.movej("pLaserHome", robot.tool, DESC_DEFAULT)
        robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)    
        return
    raise Exception ("Must be in home position to start move to laser")
    

def update_tool(tool=None, x_offset=0.0, y_offset=0.0, z_offset=0.0):
    #Updating tool:
    t=robot.get_tool_trsf(tool)
    t[0]=t[0] - x_offset
    t[1]=t[1] - y_offset
    t[2]=t[2] - z_offset
    robot.set_tool_trsf(t, tool)
    print "Updated " + (str(robot.tool) if (tool is None) else tool) + ": " + str(t) 
    robot.save_program()


def assert_valid_address(segment, puck, sample):
    if (segment == AUX_SEGMENT) and (puck == 1):
        return 
    if is_string(segment):
        segment = ord(segment.upper()) - ord('A') +1
    if (segment<=0) or (segment >6):
        raise Exception ("Invalid segment: " + str(segment))
    if (puck<=0) or (puck >5):
        raise Exception ("Invalid puck: " + str(puck))
    if (sample<=0) or (sample >16):
        raise Exception ("Invalid sample: " + str(sample))
    if get_puck_dev(segment, puck).isDisabled():
        raise Exception ("Puck is disabled")
        
def assert_valid_sample(sample):
    if (sample<=0) or (sample >16):
        raise Exception ("Invalid sample: " + str(sample))

def get_puck_name(segment, puck):
    try:
        assert_valid_address(segment, puck, 1)
        if type(segment) is int:
            segment = chr( ord('A') + (segment-1))
        elif is_string(segment):
            segment = segment.upper()    
        else:
            return None
        return segment + str(puck)         
    except:
        return None

def get_sample_name(segment, puck, sample):
    puck_name = get_puck_name(segment, puck)
    return None if (puck_name is None) else puck_name + str(sample)


def is_pin_detected_in_scanner():
    samples = []
    for i in range(10):
        samples.append(laser_distance.read())
        time.sleep(0.05)
    av = mean(samples)
    for s in samples:
         if s<=1:
             return False
         if abs(s-av) > 0.1:
             return False
    return True