import time
import ch.psi.pshell.device.PositionerConfig as PositionerConfig

class LinearAxisMotor(PositionerBase):
    def __init__(self, robot, name = "z_lin"):
        PositionerBase.__init__(self, name, PositionerConfig())
        self.robot = robot

    #ATTENTION: Always initialize linear axis motors before scanning (or call robot.set_lin_axis_motor_enabled(True))
    def doInitialize(self):
        self.setCache(self.doRead(), None)

    def doRead(self):
        return float("nan") if self.robot.linear_axis_destination is None else float(self.robot.linear_axis_destination[self.name])

    def doWrite(self, value):        
        if self.robot.linear_axis_destination is not None:
            self.robot.linear_axis_destination.update({self.name: value})
            self.setCache(float(value), None) 
            robot.evaluate("n_moveAbsPos = " + str(value))
            robot.evaluate("setEcAo(ecao_posSet,n_moveAbsPos)")

    def doReadReadback(self):
        return float("nan") if self.robot.linear_axis_pos is None else float(self.robot.linear_axis_pos[self.name])

    def stop(self):
        robot.evaluate("setEcDo(ecdo_linMotStop,true)")
        time.sleep(0.1)
        robot.evaluate("setEcDo(ecdo_linMotStop,false)")


ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"]
class RobotCartesianMotor(PositionerBase):
    def __init__(self, robot, index):
         PositionerBase.__init__(self, ROBOT_MOTORS[index], PositionerConfig())
         self.index = index
         self.robot = robot
        
    #ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True))
    def doInitialize(self):
        self.setCache(self.doRead(), None)
                  
    def doRead(self):
        return float("nan") if self.robot.cartesian_destination is None else float(self.robot.cartesian_destination[self.name])

    def doWrite(self, value):        
        if self.robot.cartesian_destination is not None:
            self.setCache(float(value), None) 
            robot.move_cartesian(**{self.name:value})
        
    def doReadReadback(self):
        return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.name])

    def stop(self):
        robot.stop()
        robot.reset_motion()
        robot.resume()

    def move_done(self):
        """once move done, do readback update and send event and readback"""

ROBOT_SPHERICAL_MOTORS = ["r" , "gamma", "delta"]

class RobotSphericalMotor (PositionerBase):
    def __init__(self, robot, index):
         PositionerBase.__init__(self, ROBOT_SPHERICAL_MOTORS[index], PositionerConfig())
         self.index = index
         self.robot = robot
        
    def doInitialize(self):
        self.setCache(self.doRead(), None)
                  
    def doRead(self):
        return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name]

    def doWrite(self, value):
        if self.robot.spherical_destination is not None:
            self.setCache(float(value), None) 
            robot.move_spherical(**{self.name:value})
        
    def doReadReadback(self):
        return float("nan") if self.robot.spherical_pos is None else robot.spherical_pos[self.name]

    def stop(self):
        robot.stop()
        robot.reset_motion()
        robot.resume()

ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"]
class RobotJointMotor (PositionerBase):
    def __init__(self, robot, index):
         PositionerBase.__init__(self, ROBOT_JOINT_MOTORS[index], PositionerConfig())
         self.index = index
         self.robot = robot         
            
    def doInitialize(self):        
        self.setpoint = self.doReadReadback()
        self.setCache(self.doRead(), None)        
                  
    def doRead(self):        
        return self.setpoint

    def doWrite(self, value):                
        #print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value)
        self.setpoint = value
        joint = self.robot.herej()
        joint[self.index] = value
        self.robot.set_jnt(joint, name="tcp_j")
        self.robot.movej("tcp_j", self.robot.tool , DESC_DEFAULT)       
    
    def doReadReadback(self):
        return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.name])