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 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 release_safety():
    pass

def system_check(robot_move=True):
    pass