import threading

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 = []
         
    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_trf(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_tr(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        

    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_trf()

    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):
        self.evaluate("enablePower()")
        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()")

    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 _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 resetMotion(self):
        self.evaluate("resetMotion()")

    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.powered
        
    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):
        return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")")

    def point_to_joint(self, tool, initial_joint, point):
        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 + ")")
    
    #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 = 2000)

    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)        
        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 (not self.settled) or (self.current_task is not None): self.setState(State.Busy)
        if 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._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[6] if len(sts)>6 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)):
                self.current_task = None
            self._update_state()              
            self.setCache({"powered": self.powered,
                           "speed": self.speed,
                           "empty": self.empty,
                           "settled": self.settled,
                           "task": self.current_task }, None)

            #print time.time() - start                        
        except:
            if self.state != State.Offline:
                print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
            self.setState(State.Offline)            

    #High-level, exclusive motion task.
    def start_task(self, program, *args,  **kwargs):
        for task in (self.high_level_tasks + [program]):
            if (task is not None) and (self.get_task_status(task)[0]>=0):
                raise Exception("Ongoing high-level task: " + task)
        self.current_task = program
        self.task_create(program, *args, **kwargs)

    def stop_task(self):
        for task in (self.high_level_tasks + [self.current_task]):
            if (task is not None) and (self.get_task_status(task)[0]>=0):
                self.task_kill(task)

    def on_event(self,ev):
        pass