import struct
import serial
import serial.tools.list_ports
import numpy as np
import atexit
import threading
import time
import yaml

import subprocess
import psutil
import sys
import os
sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "Massage/MassageControl")))


class XjcSensor:
    def __init__(self, port=None, baudrate=115200, rate=250):
        self.port = '/dev/ttyUSB0'
        self.baudrate = baudrate
        self.rate = rate
        
        self.crc16_table = self.generate_crc16_table()

        self.slave_adress = 0x01
        self.ser = None
        
        # 后台读取相关的属性
        self._background_thread = None
        self._stop_background = False
        self._last_reading = None
        self._reading_lock = threading.Lock()

        if not self.port:
            self.port = self.find_sensor_port()
        
        try:
            self.connect()
            atexit.register(self.disconnect)

        except KeyboardInterrupt:
            print("Process interrupted by user")
            self.disconnect()
        
    def __new__(cls, *args, **kwargs):
        """
        Making this class singleton
        """
        if not hasattr(cls, 'instance'):
            cls.instance = super(XjcSensor, cls).__new__(cls)
        return cls.instance
    
    def connect(self):
        self.ser = serial.Serial(self.port, self.baudrate,timeout=0.1)
    
    def find_sensor_port(self):
        """寻找对应六维力传感器的串口"""
        try:
            # 目标 YAML 文件路径
            yaml_file_path = '/home/jsfb/jsfb_ws/global_config/force_sensor.yaml'

            # 读取现有的 YAML 文件内容
            with open(yaml_file_path, 'r') as file:
                data = yaml.safe_load(file)  # 读取并解析 YAML 内容

            # 获取 target_usb_device_path 的值
            target_usb_device_path = data.get('target_usb_device_path')

            print(f"已成功将 target_usb_device_path 添加到 {yaml_file_path}")

            ports = serial.tools.list_ports.comports()
            for port in ports:
                print(port.usb_device_path)
                if target_usb_device_path == port.usb_device_path:
                    print(f"找到六维力传感器在端口: {port.usb_device_path}")
                    return port.device
            print("未找到具有指定描述的六维力传感器。")
        except Exception as e:
            print(f"查找串口时发生错误: {e}")
        return None

    @staticmethod
    def generate_crc16_table():
        table = []
        polynomial = 0xA001
        for i in range(256):
            crc = i
            for _ in range(8):
                if crc & 0x0001:
                    crc = (crc >> 1) ^ polynomial
                else:
                    crc >>= 1
            table.append(crc)
        return table
    
    def crc16(self,data: bytes):
        crc = 0xFFFF
        for byte in data:
            crc = (crc >> 8) ^ self.crc16_table[(crc ^ byte) & 0xFF]
        return (crc >> 8) & 0xFF, crc & 0xFF


    def set_zero(self):
        """
        传感器置零，重新上电之后需要置零
        """
        # 确保后台读取已停止
        was_reading = False
        if self._background_thread is not None and self._background_thread.is_alive():
            was_reading = True
            self.stop_background_reading()

        try:
            # 重置输入输出缓冲区
            self.ser.reset_input_buffer()
            self.ser.reset_output_buffer()

            # 清除串口缓冲区中的数据
            if self.ser.in_waiting > 0:
                self.ser.read(self.ser.in_waiting)

            # 根据地址设置置零命令
            if self.slave_adress == 0x01:
                zero_cmd = bytes.fromhex('01 10 46 1C 00 02 04 00 00 00 00 E8 95')  # 传感器内置通信协议--清零传感器数据  
            elif self.slave_adress == 0x09:
                zero_cmd = bytes.fromhex('09 10 46 1C 00 02 04 00 00 00 00 C2 F5')

            # 发送置零命令
            self.ser.write(zero_cmd)
            response = bytearray(self.ser.read(8))
            print(f"set zero response: {response}")

            # CRC 校验
            Hi_check, Lo_check = self.crc16(response[:-2])
            if response[0] != self.slave_adress or response[1] != 0x10 or \
            response[-1] != Hi_check or response[-2] != Lo_check:
                print("set zero failed!")
                return -1

            print("set zero success!")
            result = 0

        except serial.SerialException as e:
            print(f"Serial communication error: {e}")
            print(f"Details - Address: {self.slave_adress}, Port: {self.ser.port}")
            result = -1
        except Exception as e:
            print(f"Unexpected error: {e}")
            result = -1

        # 如果之前在进行后台读取，恢复它
        if was_reading:
            self.start_background_reading()

        return result

    # 可能比较慢
    def read_data_f32(self):
        self.ser.reset_input_buffer()   # 清除输入缓冲区
        self.ser.reset_output_buffer()  # 清除输出缓冲区
        if self.ser.in_waiting > 0:
            self.ser.read(self.ser.in_waiting)
        if self.slave_adress == 0x01:
            # command = bytes.fromhex('01 04 00 54 00 0C B1 DF')  # (旧版)传感器内置通信协议--读取一次六维力数据
            command = bytes.fromhex('01 04 00 00 00 0C F0 0F')  # (新版)传感器内置通信协议--读取一次六维力数据
        elif self.slave_adress == 0x09:
            command = bytes.fromhex('09 04 00 54 00 0C B0 97')  # 传感器内置通信协议--读取一次六维力数据      
        try:
            self.ser.write(command)
            response = bytearray(self.ser.read(29))
            # print(response)
            Hi_check, Lo_check = self.crc16(response[:-2])
            if response[0] != self.slave_adress or response[1] != 0x04 or \
                response[-1] != Hi_check or response[-2] != Lo_check or response[2] != 24:
                print("read data failed!")
                return -1
            sensor_data = struct.unpack('>ffffff', response[3:27])
        except serial.SerialException as e:
            print(f"Serial communication error: {e}")
            return -1
        except Exception as e:
            print(f"Unexpected error: {e}")
            return -1
        return np.array(sensor_data)
    
    def enable_active_transmission(self):
        """
        Activate the sensor's active transmission mode
        """
        # 确保后台读取已停止
        was_reading = False
        if self._background_thread is not None and self._background_thread.is_alive():
            was_reading = True
            self.stop_background_reading()

        try:
            # 根据不同的速率设置命令
            if self.rate == 100:
                if self.slave_adress == 0x01:
                    rate_cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 00 AB 6A')  # 传感器内置通信协议--100Hz
                elif self.slave_adress == 0x09:
                    rate_cmd = bytes.fromhex('09 10 01 9A 00 01 02 00 00 CC AA')
                print("Set mode in 100Hz")
            elif self.rate == 250:
                if self.slave_adress == 0x01:
                    rate_cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 01 6A AA')
                elif self.slave_adress == 0x09:
                    rate_cmd = bytes.fromhex('09 10 01 9A 00 01 02 00 01 0D 6A')
                print("Set mode in 250Hz")
            elif self.rate == 500:  
                if self.slave_adress == 0x01:
                    rate_cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 02 2A AB')  # 传感器内置通信协议--500Hz
                elif self.slave_adress == 0x09:
                    rate_cmd = bytes.fromhex('09 10 01 9A 00 01 02 00 02 4D 6B')
                print("Set mode in 500Hz")
            else:
                print("Rate not supported")
                result = -1
                return result

            # 检查串口是否打开
            if not self.ser.is_open:
                raise serial.SerialException("Serial port is not open")

            # 重置输入缓冲区
            self.ser.reset_input_buffer()
            
            # 写入指令
            self.ser.write(rate_cmd)
            print("Transmission command sent successfully")
            result = 0

        except serial.SerialException as e:
            print(f"Serial communication error: {e}")
            print(f"Details - Rate: {self.rate}, Address: {self.slave_adress}, Port: {self.ser.port}")
            result = -1
        except Exception as e:
            print(f"Unexpected error: {e}")
            result = -1

        # 如果之前在进行后台读取，恢复它
        if was_reading:
            self.start_background_reading()

        return result

    def disable_active_transmission(self):
        """
        Disable the sensor's active transmission mode
        """
        try:
            # 禁用传感器活动传输模式的命令
            rate_cmd = bytes.fromhex('FF FF FF FF FF FF FF FF FF FF FF')
            print("Disable the sensor's active transmission mode")

            # 重置输入缓冲区
            if not self.ser.is_open:
                raise serial.SerialException("Serial port is not open")
            self.ser.reset_input_buffer()

            # 发送禁用传输模式的命令
            self.ser.write(rate_cmd)
            print("Transmission disabled successfully")
            return 0

        except serial.SerialException as e:
            print(f"Serial communication error: {e}")
            print(f"Details - Port: {self.ser.port}")
            return -1
        except Exception as e:
            print(f"Unexpected error: {e}")
            return -1


    def read(self):
        """
        Read the sensor's data.
        """
        try:
            if self.ser.in_waiting > 0:
                self.ser.read(self.ser.in_waiting)
            while True:
                if int.from_bytes(self.ser.read(1), byteorder='big') == 0x20:
                    if int.from_bytes(self.ser.read(1), byteorder='big') == 0x4E:
                        break
            response = bytearray([0x20, 0x4E])  # 使用bytearray创建16进制数据的列表
            response.extend(self.ser.read(14))  # 读取12个字节的数据并添加到列表中
            Hi_check, Lo_check = self.crc16(response[:-2])
            if response[-1] != Hi_check or response[-2] != Lo_check:
                print("check failed!")
                return None
            sensor_data = self.parse_data_passive(response)
            return sensor_data
        except serial.SerialException as e:
            print(f"Serial communication error: {e}")
            return None    
    

    def parse_data_passive(self, buffer):
        values = [
                int.from_bytes(buffer[i:i+2], byteorder='little', signed=True)
                for i in range(2, 14, 2)
                ]
        Fx, Fy, Fz = np.array(values[:3]) / 10.0
        Mx, My, Mz = np.array(values[3:]) / 1000.0
        return np.array([Fx, Fy, Fz, Mx, My, Mz])
    
    def disconnect(self):
        """
        完全断开传感器连接并清理相关资源
        """
        # 首先停止后台读取任务
        if hasattr(self, '_background_thread') and self._background_thread is not None:
            self.stop_background_reading()
            
        try:
            # 1. 尝试禁用传输
            for _ in range(3):
                try:
                    send_str = "FF " * 50
                    send_str = send_str[:-1]
                    rate_cmd = bytes.fromhex(send_str)
                    if hasattr(self, 'ser') and self.ser.is_open:
                        self.ser.write(rate_cmd)
                        time.sleep(0.1)
                except Exception as e:
                    print(f"禁用传输失败: {e}")

            # 2. 关闭并清理串口
            if hasattr(self, 'ser'):
                try:
                    if self.ser.is_open:
                        self.ser.flush()  # 清空缓冲区
                        self.ser.reset_input_buffer()  # 清空输入缓冲区
                        self.ser.reset_output_buffer()  # 清空输出缓冲区
                        self.ser.close()
                    del self.ser  # 删除串口对象
                except Exception as e:
                    print(f"关闭串口失败: {e}")

        except Exception as e:
            print(f"断开连接过程中发生错误: {e}")
        
        finally:
            # 确保清理所有属性
            if hasattr(self, 'ser'):
                del self.ser

    def __del__(self):
        self.disconnect()
    
    def start_background_reading(self):
        """
        启动后台读取任务，每秒读取一次传感器数据
        """
        if self._background_thread is not None and self._background_thread.is_alive():
            print("Background reading is already running")
            return False
        
        self._stop_background = False
        self._background_thread = threading.Thread(target=self._background_reading_task)
        self._background_thread.daemon = True  # 设置为守护线程
        self._background_thread.start()
        return True

    def stop_background_reading(self):
        """
        停止后台读取任务
        """
        if self._background_thread is None or not self._background_thread.is_alive():
            print("No background reading is running")
            return False
        
        self._stop_background = True
        self._background_thread.join(timeout=2.0)  # 等待线程结束，最多等待2秒
        self._background_thread = None
        return True

    def _background_reading_task(self):
        """
        后台读取任务的实现
        """
        while not self._stop_background:
            try:
                data = self.read_data_f32()
                if isinstance(data, np.ndarray):
                    with self._reading_lock:
                        self._last_reading = data
                else:
                    self.connect()
            except Exception as e:
                print(f"Error in background reading: {e}")
            time.sleep(1.0)  # 每秒读取一次

    def get_last_reading(self):
        """
        获取最近一次的读数
        """
        with self._reading_lock:
            return self._last_reading

