TOOL_CALIBRATION = "tCalib"
TOOL_SUNA= "tSuna"
TOOL_DEFAULT= TOOL_SUNA

DESC_FAST = "mFast"
DESC_SLOW = "mSlow"
DESC_SCAN = "mScan"
DESC_DEFAULT = DESC_FAST


DEFAULT_ROBOT_POLLING = 500
TASK_WAIT_ROBOT_POLLING = 50


run("devices/RobotTCP")


simulation = False

joint_forces = False


class RobotSC(RobotTCP):    
    def __init__(self, name, server, timeout = 1000, retries = 1):
        RobotTCP.__init__(self, name, server, timeout, retries)   
        self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "robotRecover", "moveDewar", "movePark"])
        self.setPolling(DEFAULT_ROBOT_POLLING)      
        self.known_points = ["pHome", "pPark", "pDewarHome", "pGonioHome", "pDewarWait"]
        
    def get_dewar(self, segment, puck, sample):
        segment = self.toSegmentNumber(segment)
        self.assert_dewar()
        self.start_task('getDewar',segment, puck, sample) 
        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
        self.assert_dewar()

    def put_dewar(self, segment, puck, sample):
        segment = self.toSegmentNumber(segment)
        self.assert_dewar()
        self.start_task('putDewar',segment, puck, sample) 
        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
        self.assert_dewar()

    def put_gonio(self):    
        self.start_task('putGonio',segment, puck, sample) 
        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
        #TODO

    def get_gonio(self):
        self.start_task('getGonio') 
        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
        #TODO

    def robot_recover(self):
        self.start_task('robotRecover')
        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    

    def move_dewar_wait(self):
        self.start_task('moveDewar')  
        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
        self.assert_dewar_wait()

    def move_park(self):
        self.start_task('movePark')   
        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
        self.assert_park()

    def toSegmentNumber(self, segment):
        if type(segment) == str:
            segment = ord(segment.upper()) - ord('A') +1
        return segment
    
       
    def on_event(self,ev):
        #print "EVT: " + ev
        pass
    
    def doUpdate(self):    
        #start = time.time()        
        RobotTCP.doUpdate(self)
        global simulation
        if not simulation:
            if joint_forces:
                if self.state != State.Offline:
                    self.get_joint_forces()
                    for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]: 
                        dev.update()        
        #print time.time() -start

    def start_task(self, program, *args,  **kwargs):
        #TODO: Check safe position
        return RobotTCP.start_task(self, program, *args,  **kwargs)

    def stop_task(self):
        RobotTCP.stop_task(self)
        #TODO: Restore safe position          
          

    def set_remote_mode(self):
        robot.set_profile("remote")
                
    def set_local(self):
        robot.set_profile("default")

    def is_home(self):
        return  self.is_in_point("pHome")       
    
    def is_park(self):
        return  self.is_in_point("pPark")   
    
    def is_dewar_home(self):
        return  self.is_in_point("pDewarHome")

    def is_dewar_wait(self):
        return  self.is_in_point("pDewarWait")   

    def is_gonio(self):
        return  self.is_in_point("pGonioHome")   
                
    def is_cleared(self):
        return self.is_home() or self.is_park() or self.is_dewarHome() or self.is_dewarWait()

    def assert_home(self):
        self.assert_in_point("pHome")       
    
    def assert_park(self):
        self.assert_in_point("pPark")   
    
    def assert_dewar_home(self):
        self.assert_in_point("pDewarHome")

    def assert_dewar_wait(self):
        self.assert_in_point("pDewarWait")   

    def assert_gonio(self):
        self.assert_in_point("pGonioHome")   

    def assert_cleared(self):
        if not self.is_cleared():
            raise Exception("Robot not in cleared position")

    def get_current_point(self):      
        ret = robot.is_in_points(*self.known_points, tolerance = 20)
        for i in range(len(ret)):
            if ret[i] == True:
                return self.known_points[i]
        return None
        

if simulation:
    #add_device(RobotSC("robot","129.129.126.92:1000"),force = True)
    add_device(RobotSC("robot","129.129.110.81:1000"),force = True)
else:
    add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) 

#robot.set_monitor_speed(20)
robot.set_default_desc(DESC_DEFAULT)


class jf1(ReadonlyRegisterBase):
    def doRead(self):
        return  None if robot.joint_forces == None else robot.joint_forces[0]
class jf2(ReadonlyRegisterBase):
    def doRead(self):
        return  None if robot.joint_forces == None else robot.joint_forces[1]
class jf3(ReadonlyRegisterBase):
    def doRead(self):
        return  None if robot.joint_forces == None else robot.joint_forces[2]
class jf4(ReadonlyRegisterBase):
    def doRead(self):
        return  None if robot.joint_forces == None else robot.joint_forces[3]
class jf5(ReadonlyRegisterBase):
    def doRead(self):
        return  None if robot.joint_forces == None else robot.joint_forces[4]
class jf6(ReadonlyRegisterBase):
    def doRead(self):
        return  None if robot.joint_forces == None else robot.joint_forces[5]
class jfc(ReadonlyRegisterBase):
    def doRead(self):
        if robot.joint_forces == None:
            return   float('NaN')  
        if robot.powered == False:
            return  float('NaN') 
        return (robot.joint_forces[1]+74)/4  + (robot.joint_forces[2]+30)/4 + (robot.joint_forces[4]-0.8)/0.2

if joint_forces:
    add_device(jf1(), force = True)   
    add_device(jf2(), force = True)   
    add_device(jf3(), force = True)   
    add_device(jf4(), force = True)   
    add_device(jf5(), force = True)   
    add_device(jf6(), force = True)   
    add_device(jfc(), force = True)   



    
"""