from ch.psi.pshell.serial import TcpDevice

import threading

FRAME_DEFAULT = "world"



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



ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"]


class RobotCartesianMotor (PositionerBase):
    def __init__(self, robot, index):
         PositionerBase.__init__(self, robot.name + "_" + 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(None, None)
        self.setCache(self.doRead(), None)
                  
    def doRead(self):
        return float("nan") if self.robot.cartesian_destination is None else float(robot.cartesian_destination[self.index])

    def doWrite(self, value):        
        if self.robot.cartesian_destination is not None:
            print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value)
            self.robot.updating = True
            try:
                robot.cartesian_destination[self.index] = float(value)  
                self.robot.set_pnt(robot.cartesian_destination , "tcp_p")        
                self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN)        
            except:
                self.robot.updating = False
        
    def doReadReadback(self):
        #return float(self.robot.get_cartesian_pos(self._get_tool(),self.frame)[self.index])
        return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.index])




ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"]


class RobotJointMotor (PositionerBase):
    def __init__(self, robot, index):
         PositionerBase.__init__(self, robot.name + "_" + 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.updating = True
        try:
            self.robot.set_jnt(joint, name="tcp_j")
            self.robot.movej("tcp_j", self.robot.tool , DESC_SCAN)       
        finally:
            self.robot.updating = False

    
    def doReadReadback(self):
        return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.index])
        
                

