# 调试小车，windows版本代码
# 可以按格式发送、接受数据

import serial
import time
import struct
import keyboard
import sys
import msvcrt  # Windows 环境下用于捕捉键盘输入

class SerialPort:
    def __init__(self, port, baudrate=9600, timeout=1):
        self.port = port
        self.baudrate = baudrate
        self.timeout = timeout
        self.serial = None

    def open(self):
        try:
            self.serial = serial.Serial(self.port, self.baudrate, timeout=self.timeout)
            print(f"成功打开串口: {self.port}")
        except serial.SerialException as e:
            print(f"打开串口失败: {e}")
            self.serial = None

    def close(self):
        if self.serial and self.serial.is_open:
            self.serial.close()
            print("串口已关闭")

    def write(self, data):
        if self.serial and self.serial.is_open:
            if isinstance(data, str):
                data = data.encode()
            self.serial.write(data)

    def write_hex(self, data):
        if self.serial and self.serial.is_open:
            self.serial.write(data)

    def read(self, size=22):
        if self.serial and self.serial.is_open:
            data = self.serial.read(size)
            hex_data = ', '.join(f'0x{byte:02X}' for byte in data)
            print(f"接收数据: {hex_data}")
            return data
        return None


def parse_serial_feedback(data):
    struct_format = '<HhhhhhhhhHH'
    struct_size = struct.calcsize(struct_format)

    if len(data) < struct_size:
        raise ValueError("接收到的数据长度不足，无法解析。")

    parsed_data = struct.unpack(struct_format, data[:struct_size])
    feedback = {
        'start': parsed_data[0],
        'cmd1': parsed_data[1],
        'cmd2': parsed_data[2],
        'speedR_meas': parsed_data[3],
        'speedL_meas': parsed_data[4],
        'wheelR_cnt': parsed_data[5],
        'wheelL_cnt': parsed_data[6],
        'batVoltage': parsed_data[7] / 100.0,
        'boardTemp': parsed_data[8] / 10.0,
        'cmdLed': parsed_data[9],
        'checksum': parsed_data[10]
    }
    print(feedback)
    return feedback


def calculate_checksum(data):
    checksum = sum(data) & 0xFFFF
    return checksum


def send_speed_command(serial_port, steer, speed):
    start_frame = 0xABCD
    command_format = '<HhhH'
    command_data = struct.pack(command_format, start_frame, steer, speed, 0)
    checksum = (start_frame ^ steer ^ speed) & 0xFFFF
    command_data = struct.pack(command_format, start_frame, steer, speed, checksum)

    hex_data = ', '.join(f'0x{byte:02X}' for byte in command_data)
    print(f"发送: {hex_data}, {hex(checksum)}")
    print(f"发送字节流: {command_data}")
    serial_port.write_hex(command_data)


def set_speed(serial_port, ls, rs):
    set_speed_l = ls / 0.10472
    set_speed_r = rs / 0.10472

    print(ls, rs)

    speed = (int)((set_speed_l + set_speed_r) / 2.0)
    steer = (int)((set_speed_l - speed) * 2.0)

    send_speed_command(serial_port, steer, speed)


def get_key():
    """捕捉 Windows 上的键盘输入"""
    if msvcrt.kbhit():
        return msvcrt.getch().decode('utf-8')
    return None


if __name__ == "__main__":
    # 修改为 Windows 上的串口名称，例如 'COM3'
    port_name = 'COM9'
    baudrate = 115200

    serial_port = SerialPort(port=port_name, baudrate=baudrate)
    serial_port.open()

    ls = 3
    rs = 3

    try:
        while True:
            time.sleep(0.1)

            key = get_key()  # 获取按键输入

            if key == 'w':  # Up arrow
                ls += 3
                rs += 3
            elif key == 's':  # Down arrow
                ls = 1
                rs = 1
            elif key == 'a':  # Left arrow
                ls = 0.5
                rs += 3
            elif key == 'd':  # Right arrow
                ls += 3
                rs = 0.5
            elif key == 'q':  # 按 'q' 键退出
                break

            if ls < 0:
                ls = 0

            if rs < 0:
                rs = 0

            set_speed(serial_port, ls, rs)
            received_data = serial_port.read(44)
            parse_serial_feedback(received_data)

            # 重置速度
            if key is None:  # 如果没有按下键，重置速度
                ls = 0
                rs = 0

    except KeyboardInterrupt:
        print("停止机器人")
    finally:
        serial_port.close()