POSITION_TOLERANCE = 50

def enable_motion():
    """
    Check safety and enable arm power if in remote mode
    """
    auto = robot.working_mode != "manual"
    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") 
            
    if auto:
        if not robot.state.isNormal():
            raise Exception("Cannot enable power: robot state is " + str(robot.state))
        robot.enable()

def set_hexiposi(pos, force = False):
    """
    Set the hexiposi position in remote mode, or wait for it to be set in manual mode
    """
    if force == False:
        if hexiposi.position == pos:
            return 
        
    if robot.working_mode == "manual":
        set_status("Move Hexiposi to position " + str(pos) + " ...")
        try:
            hexiposi.waitInPosition(pos, -1)
        finally:
            set_status(None)    
    else:
        hexiposi.move(pos)

#Can be used if cover has following error (no checking readback)
def _set_hexiposi(pos):
    hexiposi.moveAsync(pos)
    time.sleep(1.0)
    hexiposi.waitReady(-1)
            

def visual_check_hexiposi(segment):
    #if robot.working_mode == "manual" ?
    if hexiposi.moved:
        #Clearing for image processing
        if not robot.is_park():
            print "Moving robot to park to clear camera view..."
            robot.move_park()        
        assert_img_in_cover_pos(segment)    
        hexiposi.moved = False           



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_home():        
    #robot.reset_motion("jHome")
    robot.movej("pHome", robot.tool , DESC_SCAN)    
    wait_end_of_move()

def move_to_laser():
    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("pHome")
    if d<0:
        raise Exception ("Error calculating distance to home: " + str(d))
    if d<POSITION_TOLERANCE:
        print "FROM HOME"
        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 assertValidAddress(segment, puck, sample):
    if is_string(segment):
        segment = ord(segment.upper()) - ord('A') +1
    if segment<=0 or segment >6:
        raise Exception ("Invalid segment")
    if puck<=0 or puck >5:
        raise Exception ("Invalid puck")
    if sample<=0 or sample >16:
        raise Exception ("Invalid sample")