import json
import socket
import time
import os
import selectors
import threading
from datetime import datetime
import hashlib
from typing import Callable, Dict

from loguru import logger
from pymodbus.client.tcp import ModbusTcpClient
from paramiko import SSHClient, AutoAddPolicy
import plotly.graph_objects as go

from codes import clibs


class HmiRequest:
    def __init__(self):
        self.c: socket.socket
        self.c_xs: socket.socket
        self.t_rx_pkg: threading.Thread
        self.t_rx_pkg_xs: threading.Thread
        self.t_s_heart: threading.Thread
        self.data = self.data_xs = b""
        self.is_connected = True
        self.sock_conn()

    def sock_conn(self):
        count = 0
        while True:
            try:
                self.c = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.c.settimeout(clibs.interval * 3)
                self.c.connect((clibs.ip_addr, clibs.socket_port))
                self.c.setblocking(False)
                self.c_xs = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.c_xs.settimeout(clibs.interval * 3)
                self.c_xs.connect((clibs.ip_addr, clibs.xService_port))
                self.c_xs.setblocking(False)
                for _ in range(3):
                    _ = self.exec_cmd("controller.heart")
                    time.sleep(clibs.interval/4)
                count += 1
                if count > 3:
                    logger.success("hr: HMI connection success")
                    break
            except Exception as e:
                clibs.to_logdb("error", "HmiRequest", str(e))
                logger.warning(f"hr: HMI connection failed, will retry after {clibs.retry_time}s")
                try:
                    self.c.close()
                    self.c_xs.close()
                except Exception as e:
                    clibs.to_logdb("error", "HmiRequest", str(e))
                count = 0
                time.sleep(clibs.retry_time)

        self.t_rx_pkg = threading.Thread(target=self.rx_pkg, args=(self.c,))
        self.t_rx_pkg.daemon = True
        self.t_rx_pkg.start()
        self.t_rx_pkg_xs = threading.Thread(target=self.rx_pkg, args=(self.c_xs,))
        self.t_rx_pkg_xs.daemon = True
        self.t_rx_pkg_xs.start()
        self.t_s_heart = threading.Thread(target=self.s_heart)
        self.t_s_heart.daemon = True
        self.t_s_heart.start()

    def close(self):
        self.is_connected = False
        time.sleep(clibs.interval)
        try:
            self.c.close()
            self.c_xs.close()
        except Exception as e:
            clibs.to_logdb("error", "HmiRequest", str(e))
        finally:
            logger.info("hr: HMI connection has been disconnected")

    def s_heart(self):
        while self.is_connected:
            self.exec_cmd("controller.heart")
            time.sleep(clibs.interval)

    @staticmethod
    def tx_pkg(cmd):
        frm = (len(cmd) + 6).to_bytes(length=2)
        pkg = len(cmd).to_bytes(length=4)
        protocol = int(2).to_bytes()
        reserved = int(0).to_bytes()
        return frm + pkg + protocol + reserved + cmd.encode()

    @staticmethod
    def tx_pkg_xs(cmd):
        return "".join([cmd, "\r"]).encode()

    def rx_pkg(self, sock):
        def to_read(conn, mask):
            data = conn.recv(clibs.MAX_RECEIVED_SIZE)
            if data:
                ip, port = sock.getpeername()
                func_dict[port](data)

            else:
                sel.unregister(conn)
                conn.close()

        func_dict: Dict[int, Callable[[bytes], None]] = {clibs.socket_port: self.get_response, clibs.xService_port: self.get_response_xs}
        try:
            sel = selectors.DefaultSelector()
            sel.register(sock, selectors.EVENT_READ, to_read)
            while self.is_connected:
                events = sel.select()
                for key, mask in events:
                    callback = key.data
                    callback(key.fileobj, mask)
        except Exception as e:
            clibs.to_logdb("error", "HmiRequest", str(e))

    def get_response(self, data: bytes) -> None:
        def get_valid_data():
            nonlocal record, frm_number, pkg_number
            valid_data, is_first_frame = b"", True
            while len(record) != 0:
                if is_first_frame is True:
                    is_first_frame = False
                    valid_data += record[8:8 + frm_number - 6]
                    record = record[8 + frm_number - 6:]

                frm_number = int.from_bytes(record[:2])
                valid_data += record[2:2 + frm_number]
                record = record[2 + frm_number:]
            else:
                if len(valid_data) != pkg_number:
                    logger.error("hr: 数据校验失败，解包有误，需要检查！")
                    raise Exception()
            return str(json.loads(valid_data.decode()))

        self.data += data
        frm_number = int.from_bytes(self.data[0:2])
        pkg_number = int.from_bytes(self.data[2:6])
        protocol = int.from_bytes(self.data[6:7])
        reserved = int.from_bytes(self.data[7:8])

        if frm_number == pkg_number + 6 and protocol == 2 and reserved == 0:
            record_len = pkg_number+8
            record, self.data = self.data[:record_len], self.data[record_len:]
            clibs.to_logdb("debug", "HmiRequest", str(json.loads(record[8:].decode())))

        elif frm_number == clibs.max_frame_size and pkg_number > clibs.max_frame_size - 8 and protocol == 2 and reserved == 0:
            one_or_zero = 1 if (pkg_number - 1018) % clibs.max_frame_size > 0 else 0
            record_len = pkg_number + 8 + ((pkg_number - 1018) // clibs.max_frame_size + one_or_zero) * 2
            if len(self.data) < record_len:
                return
            record, self.data = self.data[:record_len], self.data[record_len:]
            clibs.to_logdb("debug", "HmiRequest", get_valid_data())

        else:
            logger.critical(f"hr: 其他未考虑到的情况：data = {self.data}")
            raise Exception()

    def get_response_xs(self, data: bytes) -> None:
        self.data_xs += data
        res = self.data_xs.split(b"\r", 1)
        if len(res) != 2:
            return

        record, self.data_xs = res
        clibs.to_logdb("debug", "HmiRequest", str(json.loads(record)))

    @staticmethod
    def get_from_id(msg_id: str) -> str:
        f_text, flag, records, time_delay = f"%{msg_id}%", False, "Null", clibs.interval*10
        cmd, ts = msg_id.split("@")
        t = ts.replace("T", " ")
        t_ts = datetime.timestamp(datetime.strptime(t, "%Y-%m-%d %H:%M:%S.%f"))
        t_send = datetime.strftime(datetime.fromtimestamp(t_ts-time_delay), "%Y-%m-%d %H:%M:%S.%f")
        t_receive = datetime.strftime(datetime.fromtimestamp(t_ts+time_delay), "%Y-%m-%d %H:%M:%S.%f")
        for idx in range(time_delay):
            time.sleep(clibs.interval)
            try:
                clibs.lock.acquire(True)
                clibs.cursor.execute(f"SELECT content FROM logs WHERE timestamp BETWEEN '{t_send}' AND '{t_receive}' AND content LIKE '{f_text}'")
                records = clibs.cursor.fetchall()
                flag = True if len(records) == 1 else False
            finally:
                clibs.lock.release()
                if flag is True:
                    return records[0][0]
        else:
            logger.error(f"hr: {time_delay}s内无法找到请求 {msg_id} 的响应！")
            # raise Exception()

    def exec_cmd(self, cmd: str, **kwargs):
        try:
            with open(f"{clibs.base_path}/assets/protocols/{cmd}.json", mode="r", encoding="utf-8") as f_cmd:
                req = json.load(f_cmd)
                t = datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%f")
                req["id"] = f"{cmd}@{t}"
                p_type = req["p_type"]
                del req["p_type"]
        except FileNotFoundError as err:
            logger.error(f"hr: 暂不支持 {cmd} 功能，或确认该功能存在，err = {err}")
            raise Exception()

        if cmd in ["state.set_tp_mode", "overview.set_autoload", "overview.reload", "rl_task.pp_to_main", "rl_task.run", "rl_task.stop", "rl_task.set_run_params", "diagnosis.set_params", "diagnosis.open", "drag.set_params", "controller.set_params", "collision.set_params", "move.set_quickstop_distance", "move.set_params", "move.set_monitor_cfg", "modbus.get_values", "modbus.set_params", "system_io.update_configuration", "diagnosis.get_params", "jog.set_params", "jog.start", "move.stop", "move.quick_turn", "move.set_quickturn_pos", "soft_limit.set_params", "fieldbus_device.set_params", "socket.set_params", "diagnosis.save", "register.set_value"]:
            req["data"].update(kwargs)
        elif cmd in ["safety.safety_area.signal_enable", "safety.safety_area.overall_enable", "safety.safety_area.safety_area_enable", "safety.safety_area.set_param"]:
            req["c"].update(kwargs)
        elif cmd in ["log_code.data.code_list"]:
            req["s"]["log_code.data"]["code_list"] = kwargs["code_list"]
        else:
            pass

        cmd = json.dumps(req, separators=(",", ":"))
        if p_type == 0:
            self.c.send(self.tx_pkg(cmd))
        elif p_type == 1:
            self.c_xs.send(self.tx_pkg_xs(cmd))
        else:
            raise Exception("Unexpected CMD Input")

        time.sleep(clibs.interval/4)
        return req["id"]


class ModbusRequest:
    def __init__(self):
        self.c: ModbusTcpClient
        self.sock_conn()

    def sock_conn(self):
        self.c = ModbusTcpClient(host=clibs.ip_addr, port=clibs.modbus_port)
        if self.c.connect():
            logger.success(f"md: Modbus connection({clibs.ip_addr}:{clibs.modbus_port}) success!")
        else:
            logger.error(f"md: Modbus connection({clibs.ip_addr}:{clibs.modbus_port}) failed!")
            raise Exception()

    def __reg_high_pulse(self, addr: int, desc: str) -> None:
        self.c.write_register(addr, 0)
        time.sleep(clibs.interval/4)
        self.c.write_register(addr, 1)
        time.sleep(clibs.interval/4)
        self.c.write_register(addr, 0)
        logger.info(f"md: {addr}-010 执行{desc}的操作")

    def r_clear_alarm(self):
        self.__reg_high_pulse(40000, "清除告警信息")

    def r_reset_estop(self):
        self.__reg_high_pulse(40001, "复位外部急停状态")

    def r_reset_estop_clear_alarm(self):
        self.__reg_high_pulse(40002, "复位外部急停状态，并清除告警信息")

    def r_motor_on(self):
        self.__reg_high_pulse(40004, "上电")

    def r_motor_off(self):
        self.__reg_high_pulse(40005, "下电")

    def r_motor_on_off(self, action: bool):
        self.c.write_register(40006, action)
        desc = {False: "下电", True: "上电"}
        logger.info(f"md: 40006-{action} 执行{desc[action]}的操作")
        time.sleep(clibs.interval)

    def r_motoron_pp2main_start(self):
        self.__reg_high_pulse(40007, "上电/pp2main/开始运行程序")

    def r_motoron_start(self):
        self.__reg_high_pulse(40008, "上电/开始运行程序")

    def r_pulse_motoroff(self):
        self.__reg_high_pulse(40009, "停止并下电")

    def r_pp2main(self):
        self.__reg_high_pulse(40010, "pp2main")

    def r_program_start(self):
        self.__reg_high_pulse(40011, "启动程序运行")

    def r_program_stop(self):
        self.__reg_high_pulse(40012, "停止程序运行")

    def r_program_start_stop(self, action: bool):
        self.c.write_register(40013, action)
        desc = {False: "停止程序运行", True: "启动程序运行"}
        logger.info(f"md: 40013-{action} 执行{desc[action]}的操作")
        time.sleep(clibs.interval)

    def r_set_program_speed(self, speed: int):
        result = self.c.convert_to_registers(int(speed), self.c.DATATYPE.INT32, "little")
        self.c.write_registers(40014, result)
        logger.info(f"md: 40014 设置程序运行速度为程序给定速度的{speed}%")

    def r_soft_estop(self, action: bool):
        self.c.write_register(40015, action)
        desc = {False: "触发软急停状态", True: "解除软急停状态"}
        logger.info(f"md: 40015-{action} 执行{desc[action]}的操作")
        time.sleep(clibs.interval)

    def r_switch_auto_motoron(self):
        self.__reg_high_pulse(40016, "切换为自动模式并上电")

    def r_switch_auto(self):
        self.__reg_high_pulse(40017, "切换为自动模式")

    def r_switch_manual(self):
        self.__reg_high_pulse(40018, "切换为手动模式")

    def r_switch_auto_manual(self, action: bool):
        self.c.write_register(40019, action)
        desc = {False: "切换为手动模式", True: "切换为自动模式"}
        logger.info(f"md: 40019-{action} 执行{desc[action]}的操作")
        time.sleep(clibs.interval)

    def r_reduced_mode(self, action: bool):
        self.c.write_register(40020, action)
        desc = {False: "退出缩减模式", True: "进入缩减模式"}
        logger.info(f"md: 40020-{action} 执行{desc[action]}的操作")
        time.sleep(clibs.interval)

    def r_switch_safe_region(self, idx: int, enable: bool):
        addr = 40020 + idx
        if enable:
            self.c.write_register(addr, False)
            time.sleep(clibs.interval)
            self.c.write_register(addr, True)
        else:
            self.c.write_register(addr, True)
            time.sleep(clibs.interval)
            self.c.write_register(addr, False)
        desc = {False: "关闭安全区域", True: "打开安全区域"}
        logger.info(f"md: {addr}-{enable} 执行{desc[enable]}[{idx}]的操作")

    def __get_state(self, addr: int, count: int = 1, **kwargs):
        res = self.c.read_holding_registers(addr, count=count).registers[0]
        logger.info(f'md: {addr}-{kwargs["func"]}，结果为{res} {kwargs["desc"]}')
        return res

    @property
    def w_alarm_state(self):
        return self.__get_state(41000, func="告警状态", desc=":--: 0表示无告警，1表示有告警")

    @property
    def w_sta_collision(self):
        return self.__get_state(41001, func="碰撞检测状态", desc=":--: 0表示无碰撞，1表示检测到碰撞")

    @property
    def w_sta_collision_alarm(self):
        return self.__get_state(41002, func="碰撞检测报警", desc=":--: 0表示无碰撞，1表示检测到碰撞")

    @property
    def w_sta_collision_open(self):
        return self.__get_state(41003, func="碰撞检测开启状态", desc=":--: 0表示未开启，1表示已开启")

    @property
    def w_controller_is_running(self):
        return self.__get_state(41004, func="控制器运行状态", desc=":--: 0表示未运行，1表示已运行")

    @property
    def w_encoder_low_battery(self):
        return self.__get_state(41005, func="编码器低电压报警状态", desc=":--: 0表示正常，1表示异常")

    @property
    def w_sta_error_code(self):
        result = self.c.read_holding_registers(41006, count=2)
        error_code = self.c.convert_from_registers(registers=result.registers, data_type=self.c.DATATYPE.INT16, word_order="little")[0]
        return error_code+30000

    @property
    def w_sta_estop(self):
        return self.__get_state(41007, func="外部急停状态", desc=":--: 0表示非急停，1表示急停")

    @property
    def w_sta_heartbeat(self):
        return self.__get_state(41008, func="心跳电平状态", desc=":--: 0表示低电平，1表示高电平")

    @property
    def w_sta_home(self):
        return self.__get_state(41009, func="当前点是否是home点", desc=":--: 0表示非home点，1表示home点")

    @property
    def w_sta_motor(self):
        return self.__get_state(41011, func="上电状态", desc=":--: 0表示下电，1表示上电")

    @property
    def w_sta_operation_mode(self):
        return self.__get_state(41012, func="运行模式", desc=":--: 0表示手动，1表示自动")

    @property
    def w_sta_program(self):
        return self.__get_state(41013, func="程序运行状态", desc=":--: 0表示未运行，1表示正在运行")

    @property
    def w_sta_program_full(self):
        result = self.c.read_holding_registers(41014, count=2)
        return self.c.convert_from_registers(registers=result.registers, data_type=self.c.DATATYPE.INT16, word_order="little")[0]

    @property
    def w_sta_program_not_run(self):
        return self.__get_state(41015, func="程序未运行状态", desc=":--: 0表示正在运行，1表示未运行")

    @property
    def w_sta_program_reset(self):
        return self.__get_state(41016, func="程序指针复位状态", desc=":--: 0表示未复位，1表示已复位")

    @property
    def w_sta_program_speed(self):
        result = self.c.read_holding_registers(41017, count=2)
        return self.c.convert_from_registers(registers=result.registers, data_type=self.c.DATATYPE.INT16, word_order="little")[0]

    @property
    def w_sta_reduce_mode(self):
        return self.__get_state(41018, func="缩减模式状态", desc=":--: 0表示非缩减模式，1表示缩减模式")

    @property
    def w_sta_robot_is_busy(self):
        return self.__get_state(41019, func="机器人忙碌状态", desc=":--: 0表示空闲，1表示忙碌")

    @property
    def w_sta_robot_is_moving(self):
        return self.__get_state(41020, func="机器人运动状态", desc=":--: 0表示非运动状态，1表示正在运动")

    @property
    def w_safe_door_state(self):
        return self.__get_state(41021, func="安全门状态", desc=":--: 0表示未触发，1表示已触发")

    @property
    def w_sta_safe_jnt_pos(self):
        res = [self.c.read_holding_registers(41022+idx, count=1).registers[0] for idx in range(8)]
        logger.info(f"md: 41022-41029-安全位置触发状态 结果为{res}")
        return res

    @property
    def w_sta_soft_estop(self):
        return self.__get_state(41030, func="软急停状态", desc=":--: 0表示未触发，1表示已触发")

    @property
    def w_sta_safe_region(self):
        res = [self.c.read_holding_registers(41031+idx, count=1).registers[0] for idx in range(10)]
        logger.info(f"md: 41031-41040-安全区域触发状态 结果为{res} :--: 0表示未触发，1表示已触发")
        return res

    @property
    def w_sta_on_path(self):
        return self.__get_state(41030, func="当前是否在预设轨迹上", desc=":--: 0表示偏离预设轨迹，1表示在规划轨迹上")

    @property
    def w_sta_near_path(self):
        return self.__get_state(41030, func="当前是否在预设轨迹附近", desc=":--: 0表示偏离预设轨迹附近的标准，1表示在规划轨迹附近")

    @property
    def w_sta_cart_pose(self) -> list:
        res = self.c.read_holding_registers(41043, count=14)
        return self.c.convert_from_registers(registers=res.registers, data_type=self.c.DATATYPE.FLOAT32, word_order="little")

    @property
    def w_sta_cart_vel(self) -> list:
        res = self.c.read_holding_registers(41057, count=14)
        return self.c.convert_from_registers(registers=res.registers, data_type=self.c.DATATYPE.FLOAT32, word_order="little")

    @property
    def w_sta_jnt_pose(self) -> list:
        res = self.c.read_holding_registers(41071, count=14)
        return self.c.convert_from_registers(registers=res.registers, data_type=self.c.DATATYPE.FLOAT32, word_order="little")

    @property
    def w_sta_jnt_vel(self) -> list:
        res = self.c.read_holding_registers(41085, count=14)
        return self.c.convert_from_registers(registers=res.registers, data_type=self.c.DATATYPE.FLOAT32, word_order="little")

    @property
    def w_sta_tcp_pose(self) -> list:
        res = self.c.read_holding_registers(41113, count=14)
        return self.c.convert_from_registers(registers=res.registers, data_type=self.c.DATATYPE.FLOAT32, word_order="little")

    @property
    def w_sta_tcp_vel(self) -> list:
        res = self.c.read_holding_registers(41127, count=14)
        return self.c.convert_from_registers(registers=res.registers, data_type=self.c.DATATYPE.FLOAT32, word_order="little")

    @property
    def w_sta_tcp_vel_mag(self):
        res = self.c.read_holding_registers(41141, count=2)
        return self.c.convert_from_registers(registers=res.registers, data_type=self.c.DATATYPE.FLOAT32, word_order="little")

    def io_write_coils(self, addr, value: bool):
        # e.g. io_write_coils(0, 1)  给0号DI赋值为1
        self.c.write_coils(addr, [value, ])
        logger.info(f"md: 执行给DI地址{addr}赋值为{value}")

    def io_read_coils(self):
        res = self.c.read_coils(0, count=16).bits
        logger.info(f"md: 执行读取所有DI的结果为{res}")
        return res

    def io_read_discretes(self):
        res = self.c.read_discrete_inputs(0, count=16).bits
        logger.info(f"md: 执行读取所有DO的结果为{res}")
        return res


class EcRequest:
    def __init__(self):
        self.c: socket.socket
        self.exec_desc = " :--: true表示执行成功，false表示执行失败"
        self.sock_conn()

    def sock_conn(self):
        try:
            self.c = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.c.setblocking(False)
            self.c.settimeout(clibs.interval*3)
            self.c.connect((clibs.ip_addr, clibs.external_port))
            logger.success(f"ec: 外部通信连接成功")
        except Exception as e:
            logger.success(f"ec: 外部通信连接失败 -- {e}")
            raise Exception()

    def sr_string(self, directive, description, more_desc=""):
        self.s_string(directive)
        time.sleep(clibs.interval/2)
        result = self.r_string()
        logger.info(f"ec: 执行{description}指令是{directive}，返回值为{result}{more_desc}")
        return result

    def s_string(self, directive: str):
        cmd = "".join([directive, clibs.suffix])
        self.c.send(cmd.encode())

    def r_string(self):
        result = self.c.recv(10240).decode().strip()
        return result

    # =================================== ↓↓↓ specific functions ↓↓↓ ===================================
    def motor_on(self):  # auto | manual
        return self.sr_string("motor_on", "机器人上电", self.exec_desc)

    def motor_off(self):  # auto | manual
        return self.sr_string("motor_off", "机器人下电", self.exec_desc)

    def pp_to_main(self):  # auto
        return self.sr_string("pp_to_main", "程序pptomain", self.exec_desc)

    def program_start(self):  # auto
        return self.sr_string("start", "程序启动(必要时需清告警)", self.exec_desc)

    def program_stop(self):  # auto
        return self.sr_string("stop", "程序停止", self.exec_desc)

    def clear_alarm(self):  # auto | manual
        return self.sr_string("clear_alarm", "清除伺服报警", self.exec_desc)

    def switch_operation_auto(self):  # auto | manual
        return self.sr_string("switch_mode:auto", "切换到自动模式", self.exec_desc)

    def switch_operation_manual(self):  # auto | manual
        return self.sr_string("switch_mode:manual", "切换到手动模式", self.exec_desc)

    def open_drag_mode(self):  # manual
        return self.sr_string("open_drag", "打开拖动", self.exec_desc)

    def close_drag_mode(self):  # manual
        return self.sr_string("close_drag", "关闭拖动", self.exec_desc)

    def get_project_list(self):  # auto | manual
        return self.sr_string("list_prog", "获取工程列表")

    def get_current_project(self):  # auto | manual
        return self.sr_string("current_prog", "当前工程")

    def load_project(self, project_name):  # auto | manual
        return self.sr_string(f"load_prog:{project_name}", "加载工程", self.exec_desc)

    def estop_reset(self):  # auto | manual | 复原外部 IO 急停和安全门急停，前提是硬件已复原
        return self.sr_string("estop_reset", "急停复位", self.exec_desc)

    def estopreset_and_clearalarm(self):  # auto | manual | 外部 IO/安全门急停/安全碰撞告警都可以清除，前提是硬件已复原
        return self.sr_string("estopreset_and_clearalarm", "急停复位并清除报警", self.exec_desc)

    def motoron_pp2main_start(self):  # auto | manual
        return self.sr_string("motoron_pptomain_start", "依次执行上电，程序指针到main，启动程序(必要时需清告警)", self.exec_desc)

    def motoron_start(self):  # auto | manual
        return self.sr_string("motoron_start", "依次执行上电，启动程序(必要时需清告警)", self.exec_desc)

    def pause_motoroff(self):  # auto | manual
        return self.sr_string("pause_motoroff", "暂停程序并下电", self.exec_desc)

    def set_program_speed(self, speed: int = 20):  # auto | manual | 1-100
        return self.sr_string(f"set_program_speed:{speed}", "设置程序运行速率(滑块)", self.exec_desc)

    def set_soft_estop(self, enable: str):  # auto | manual
        return self.sr_string(f"set_soft_estop:{enable}", "触发(true)/解除(false)机器人软急停", self.exec_desc)

    def switch_auto_motoron(self):  # auto | manual
        return self.sr_string("switch_auto_motoron", "切换自动模式并上电", self.exec_desc)

    def open_safe_region(self, number: int):  # auto | manual | 1-10
        return self.sr_string(f"open_safe_region:{number}", f"打开第 {number} 个安全区域(信号控制开关需打开)", self.exec_desc)

    def close_safe_region(self, number: int):  # auto | manual | 1-10
        return self.sr_string(f"close_safe_region:{number}", f"关闭第 {number} 个安全区域(信号控制开关需打开)", self.exec_desc)

    def open_reduced_mode(self):  # auto | manual
        return self.sr_string("open_reduced_mode", "开启缩减模式", self.exec_desc)

    def close_reduced_mode(self):  # auto | manual
        return self.sr_string("close_reduced_mode", "关闭缩减模式", self.exec_desc)

    def setdo_value(self, do_name: str, do_value: str):  # - | do_value 为 true/false
        return self.sr_string(f"setdo:{do_name},{do_value}", f"设置 {do_name} 的值为 {do_value} ", self.exec_desc)

    def set_robot_time(self, robot_time: str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime())):  # auto | manual | YY-mm-dd HH:MM:SS
        return self.sr_string(f"set_robot_time:{robot_time}", f"修改控制器和示教器的时间为 {robot_time} 的", self.exec_desc)

    # --------------------------------------------------------------------------------------------------
    @property
    def motor_on_state(self):  # auto | manual
        return self.sr_string("motor_on_state", "获取上电状态", " :--: 返回true表示已上电，false表示已下电")

    @property
    def robot_running_state(self):  # auto | manual | 只针对运动任务和常规任务
        return self.sr_string("robot_running_state", "获取程序运行状态", " :--: 返回true表示正在运行，false表示未运行")

    @property
    def estop_state(self):  # auto | manual | 只表示外部急停，安全门触发不会返回true，只要有急停标识E字母，且非软急停就会返回true
        return self.sr_string("estop_state", "急停状态", " :--: 返回true表示处于急停状态，false表示非急停")

    @property
    def operation_mode(self):  # auto | manual
        return self.sr_string("operating_mode", "获取工作模式", " :--: 返回true表示自动模式，false手动模式")

    @property
    def home_state(self):  # # auto | manual | 需要设置一下 "HMI 设置->快速调整"
        return self.sr_string("home_state", "获取HOME输出状态", " :--: 返回true表示法兰中心处于HOME点，false 未处于HOME点")

    @property
    def fault_state(self):  # -
        return self.sr_string("fault_state", "获取 故障状态", " :--: 返回true表示处于故障状态，false表示非故障状态")

    @property
    def collision_state(self):  # -
        return self.sr_string("collision_state", "获取碰撞触发状态", " :--: 返回true表示碰撞已触发，false表示未触发")

    @property
    def task_state(self):  # auto | manual | 只适用于运动任务和常规任务
        return self.sr_string("task_state", "获取机器人运行任务状态", " :--: 返回program表示任务正在运行，ready表示未运行")

    @property
    def get_cart_pos(self):  # auto | manual | cart_pos/cart_pos_name都可以正常返回，区别在于前者直接返回value，后者返回的是"cart_pos" : value的形式
        return self.sr_string("cart_pos", "获取笛卡尔位置")

    @property
    def get_joint_pos(self):  # auto | manual | jnt_pos/jnt_pos_name都可以正常返回，区别在于前者直接返回value，后者返回的是"joint_pos" : value的形式
        return self.sr_string("jnt_pos", "获取轴位置")

    @property
    def get_joint_vel(self):  # auto | manual | jnt_vel/jnt_vel_name都可以正常返回，区别在于前者直接返回value，后者返回的是"jnt_vel" : value的形式
        return self.sr_string("jnt_vel", "获取轴速度")

    @property
    def get_joint_trq(self):  # auto | manual | jnt_trq/jnt_trq_name都可以正常返回，区别在于前者直接返回value，后者返回的是"jnt_trq" : value的形式
        return self.sr_string("jnt_trq", "获取轴力矩")

    @property
    def reduced_mode_state(self):  # auto | manual
        return self.sr_string("reduced_mode_state", "获取缩减模式状态", " :--: 返回true表示缩减模式，false表示非缩减模式")

    def get_io_state(self, io_list: str):  # - | DO0_0,DI1_3,DO2_5，不能有空格
        return self.sr_string(f"io_state:{io_list}", "获取 IO 状态值")

    @property
    def alarm_state(self):  # auto | manual
        return self.sr_string("alarm_state", "获取报警状态", " :--: 返回true表示当前有告警，false 没有告警")

    @property
    def collision_alarm_state(self):  # -
        return self.sr_string("collision_alarm_state", "获取碰撞报警状态", " :--: 返回true表示有碰撞告警，false表示没有碰撞告警")

    @property
    def collision_open_state(self):  # auto | manual
        return self.sr_string("collision_open_state", "获取碰撞检测开启状态", " :--: 返回true表示已开启碰撞检测，false表示未开启")

    @property
    def controller_is_running(self):  # auto | manual
        return self.sr_string("controller_is_running", "判断控制器是否开机", " :--: 返回true表示控制器正在运行，false表示未运行")

    @property
    def encoder_low_battery_state(self):  # auto | manual
        return self.sr_string("encoder_low_battery_state", "编码器低电压报警状态", " :--: 返回true表示编码器处于低电压状态，false表示电压正常")

    @property
    def robot_error_code(self):  # auto | manual
        return self.sr_string("robot_error_code", "获取机器人错误码")

    @property
    def rl_pause_state(self):  # -
        # 0 -- 初始化状态，刚开机上电时
        # 1 -- RL 运行中
        # 2 -- HMI 暂停
        # 3 -- 系统 IO 暂停
        # 4 -- 寄存器功能码暂停
        # 5 -- 外部通讯暂停
        # 6 --
        # 7 -- Pause 指令暂停
        # 8 --
        # 9 --
        # 10 -- 外部 IO 急停
        # 11 -- 安全门急停
        # 12 -- 其他因素停止，比如碰撞检测
        return self.sr_string("program_full", "获取RL的暂停状态", " :--: 返回值含义详见功能定义")

    @property
    def program_reset_state(self):  # auto | manual
        return self.sr_string("program_reset_state", "获取程序复位状态", " :--: 返回true表示指针指向main，false表示未指向main")

    @property
    def program_speed_value(self):  # auto | manual | 速度滑块
        return self.sr_string("program_speed", "获取程序运行速度")

    @property
    def robot_is_busy(self):  # auto | manual | 触发条件为 pp2main/重载工程/推送到控制器，最好测试工程大一些，比较容易触发
        return self.sr_string("robot_is_busy", "获取程序忙碌状态", " :--: 返回1表示控制器忙碌，0表示非忙碌状态")

    @property
    def robot_is_moving(self):  # auto | manual
        return self.sr_string("robot_is_moving", "获取程序运行状态", " :--: 返回true表示机器人正在运动，false未运动")

    @property
    def safe_door_state(self):  # -
        return self.sr_string("safe_door_state", "获取安全门状态", " :--: 返回true表示安全门已触发，false表示未触发")

    @property
    def soft_estop_state(self):  # auto | manual
        return self.sr_string("soft_estop_state", "获取软急停状态", " :--: 返回true表示软急停已触发，false表示未触发")

    @property
    def get_cart_vel(self):  # auto | manual
        return self.sr_string("cart_vel", "获取笛卡尔速度")

    @property
    def get_tcp_pos(self):  # auto | manual
        return self.sr_string("tcp_pose", "获取TCP位姿")

    @property
    def get_tcp_vel(self):  # auto | manual
        return self.sr_string("tcp_vel", "获取TCP速度")

    @property
    def get_tcp_vel_mag(self):  # auto | manual
        return self.sr_string("tcp_vel_mag", "获取TCP合成线速度")

    @property
    def ext_estop_state(self):  # -
        return self.sr_string("ext_estop_state", "获取外部轴急停状态", " :--: 返回true表示外部轴急停已触发，false表示未触发")

    @property
    def hand_estop_state(self):  # -
        return self.sr_string("hand_estop_state", "获取手持急停状态", " :--: 返回true表示手持急停已触发，false表示未触发")

    @property
    def sta_on_path(self):  # auto | manual
        return self.sr_string("sta_on_path", "获取当前坐标是否在路径上", " :--: 返回true表示在规划路径上，false表示不在规划路径上")

    @property
    def sta_near_path(self):  # -
        return self.sr_string("sta_near_path", "获取当前坐标是否在路径附近", " :--: 返回true表示在规划路径附近，false表示不在规划路径附近")


