import copy
import socket
import logging
from robot.mech_robot import *
from robot.no_moveid_robot import NoMoveIdRobot
from .epson_monitor import EpsonMonitor
from util.pose import *
CMD_SET_DO = 5


class EpsonRobot(NoMoveIdRobot):
    ROBOT_VENDOR = "epson"
    axes = "rzyx"
    _MONITOR_PORT = 1001
    _MOTION_PORT = 1000

    def __init__(self, host, **kwargs):
        super().__init__(**kwargs)
        self._motion_connect = socket.create_connection((host, self._MOTION_PORT), timeout=2)
        self._monitor_connect = socket.create_connection((host, self._MONITOR_PORT), timeout=2)
        self._monitor_connect.settimeout(None)
        self._motion_connect.settimeout(None)
        self._monitor = self.set_monitor()

    def set_monitor(self):
        return EpsonMonitor(self, self._monitor_connect, self.dof, self.is_linkage_robot)

    def _pack_stop(self):
        return ("{}," + "\r\n").format(CMD.STOP).encode()

    def _pack_move(self, move_param):
        motion_type, target_jps, vel, acc, _, _ = super().unpack_move(move_param)
        target = copy.deepcopy(target_jps)
        if self.is_linkage_robot:
            target[2] -= target[1]
        target = [round(j, 3) for j in target]
        if self.dof == 4:
            assert len(target) == 4
            target += [0, 0]

        assert len(target) == 6
        if motion_type == "J" or motion_type == "L":
            motion_cmd = ("{}," * 10 + "\r\n").format(to_motion_type(motion_type), *target, vel, acc, acc)
        else:
            raise TypeError("motion_type error")
        return motion_cmd.encode()

    def _pack_set_digital_out(self, port, value):
        return ("{}," * 3 + "\r\n").format(CMD.SET_DO, port, int(value)).encode()

    def to_self_pose(self, mmind_pose):
        e = transform_list(list(euler.quat2euler([x * self.unit_per_radian for x in mmind_pose[3:]], axes=self.axes)),
                           self.unit_per_radian)
        return transform_pose(list(mmind_pose[:3]), self.unit_per_meter) + e

    def set_tcp_self_unit(self, tcp_to_flange, index):
        with self._motion_lock:
            self._send_program(("{}," * 7 + "\r\n").format(CMD.SET_TCP, *tcp_to_flange).encode())

    def disconnect(self):
        logging.info("Disconnect robot.")
        self.set_connect_state(False)
        try:
            self._motion_connect.close()
            self.status_connect.close()
            self._stop_flag = True
        except Exception as e:
            logging.info("Disconnect error!")

    def get_pose_self_unit(self, cmd):
        with self._motion_lock:
            pose_str = self._send_program((str(cmd) + "\r\n").encode()).decode().split(",")[1:]
        pose = [float(p) for p in pose_str]
        logging.info("cmd:{}:{}".format(cmd, list(pose)))
        return pose

    def get_tcp_self_unit(self):
        return self.get_pose_self_unit(CMD.GET_TOOL)

    def getFL_self_unit(self, wait=False): # get flange pose
        pose = Pose.from_list(self.getL())
        tool = Pose.from_list(self.get_tcp())
        flange_pose = (pose * (~tool)).to_list()
        # print("pose = ", pose)
        # print("tool = ", tool)
        # print("flange_pose = ", flange_pose)
        return flange_pose

    def getL_self_unit(self, wait=False):
        return self.get_pose_self_unit(CMD.GET_TCP)

    def destroy_robot(self):
        self._monitor.stop_monitor = True
        self.disconnect()