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


def on_motion_events(Motor):
    def on_combined_motion(self, changed_motors):
        s = "Combining " + str(len(changed_motors)) + " motions to new target:\n"
        for k, v in changed_motors.items():
            s += "{:8} {:6}  -->  {}\n".format(k,v[0], v[1])
        get_context().sendEvent("motion", s)

    def on_coordinate_system_change(self, old, new):
        s = "User changed coordinate system " + old + "  -->  " +new + ":\n"
        get_context().sendEvent("motion", s)
        
    Motor.on_combined_motion = on_combined_motion
    Motor.on_coordinate_system_change = on_coordinate_system_change
    return Motor





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

    @property
    def target_coordinates(self):
        return {k: m.setpoint for k,m in robot.linear_axis_motors.items()}
        
    #ATTENTION: Always initialize linear axis motors before scanning (or call robot.set_lin_axis_motor_enabled(True))
    def doInitialize(self):
        self.setpoint = self.doReadReadback()
        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.setpoint = value
            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 len(self.robot.linear_axis_pos)==0 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)")
        
@on_motion_events
class RobotCartesianMotor(PositionerBase):
    def __init__(self, robot, name):
         PositionerBase.__init__(self, name, PositionerConfig())
         self.robot = robot


    @property
    def target_pos(self):
        return {k: m.setpoint for k,m in robot.cartesian_motors.items()}
    @target_pos.setter
    def target_pos(self, value):
        for k,m in robot.cartesian_motors.items():
            m.setpoint = value[k] if k in value.keys() else robot.cartesian_pos[k]
            
    #ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True))
    def doInitialize(self):
        self.setpoint = self.doReadReadback()
        self.setCache(self.doRead(), None)
                  
    def doRead(self):
        return self.doReadReadback()

    def doWrite(self, value):        
        if self.robot.cartesian_destination is not None:
            self.setpoint = value
            self.setCache(float(value), None) 
            changed_motors = {k: [self.robot.cartesian_pos[k], self.target_pos[k]] for k in self.target_pos.keys() if round(self.robot.cartesian_pos[k], 2) != round(self.target_pos[k], 2)}
            if (len(changed_motors) > 1) & (self.robot.last_remote_motion == "cartesian"):
                self.robot.evaluate("resetMotion()")
                target = self.target_pos
                print("moving to combined", target)
                self.on_combined_motion(changed_motors)
            else:
                if self.robot.last_remote_motion != "cartesian":
                    if not self.robot.empty:
                        self.robot.evaluate("resetMotion()")
                    self.on_coordinate_system_change(self.robot.last_remote_motion, "cartesian")
                target = {self.name:value}
                self.target_pos = target
                print("moving to", target)
            robot.move_cartesian(**target)
            self.robot.last_remote_motion = "cartesian"
        
    def doReadReadback(self):
        return float("nan") if len(self.robot.cartesian_pos)==0 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"""

@on_motion_events
class RobotSphericalMotor (PositionerBase):
    def __init__(self, robot, name):
         PositionerBase.__init__(self, name, PositionerConfig())
         self.robot = robot

    @property
    def target_pos(self):
        return {k: m.setpoint for k,m in robot.spherical_motors.items()}
    @target_pos.setter
    def target_pos(self, value):
        for k,m in robot.spherical_motors.items():
            m.setpoint = value[k] if k in value.keys() else robot.spherical_pos[k]
            
    def doInitialize(self):
        self.setpoint = self.doReadReadback()
        self.setCache(self.doRead(), None)
                  
    def doRead(self):
        return self.doReadReadback()

    def doWrite(self, value):
        if self.robot.spherical_destination is not None:
            self.setpoint = value
            self.setCache(float(value), None) 
            changed_motors = {k: [self.robot.spherical_pos[k], self.target_pos[k]] for k in self.target_pos.keys() if round(self.robot.spherical_pos[k], 2) != round(self.target_pos[k], 2)}
            if (len(changed_motors) > 1) & (self.robot.last_remote_motion == "spherical"):
                self.robot.evaluate("resetMotion()")
                target = self.target_pos
                print("moving to combined", target)
                self.on_combined_motion(changed_motors)
            else:
                if self.robot.last_remote_motion != "spherical":
                    if not self.robot.empty:
                        self.robot.evaluate("resetMotion()")
                    self.on_coordinate_system_change(self.robot.last_remote_motion, "spherical")
                target = {self.name:value}
                self.target_pos = target
                print("moving to", target)
            robot.move_spherical(**target)
            self.robot.last_remote_motion = "spherical"

    def doReadReadback(self):
        return float("nan") if len(self.robot.spherical_pos)==0 else robot.spherical_pos[self.name]

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

@on_motion_events
class RobotJointMotor (PositionerBase):
    def __init__(self, robot, name):
         PositionerBase.__init__(self, name, PositionerConfig())
         self.robot = robot

    @property
    def target_pos(self):
        return {k: m.setpoint for k,m in robot.joint_motors.items()}
    @target_pos.setter
    def target_pos(self, value):
        print(value)
        for k,m in robot.joint_motors.items():
            m.setpoint = value[k] if k in value.keys() else robot.joint_pos[k]

    def doInitialize(self):        
        self.setpoint = self.doReadReadback()
        self.setCache(self.doRead(), None)        
                  
    def doRead(self):        
        return self.doReadReadback()

    def doWrite(self, value):
        self.setpoint = value
        changed_motors = {k: [self.robot.joint_pos[k], self.target_pos[k]] for k in self.target_pos.keys() if round(self.robot.joint_pos[k], 2) != round(self.target_pos[k], 2)}
        if (len(changed_motors) > 1) & (self.robot.last_remote_motion == "joint"):
            self.robot.evaluate("resetMotion()")
            target = self.target_pos
            print("moving to combined", target)
            self.on_combined_motion(changed_motors)
        else:
            if self.robot.last_remote_motion != "joint":
                if not self.robot.empty:
                    self.robot.evaluate("resetMotion()")
                self.on_coordinate_system_change(self.robot.last_remote_motion, "joint")
            target = {self.name:value}
            self.target_pos = target
            print("moving to", target)
        self.robot.move_joint(**target)
        self.robot.last_remote_motion = "joint"

    def doReadReadback(self):
        return self.robot.get_joint_pos()[self.name] if len(self.robot.joint_pos)==0 else float(self.robot.joint_pos[self.name])
        
    def stop(self):
        robot.stop()
        robot.reset_motion()
        robot.resume()