class PreDos(object):
    def __init__(self):
        self.__ssh = None
        self.__sftp = None

    def __ssh2server(self):
        try:
            self.__ssh = SSHClient()
            self.__ssh.set_missing_host_key_policy(AutoAddPolicy())
            self.__ssh.connect(hostname=clibs.ip_addr, port=clibs.ssh_port, username=clibs.username, password=clibs.password)
            self.__sftp = self.__ssh.open_sftp()
            # logger.success("pd: SSH连接成功")
        except Exception as e:
            logger.error(f"pd: SSH无法连接，需检查网络连通性或者登录信息是否正确 -- {e}")
            raise Exception()

    def push_prj_to_server(self, prj_file: str):
        # prj_file：工程的本地完整路径
        self.__ssh2server()
        prj_name, postfix = os.path.basename(prj_file).split(".")
        self.__sftp.put(prj_file, f"/tmp/{prj_name}.zip")
        cmd = f"cd /tmp; mkdir {prj_name}; unzip -q {prj_name}.zip; rm -rf /tmp/{prj_name}.zip; "
        cmd += f"sudo rm -rf /home/luoshi/bin/controller/projects/{prj_name}; "
        cmd += f"sudo mv /tmp/{prj_name}/ /home/luoshi/bin/controller/projects/"
        stdin, stdout, stderr = self.__ssh.exec_command(cmd, get_pty=True)
        stdin.write(clibs.password + "\n")
        _ = stdout.read().decode()  # 需要read一下才能正常执行
        _ = stderr.read().decode()
        self.__ssh.close()

    def pull_prj_from_server(self, prj_name: str, local_prj_path: str):
        # prj_name：要拉取的服务端工程名
        # local_prj_path：存放工程文件的本地路径
        self.__ssh2server()
        cmd = f"cd /tmp/; sudo rm -rf {prj_name}*; sudo cp -rf /home/luoshi/bin/controller/projects/{prj_name} .; "
        cmd += f"sudo zip -q -r {prj_name}.zip {prj_name}/; sudo rm -rf {prj_name}"
        stdin, stdout, stderr = self.__ssh.exec_command(cmd, get_pty=True)
        stdin.write(clibs.password + "\n")
        _ = stdout.read().decode()
        _ = stderr.read().decode()

        self.__sftp.get(f"/tmp/{prj_name}.zip", f"{local_prj_path}/{prj_name}.zip")
        cmd = f"sudo rm -rf /tmp/{prj_name}.zip"
        stdin, stdout, stderr = self.__ssh.exec_command(cmd, get_pty=True)
        stdin.write(clibs.password + "\n")
        _ = stdout.read().decode()
        _ = stderr.read().decode()

        self.__ssh.close()

    def push_file_to_server(self, local_file: str, server_file: str):
        # local_file：本地文件完整路径
        # server_file：服务端文件完整路径
        self.__ssh2server()
        filename = os.path.basename(local_file)
        self.__sftp.put(local_file, f"/tmp/{filename}")
        cmd = f"sudo mv /tmp/{filename} {server_file}"
        stdin, stdout, stderr = self.__ssh.exec_command(cmd, get_pty=True)
        stdin.write(clibs.password + "\n")
        _ = stdout.read().decode()  # 需要read一下才能正常执行
        _ = stderr.read().decode()
        self.__ssh.close()

    def pull_file_from_server(self, server_file: str, local_file: str):
        # local_file：本地文件完整路径
        # server_file：服务端文件完整路径
        self.__ssh2server()
        cmd = f"sudo cp {server_file} /tmp/"
        stdin, stdout, stderr = self.__ssh.exec_command(cmd, get_pty=True)
        stdin.write(clibs.password + "\n")
        _ = stdout.read().decode()  # 需要read一下才能正常执行
        _ = stderr.read().decode()

        filename = server_file.split("/")[-1]
        self.__sftp.get(f"/tmp/{filename}", f"{local_file}")
        cmd = f"sudo rm -rf /tmp/{filename}"
        stdin, stdout, stderr = self.__ssh.exec_command(cmd, get_pty=True)
        stdin.write(clibs.password + "\n")
        _ = stdout.read().decode()  # 需要read一下才能正常执行
        _ = stderr.read().decode()
        self.__ssh.close()


