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

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


run("devices/RobotTCP")


simulation = True
joint_forces = False


class RobotSC(RobotTCP):    
    def __init__(self, name, server, timeout = 1000, retries = 1):
        RobotTCP.__init__(self, name, server, timeout, retries)         
        
    def mount(self, puck, sample):
        return self.execute('mount',segment, puck, sample)   

    def firstmount(self, puck, sample):
        return self.execute('firstmount', segment, puck, sample)

    def unmount(self, puck, sample):
        return self.execute('unmount',segment, puck, sample)
       
    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
        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")
                
        

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

robot.high_level_tasks = ["mount", "firstmount"]
robot.set_tool(TOOL_CALIBRATION)
robot.setPolling(500)

#robot.set_monitor_speed(20)



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)   


    
"""