if __name__ == "__main__":
    import sys
    import pathlib
    sys.path.append(str(pathlib.Path(__file__).parent.parent))
    from tools.Rate import Rate
    import signal
    import sys
    rate = Rate(250)
    xjc_sensor = XjcSensor()
    def signal_handler(sig, frame):
        print('Received Ctrl+C, exiting gracefully...')
        sys.exit(0)
    signal.signal(signal.SIGINT, lambda signum, frame: sys.exit(0))
    # atexit.register(xjc_sensor.disconnect)
    time.sleep(1)
    xjc_sensor.disable_active_transmission()
    xjc_sensor.disable_active_transmission()
    xjc_sensor.disable_active_transmission()
    print(xjc_sensor.read_data_f32())
    print(xjc_sensor.read_data_f32())
    print(xjc_sensor.read_data_f32())
    # print(xjc_sensor.read())
    time.sleep(1)
    xjc_sensor.set_zero()
    xjc_sensor.set_zero()
    xjc_sensor.set_zero()
    # time.sleep(1)
    # xjc_sensor.enable_active_transmission()
    while True:
        # sensor_data = xjc_sensor.read()
        sensor_data = xjc_sensor.read_data_f32()
        current_time = time.strftime("%Y-%m-%d_%H-%M-%S")
        if sensor_data is None:
           print('failed to get force sensor data!')
        print(f"{current_time}-----{sensor_data}")
        rate.sleep(False)
        # break