class RobotInit(object):
    def __init__(self, hr: HmiRequest, pd: PreDos):
        self.hr: HmiRequest = hr
        self.pd: PreDos = pd

    def robot_init(self):
        # 推送配置文件
        msg_id = self.hr.exec_cmd("controller.get_params")
        record = self.hr.get_from_id(msg_id)
        robot_params = eval(record)
        robot_type = robot_params["data"]["robot_type"]
        security_type = robot_params["data"]["security_type"]
        controller_type = robot_params["data"]["controller_type"]
        io_device_file = "_".join(["io_device", controller_type, security_type, "1"])

        user_settings = "/home/luoshi/bin/controller/user_settings"
        interactive_data = f"/home/luoshi/bin/controller/interactive_data/{robot_type}"
        config_files = [
            f"{clibs.base_path}/assets/confs/fieldbus_device.json",
            f"{clibs.base_path}/assets/confs/registers.json",
            f"{clibs.base_path}/assets/confs/registers.xml"
        ]
        for config_file in config_files:
            filename = os.path.basename(config_file)
            self.pd.push_file_to_server(config_file, f"{user_settings}/{filename}")
            self.pd.push_file_to_server(config_file, f"{interactive_data}/{filename}")

        io_device_autotest = {"ai_num": 0, "ao_num": 0, "di_num": 16, "do_num": 16, "extend_attr": {"mode": "slaver", "name": "autotest", "type": "MODBUS"}, "id": 7, "name": "autotest", "type": 6}
        io_device_file_local = f"{io_device_file}"
        io_device_file_local_tmp = f"{io_device_file}_tmp"
        io_device_file_server = f"{user_settings}/{io_device_file}"
        self.pd.pull_file_from_server(io_device_file_server, io_device_file_local)
        with open(io_device_file_local, mode="r", encoding="utf-8") as f:
            data = json.load(f)
            for _ in data["device_list"]:
                if _["extend_attr"].get("name", None) == "autotest":
                    break
            else:
                data["device_list"].append(io_device_autotest)
                with open(io_device_file_local_tmp, mode="w", encoding="utf-8") as f_tmp:
                    json.dump(data, f_tmp, separators=(",", ":"), indent=4)
                self.pd.push_file_to_server(io_device_file_local_tmp, f"{user_settings}/{io_device_file}")
                self.pd.push_file_to_server(io_device_file_local_tmp, f"{interactive_data}/{io_device_file}")
        try:
            os.remove(io_device_file_local)
            os.remove(io_device_file_local_tmp)
        except FileNotFoundError:
            pass
        self.hr.exec_cmd("fieldbus_device.load_cfg")
        self.hr.exec_cmd("fieldbus_device.set_params", device_name="autotest", enable=True)
        self.hr.exec_cmd("io_device.load_cfg")
        self.hr.exec_cmd("modbus.load_cfg")

    def fw_updater(self, fw_file: str):
        """
        :param fw_file: xCore升级文件/机型文件的绝对路径
        :return: None
        """

        def get_xcore_version():
            msg_id = self.hr.exec_cmd("controller.get_params")
            record = self.hr.get_from_id(msg_id)
            self.hr.close()
            return eval(record)["data"]["version"]

        conn = None
        try:
            conn = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            conn.setblocking(False)
            conn.settimeout(clibs.interval * 3)
            conn.connect((clibs.ip_addr, clibs.upgrade_port))
            logger.success(f"ri: 升级服务连接成功")
        except Exception as e:
            logger.error(f"ri: 升级服务连接失败\n{e}")

        previous_version = get_xcore_version()
        fw_size = os.path.getsize(fw_file)  # Byte
        remote_file_path = "./upgrade/lircos.zip"
        fw_content = package_data = package_data_with_head = b""

        # construct package data: http://confluence.i.rokae.com/pages/viewpage.action?pageId=15634148
        # 1 protocol id
        protocol_id = int(0).to_bytes(length=2)
        # 2 write type
        write_type = int(0).to_bytes(length=1)
        # 6 file
        with open(fw_file, "rb") as f_fw:
            fw_content += f_fw.read()
        # 3 md5
        md5_hash = hashlib.md5()
        md5_hash.update(fw_content)
        fw_md5 = bytes.fromhex(md5_hash.hexdigest())
        # 4 remote file path len
        remote_file_path_len = len(remote_file_path).to_bytes(length=2)
        # 5 remote path
        remote_path = remote_file_path.encode()
        # 7 组包，注意顺序
        package_data += (protocol_id + write_type + fw_md5 + remote_file_path_len + remote_path + fw_content)
        # construct communication protocol: http://confluence.i.rokae.com/pages/viewpage.action?pageId=15634045
        # 1 get package data with header
        package_size = len(package_data).to_bytes(length=4)
        package_type = int(1).to_bytes()  # 0-无协议 1-文件 2-json
        package_reserve = int(0).to_bytes()  # 预留位
        package_data_with_head += (package_size + package_type + package_reserve + package_data)
        # 2 send data to server
        logger.info("ri: 正在发送数据到xCore......")
        start = 0
        len_of_package = len(package_data_with_head)
        while start < len_of_package:
            end = 10240 + start
            if end > len_of_package:
                end = len_of_package
            sent = conn.send((package_data_with_head[start:end]))
            # time.sleep(0.01)
            if sent == 0:
                raise RuntimeError("socket connection broken")
            else:
                start += sent

        if fw_size > 1000000:  # 1MB
            self.hr = HmiRequest()
            current_version = get_xcore_version()
            logger.success(f"update firmware: 控制器升级成功：from {previous_version} to {current_version} :)")
        else:
            logger.success(f"update firmware: 配置文件升级成功 :)")

        conn.close()