class RobotTCP(TcpDevice, Stoppable):    
    def __init__(self, name, server, timeout = 1000, retries = 1):
         TcpDevice.__init__(self, name, server) 
         self.timeout =  timeout       
         self.retries = retries
         self.header = None
         self.trailer = "\n"
         self.array_separator = '|'
         self.cmd_separator = ' '
         self.msg_id = 0    
         self.working_mode = "invalid"     
         self.status = "invalid" 
         self.powered = None
         self.settled = None
         self.empty = None
         self.working_mode = None
         self.status = None
         self.lock = threading.Lock()
         self.joint_forces = None
         self.current_task = None
         self.high_level_tasks = []         
         self.cartesian_destination = None  
         self.cartesian_pos = None  
         self.joint_pos = None  
         self.cartesian_motors_enabled = False
         self.cartesian_motors = []
         self.joint_motors_enabled = False
         self.joint_motors = []
         self.tool = None
         self.frame = FRAME_DEFAULT
         self.updating = False
                

    def doInitialize(self):           
        super(RobotTCP, self).doInitialize()
        self.reset = True      


    def set_tool(self,tool):
        self.tool = tool
        if self.cartesian_motors_enabled:
            self.cartesian_pos = self.get_cartesian_pos()                
            for m in self.cartesian_motors:
                m.initialize()
                m.update()                
    
    
    def get_tool(self):    
        return self.tool
        
         
    def _sendReceive(self, msg_id, msg = "", timeout =  None):  
        tx = self.header if (self.header != None) else  ""
        tx = tx + msg_id + " " + msg        
        if (len(tx)>127):
            raise Exception("Exceeded maximum message size")
        self.getLogger().finer("TX = '" +  str(tx)+ "'")
        if (self.trailer != None): tx = tx + self.trailer
        rx = self.sendReceive(tx, msg_id, self.trailer , 0,  self.timeout if timeout is None  else timeout, self.retries)
        rx=rx[:-1] #Remove 0A
        self.getLogger().finer("RX = '" + str(rx) + "'")
        if rx[:3] != msg_id:                            
            if (time.time()-start) >= timeout:
                raise Exception("Received invalid message id: " + str(rx[:3]) + " - expecting:" + msg_id )
        if len(rx)<4:
            raise Exception("Invalid message size: " + str(len(rx)) )        
        if rx[3] == "*":
            raise Exception(rx[4:])
        return rx[4:]
    
    def call(self, msg, timeout =  None):                
        self.lock.acquire()
        try:
            id = "%03d" % self.msg_id
            self.msg_id = (self.msg_id+1)%1000        
            return self._sendReceive(id, msg, timeout)        
        finally:
            self.lock.release()
    
    def execute(self, command, *args, **kwargs):        
        timeout = None if (kwargs is None) or (not kwargs.has_key("timeout")) else kwargs["timeout"]                
        msg = str(command)
        if len(args)>10:
            raise Exception("Exceeded maximum number of parameters")
        for i in range(len(args)):            
            msg += (self.cmd_separator if  (i==0) else self.array_separator) + str(args[i])                 
        rx = self.call(msg, timeout)
        if rx.count(self.array_separator)>0:
            return rx.split(self.array_separator)
        return rx
         
    def evaluate(self, cmd, timeout=None):
        ret = self.execute('eval', cmd, timeout=timeout)
        if type(ret) is str:
            if ret.strip() != "": raise Exception(ret)

    def get_var(self, name):
        return self.execute('get_var', name)
    
    #Makes app crash
    #def get_str(self, name='s'):
    #    return self.execute('get_str', name)

    def get_arr(self, name, size):
        return self.execute('get_arr', name, size)

    def get_bool(self, name = "tcp_b"):
        return True if (self.execute('get_bool', name).strip() == '1') else False        

    def get_int(self, name ="tcp_n"):
        return int(self.get_var(name))

    def get_float(self, name ="tcp_n"):
        return float(self.get_var(name))

    def get_int_arr(self, size, name="tcp_a"):
        # not working. A Jython bug in PyUnicaode?
        #return [int(x) for x in self.get_arr("tcp_a", size)] 
        ret = []
        a=self.get_arr(name, size)
        for i in range(size):
            ret.append(int(a[i]))
        return ret

    def get_float_arr(self, size, name="tcp_a"):
        #return [float(x) for x in self.get_arr("tcp_a", size)]        
        a=self.get_arr(name, size)
        ret = []; 
        for i in range(size): ret.append(float(a[i])); 
        return ret        
    
    def get_trsf(self, name="tcp_t"):
        a = self.execute('get_trf', name)
        ret = []
        for i in range(6): ret.append(float(a[i]))
        return ret        

    def set_trsf(self, l, name="tcp_t"):             
        self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")

    def get_jnt(self, name="tcp_j"):
        a = self.execute('get_jnt', name)
        ret = []
        for i in range(6): ret.append(float(a[i]))
        return ret   

    def set_jnt(self, l, name="tcp_j"):             
        self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")

    def get_pnt(self, name="tcp_p"):
        #a = self.execute('get_pnt', name)
        #ret = []
        #for i in range(6): ret.append(float(a[i]))
        #return ret        
        return self.get_trsf(name+".trsf")
        
    #trsf = (x,y,z,rx,ry,rz)
    #TODO: config = (shoulder, elbow, wrist)
    def set_pnt(self, trsf, name="tcp_p", config=None):
        self.set_trsf(trsf, name+".trsf")       

    def get_tool_trsf(self, name=None):
        if name is None: 
            name = self.tool
        return self.get_trsf(name+".trsf")

    def set_tool_trsf(self, trsf, name=None):
        if name is None: 
            name = self.tool
        self.set_trsf(trsf, name+".trsf")       

    def eval_int(self, cmd):
        self.evaluate("tcp_n=" + cmd)
        return self.get_int()

    def eval_float(self, cmd):
        self.evaluate("tcp_n=" + cmd)
        return self.get_float()

    def eval_bool(self, cmd):
        self.evaluate("tcp_b=" + cmd)
        return self.get_bool()

    #def eval_str(self, cmd):
    #    self.evaluate("s=" + cmd)
    #    return self.get_str()

    def eval_jnt(self, cmd):
        self.evaluate("tcp_j=" + cmd)
        return self.get_jnt()

    def eval_trf(self, cmd):
        self.evaluate("tcp_t=" + cmd)
        return self.get_trsf()

    def eval_pnt(self, cmd):
        self.evaluate("tcp_p=" + cmd)
        return self.get_pnt()


    #Robot control        
    def is_powered(self):
        self.powered = self.eval_bool("isPowered()")
        return self.powered

    def enable(self):
        if not self.is_powered():
            self.evaluate("enablePower()")
            time.sleep(1.0)
            if not self.is_powered():
                raise Exception("Cannot enable power")
    
    #waits for power to be actually cut off
    def disable(self):
        self.evaluate("disablePower()", timeout=5000)

    def get_monitor_speed(self):
        self.speed =  self.eval_int("getMonitorSpeed()")
        return self.speed

    def set_monitor_speed(self, speed):
        ret = self.eval_int("setMonitorSpeed(" + str(speed) + ")")
        if (ret==-1): raise Exception("The robot is not in remote working mode")
        if (ret==-2): raise Exception("The monitor speed is under the control of the operator")
        if (ret==-3): raise Exception("The specified speed is not supported")
    
    def is_calibrated(self):
        return self.eval_bool("isCalibrated()")

    def save_program(self):
        ret = self.execute('save',  timeout=5000)        
        if str(ret) != "0": raise Exception("Error saving program: " + str(ret))
            
    def _update_working_mode(self, mode, status):
        if mode==1:                 
            self.working_mode = "manual"
            self.status = "hold" if status==6 else "move"
        elif mode==2: 
            self.working_mode = "test"
            self.status = "hold" if status==3 else "move"
        elif mode==3: 
            self.working_mode = "local"
            self.status = "hold" if status==2 else "move"
        elif mode==4: 
            self.working_mode = "remote"
            self.status = "hold" if status==2 else "move"
        else:
            self.working_mode = "invalid"
            self.status = "invalid"
    
    def read_working_mode(self):
        try:
            mode =  self.eval_int("workingMode(tcp_a)")          
            status = int(self.get_var("tcp_a[0]"))                        
            self._update_working_mode(mode, status)
            self._update_state()    
        except:
            self.working_mode = "invalid"
            self.status = "invalid"        
        return self.working_mode

    def get_emergency_stop_sts(self):
        st = self.eval_int("esStatus()")        
        if (st== 1): return "active"
        if (st== 2): return "activated"
        return "off"

    def get_safety_fault_signal(self):
        fault = self.eval_bool("safetyFault(s)")
        #if (fault):
        #    return get_str()            
        return fault

    #Motion control
    def stop(self):
        self.evaluate("stopMove()")

    def resume(self):
        self.evaluate("restartMove()")

    def reset_motion(self, joint=None):
        self.evaluate("resetMotion()" if (joint is None) else ("resetMotion(" + joint + ")"))

    def is_empty(self):
        self.empty = self.eval_bool("isEmpty()")
        self._update_state()    
        return self.empty

    def is_settled(self):
        self.settled = self.eval_bool("isSettled()")
        self._update_state()    
        return self.settled
        
    def get_move_id(self):
        return self.eval_int("getMoveId()")

    def set_move_id(self, id):
        return self.evaluate("setMoveId(" + str(id) + " )")

    def get_joint_forces(self):        
        try:
            self.evaluate("getJointForce(tcp_a)")        
            self.joint_forces = self.get_float_arr(6)            
            return self.joint_forces
        except:
            self.joint_forces = None
            raise

    def movej(self, joint_or_point, tool, desc):
        return self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
        
    def movel(self, point, tool, desc):
        return self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")

    def movec(self, point_interm, point_target, tool, desc):
        return self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")")


    #Tool
    #This function can timeout since it synchronizes move. 
    #Better check state before otherwise it can freeze the communication
    def open_tool(self, tool):
        return self.evaluate("open(" + tool + " )", timeout=3000)

    #This function can timeout since it synchronizes move. Better check state before
    #Better check state before otherwise it can freeze the communication
    def close_tool(self, tool):
        return self.evaluate("close(" + tool + " )", timeout=3000)

    #Arm position
    def herej(self):
        return self.eval_jnt("herej()")
    
    def distance_t(self, trsf1, trsf2):    
        return self.eval_float("distance(" + trsf1 + ", " + trsf2 + ")")
    
    def distance_p(self, pnt1, pnt2):    
        return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")   

    def compose(self, pnt, frame, trsf):    
        return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")")

    def here(self, tool, frame):
        return self.eval_pnt("here(" + tool + ", " + frame + ")")

    def joint_to_point(self, tool, frame, joint="tcp_j"):
        return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")")

    def point_to_joint(self, tool, initial_joint="tcp_j", point="tcp_p"):
        if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"):
            return self.get_jnt()

    def position(self, point, frame):
        return self.eval_trf("position(" + point + ", " + frame + ")")
    
    #Profile
    def get_profile(self):
        return self.execute('get_profile', timeout=2000)

    def set_profile(self, name, pwd = ""):
        self.execute('set_profile', str(name), str(pwd), timeout=5000)
    
    
    #Task control
    def task_create(self, program, *args,  **kwargs):        
        program = str(program)
        priority = 10 if (kwargs is None) or (not kwargs.has_key("priority")) else kwargs["priority"]                
        name = str(program if (kwargs is None) or (not kwargs.has_key("name")) else kwargs["name"])                
        
        if self.get_task_status(name)[0] != -1:
            raise Exception("Task already exists: " + name)

        #taskCreate "t1", 10, read(sMessage)
        cmd = 'taskCreate "' + name + '", ' + str(priority) + ', ' + program + '('
        for i in range(len(args)):            
            cmd += str(args[i]) + (',' if  (i<(len(args)-1)) else '')
        cmd+=')'
        self.evaluate(cmd)
        
    def task_suspend(self, name):
        self.evaluate('taskSuspend("' + str(name)+ '")')

    #waits until the task is ready for restart
    def task_resume(self, name):
        self.evaluate('taskResume("' + str(name)+ '",0)', timeout = 2000)

    #waits for the task to be actually killed
    def task_kill(self, name):
        #self.evaluate('taskKill("' + str(name)+ '")', timeout = 5000)
        self.execute('kill', str(name), timeout=5000)

    def get_task_status(self, name):
        code = self.eval_int('taskStatus("' + str(name)+ '")')
        #TODO: String assignments in $exec causes application to freeze
        #status = self
        if code== -1: status = "Stopped"
        elif code== 0: status = "Paused"
        elif code== 1: status = "Running"
        #else: status = self.execute('get_help', code)        
        else: status = "Error"  
        return (code,status)

    def _update_state(self):
        #self.setState(State.Busy if self.status=="move" else State.Ready)
        if self.state==State.Offline:
            print "Communication resumed"
        if self.reset or (self.state==State.Offline):
            self.get_task()
            if self.current_task is not None:
                print "Ongoing task: " + self.current_task            

        if (not self.settled) or (self.current_task is not None): self.setState(State.Busy)
        elif not self.empty: self.setState(State.Paused)
        else: self.setState(State.Ready)                

    def doUpdate(self):        
        try:
            start = time.time()
            #sts = self._sendReceive("EVT").strip().split(self.array_separator)        
            sts = self.execute("get_status", self.current_task)
            self._update_working_mode(int(sts[0]), int(sts[1]))
            self.powered = sts[2] == "1"
            self.speed = int(sts[3])
            self.empty = sts[4] == "1"
            self.settled = sts[5] == "1"                                    
            ev = sts[7] if len(sts)>7 else ""            
            if len(ev.strip()) >0:
                self.getLogger().info(ev)
                self.on_event(ev) 
            #if (self.current_task is not None) and (self.get_task_status(self.current_task)[0]<0):
            if int(sts[6]) < 0:
                self.current_task = None
            self._update_state()              
            self.reset = False
            self.setCache({"powered": self.powered,
                           "speed": self.speed,
                           "empty": self.empty,
                           "settled": self.settled,
                           "task": self.current_task,
                           "mode": self.working_mode,
                           "status": self.status
                           }, None)
            if not self.updating:
                if self.cartesian_motors_enabled:
                    self.cartesian_pos = self.get_cartesian_pos()                
                    for m in self.cartesian_motors:
                        #m.update()
                        m.readback.update()
                else:
                    self.cartesian_pos = None                
                if self.joint_motors_enabled:
                    self.joint_pos = self.herej()
                    for m in self.joint_motors:
                        m.readback.update()
        except:
            if self.state != State.Offline:
                print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
            self.setState(State.Offline)            

    #Cartesian space

    def get_cartesian_pos(self):            
        return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())")
        #self.set_jnt(self.herej())
        #return self.joint_to_point(tool, frame)        

    def get_cartesian_destination(self):
         return self.here(self.tool, self.frame)

    def get_distance_to_pnt(self, name):   
        #self.here(self.tool, self.frame) #???
        self.get_cartesian_pos() 
        return  self.distance_p("tcp_p", name)

    #Cartesian peudo-motors
    def set_motors_enabled(self, value):
        if value !=self.cartesian_motors_enabled:
            self.cartesian_motors_enabled = value
            if value:    
                for i in range(6):
                    self.cartesian_motors.append(RobotCartesianMotor(self, i))
                    add_device(self.cartesian_motors[i], True)        
                self.cartesian_destination = self.get_cartesian_destination()
            else:
                for m in self.cartesian_motors:
                    remove_device(m)
                self.cartesian_motors = []            
                self.cartesian_destination = None
        else:
            if value:
                self.cartesian_destination = self.get_cartesian_destination()
                for m in self.cartesian_motors:
                    m.initialize()                
        

    #Cartesian peudo-motors
    def set_joint_motors_enabled(self, value):
        if value !=self.joint_motors_enabled:
            self.joint_motors_enabled = value
            if value:    
                for i in range(6):
                    self.joint_motors.append(RobotJointMotor(self, i))
                    add_device(self.joint_motors[i], True)                        
            else:
                for m in self.joint_motors:
                    remove_device(m)
                self.joint_motors = []            
                self.joint_destination = None
        else:
            if value:
                self.joint_destination = self.get_joint_destination()
                for m in self.joint_motors:
                    m.initialize()                
        

    #High-level, exclusive motion task.
    def start_task(self, program, *args,  **kwargs):
        tasks = [t for t in self.high_level_tasks]
        if (self.current_task is not None) and (not self.current_task in tasks): 
            tasks.append(pro)
        if not program in tasks: 
            tasks.append(pro)
        for task in tasks:
            if self.get_task_status(task)[0]>=0:
                raise Exception("Ongoing high-level task: " + task)        
        self.task_create(program, *args, **kwargs)
        start = time.time()
        while self.get_task_status(program)[0] < 0:
            if time.time() - start > 5000:
                raise Exception("Cannot start task " + task)
            time.sleep(0.1)        
        self.update()               
        self.current_task = program
        self._update_state()
        
    def stop_task(self):
        tasks = [t for t in self.high_level_tasks]        
        if (self.current_task is not None) and (not self.current_task in tasks): 
            tasks.append(self.current_task)
        for task in tasks:
            #if self.get_task_status(task)[0]>=0:                
                self.task_kill(task)                

    def get_task(self):
        if self.current_task is not None:             
            return self.current_task
        for task in self.high_level_tasks:
            if self.get_task_status(task)[0]>=0:                
                 self.current_task = task
                 return task
        return None


    def on_event(self,ev):
        pass
                    


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

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




simulation = True
joint_forces = False


class RobotSC(RobotTCP):    
    def __init__(self, name, server, timeout = 1000, retries = 1):
        RobotTCP.__init__(self, name, server, timeout, retries) 
        self.tool = TOOL_CALIBRATION
        
    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")
            
    def set_tool(self,tool):
        if tool != self.tool:
            self.tool  = tool
            for dev in (robot_x, robot_y, robot_z, robot_rx, robot_ry, robot_rz):
                dev.initialize()
                dev.update()
    
        

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.setPolling(500)            