class UpgradeRequest:
    """
    http://confluence.i.rokae.com/pages/viewpage.action?pageId=21531754
    """
    def __init__(self):
        self.c: socket.socket
        self.sock_conn()

    def sock_conn(self):
        try:
            self.c = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.c.setblocking(False)
            self.c.settimeout(clibs.interval*3)
            self.c.connect((clibs.ip_addr, clibs.upgrade_port))
            logger.success(f"ur: 升级协议通信连接成功")
        except Exception as e:
            logger.error(f"ur: 升级协议通信连接失败 -- {e}")
            raise Exception()

    def s_pkg(self, command: dict):
        cmd = json.dumps(command, separators=(",", ":")).encode("utf-8")
        package_size = len(cmd).to_bytes(length=4)
        package_type = int(2).to_bytes()
        reserved_byte = int(0).to_bytes()
        pkg = package_size + package_type + reserved_byte + cmd
        self.c.send(pkg)

    def r_pkg(self):
        try:
            res = self.c.recv(10240)[8:].decode()
        except Exception as e:
            res = e
        return res

    def sr_pkg(self, command: dict):
        self.s_pkg(command)
        time.sleep(clibs.interval/2)
        res = self.r_pkg()
        logger.info(f"upgrade: 请求命令 {str(command)} 的返回信息：\n{res}")

    def erase_cfg(self):  # OK 抹除配置，重启后生效
        self.sr_pkg({"cmd": "erase"})

    def clear_rubbish(self):  # OK 清理垃圾
        self.sr_pkg({"cmd": "clearRubbish"})

    def soft_reboot(self):  # OK 重启xCore
        self.sr_pkg({"cmd": "soft_reboot"})

    def version_query(self):  # OK 查询升级程序版本信息
        self.sr_pkg({"query": "version"})

    def robot_reboot(self):  # OK 重启系统
        self.sr_pkg({"cmd": "reboot"})

    def backup_origin(self):  # OK 生成备份
        self.sr_pkg({"cmd": "backup_origin"})

    def origin_recovery(self):  # OK 恢复备份，恢复后手动设置，并重启生效
        self.sr_pkg({"cmd": "recover"})


class PlotTrajectory:
    # hw_cart_pos_feedback_motorside：笛卡尔空间下电机侧位置反馈，单位m,rad
    # hw_cart_pos_feedback_linkside：笛卡尔空间下连杆侧位置反馈，单位m,rad
    # hw_cart_pos_feedback_encoder_tcp_in_world：笛卡尔位置反馈（关节端编码器数据），单位m,rad
    # hw_cart_pos_feedback_flan_in_world：笛卡尔位置反馈（法兰相对于世界坐标系）
    # hw_cart_pos_feedback_tcp_in_world：笛卡尔位置反馈（tcp相对于世界坐标系）
    def __init__(self, hr: HmiRequest, md: ModbusRequest, prj_name: str, tasks: list, curve_name: str = "hw_cart_pos_feedback_tcp_in_world"):
        self.hr: HmiRequest = hr
        self.md: ModbusRequest = md
        self.curve_name: str = curve_name
        self.prj_name: str = prj_name
        self.tasks: list = tasks

    def __change_curve_state(self, stat):
        if not stat:
            display_pdo_params = [{"name": "count", "channel": 0}]
        else:
            display_pdo_params = [{"name": self.curve_name, "channel": chl} for chl in range(3)]
        self.hr.exec_cmd("diagnosis.set_params", display_pdo_params=display_pdo_params)
        self.hr.exec_cmd("diagnosis.open", open=stat, display_open=stat)

    def __plot_trajectory(self, start_time, end_time):
        x, y, z = [], [], []
        try:
            clibs.lock.acquire(True)
            clibs.cursor.execute(f"SELECT content FROM logs WHERE timestamp BETWEEN '{start_time}' AND '{end_time}' AND content LIKE '%diagnosis.result%' ORDER BY id ASC")
            records = clibs.cursor.fetchall()
        finally:
            clibs.lock.release()

        for record in records:
            data = eval(record[0])["data"]
            for item in data:
                item_rm = [x * 1000 for x in reversed(item["value"])]
                # item_rm = item["value"]
                if item.get("channel", None) == 0 and item.get("name", None) == self.curve_name:
                    x.extend(item_rm)
                elif item.get("channel", None) == 1 and item.get("name", None) == self.curve_name:
                    y.extend(item_rm)
                elif item.get("channel", None) == 2 and item.get("name", None) == self.curve_name:
                    z.extend(item_rm)

        # pyplot
        trace_path = go.Scatter3d(x=x, y=y, z=z, mode="lines", line=dict(color="blue", width=4), name="运动轨迹")
        trace_start = go.Scatter3d(x=[x[0]], y=[y[0]], z=[z[0]], mode="markers+text", marker=dict(color="green", size=8), text=["起点"], textposition="top center", name="起点")
        trace_end = go.Scatter3d(x=[x[-1]], y=[y[-1]], z=[z[-1]], mode="markers+text", marker=dict(color="red", size=8), text=["终点"], textposition="bottom center", name="终点")
        fig = go.Figure(data=[trace_path, trace_start, trace_end])
        fig.write_html("trajectory.html", auto_open=False)

        fig.update_layout(
            scene=dict(
                xaxis_title="X",
                yaxis_title="Y",
                zaxis_title="Z"
            ),
            margin=dict(l=0, r=0, b=0, t=30),
            legend=dict(x=0, y=1),
            dragmode="orbit"
        )

        fig.show(config={"scrollZoom": True})

    def draw_traj(self):
        self.md.r_soft_estop(True)
        self.md.r_reset_estop_clear_alarm()
        self.md.r_switch_auto()
        self.md.r_motor_on()

        self.__change_curve_state(True)
        self.hr.exec_cmd("overview.reload", prj_path=f"{self.prj_name}/_build/{self.prj_name}.prj", tasks=self.tasks)
        self.hr.exec_cmd("rl_task.set_run_params", loop_mode=False, override=1.0)
        self.hr.exec_cmd("rl_task.pp_to_main", tasks=self.tasks)
        self.hr.exec_cmd("rl_task.run", tasks=self.tasks)

        t_start = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))
        time.sleep(clibs.interval)
        while self.md.w_sta_robot_is_moving:
            time.sleep(clibs.interval * 10)
        time.sleep(clibs.interval * 5)
        t_end = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))
        self.__change_curve_state(False)

        self.__plot_trajectory(t_start, t_end)
