# 导入必要库：网络、ESP-NOW通信、硬件控制及LCD驱动
import network
import espnow
import time
from time import sleep_ms
from machine import SPI, Pin
from driver import st7735_buf  # ST7735 LCD底层驱动
from driver import drivers         # 电机、光线传感器、温度传感器驱动
from lib.easydisplay import EasyDisplay  # 简化LCD显示操作
import ubinascii

# ----------------------------------
# 1. 定义“计算居中坐标”的函数
# ----------------------------------
def get_center_pos(text, font_width=8, font_height=8):
    """
    计算文字居中时的起始坐标 (x, y)
    text: 要显示的文字（如 "forward"）
    font_width: 字体宽度（默认 8 像素）
    font_height: 字体高度（默认 8 像素）
    """
    # 计算文字总宽度（字符数 × 字体宽度）
    text_total_width = len(text) * font_width
    # X 轴：屏幕水平中心 - 文字总宽度的一半
    x = (160 - text_total_width) // 2
    # Y 轴：屏幕垂直中心 - 字体高度的一半
    y = (128 - font_height) // 2 + 8
    return x, y

# 初始化
spi = SPI(2, baudrate=20000000, polarity=0, phase=0, sck=Pin(18), mosi=Pin(23))
dp = st7735_buf.ST7735(width=160, height=128, spi=spi, cs=5, dc=4, res=19, rotate=1, bl=None,invert=False, rgb=True)
ed = EasyDisplay(display=dp, font="/font/text_lite_16px_2312.v3.bmf", show=True, color=0xFFFF, clear=True,color_type="RGB565")
hd = drivers.HardwareDrivers()       # 创建硬件驱动实例

# 初始化ESP-NOW通信
# 初始化sta
sta = network.WLAN(network.STA_IF)
sta.active(True)
# 获取MAC地址（以字节形式）
mac_bytes = sta.config('mac')

# 将字节转换为人类可读的十六进制字符串
mac_str = ubinascii.hexlify(mac_bytes, ':').decode()

print(f"MAC地址: {mac_str}")
sta.disconnect()

ed.text(mac_str, 10, 10)

e = espnow.ESPNow()
e.active(True)  # 启用ESP-NOW

def receive_messages():
    """接收ESP-NOW消息，处理后在LCD和串口显示"""
    while True:
        try:
            host, msg = e.recv(0)  # 非阻塞接收消息
            
            if msg:
                # 解码消息
                command = msg.decode('utf-8').strip() if isinstance(msg, bytes) else str(msg).strip()
                # 格式化发送端MAC
                sender_mac = ':'.join(['%02x' % b for b in host])
                print(f"来自 {sender_mac} 的指令: {command}")
                               
                # LCD显示指令
                # ed.fill(0x0000)  # 清屏（黑色）
                dir_x, dir_y = get_center_pos(command)
                ed.text(command, dir_x, dir_y)

                # 首先判断是方向指令还是模拟量指令
                if command.startswith("ANALOG:"):
                    # 模拟量指令 - 解析X和Y值
                    try:
                        parts = command.split(":")
                        x_analog = int(parts[1])
                        y_analog = int(parts[2])
                        
                        # 死区处理 - 如果摇杆接近中心位置，则停止
                        if abs(x_analog) < 10 and abs(y_analog) < 10:
                            hd.motor_stop("ALL")
                            print("车辆停止")
                            
                        else:
                            # 基础速度计算（基于Y轴）
                            base_speed = abs(y_analog) * 2  # 转换为0-255范围
                            
                            # 转向系数（基于X轴）
                            turn_factor = x_analog / 127.0  # -1.0 到 +1.0
                            
                            if y_analog > 10:  # 前进
                                # 差速转向：一个轮子快，一个轮子慢
                                left_speed = int(base_speed * (1 - turn_factor))
                                right_speed = int(base_speed * (1 + turn_factor))
                                
                                # 限制速度在0-255范围内
                                left_speed = max(0, min(255, left_speed))
                                right_speed = max(0, min(255, right_speed))
                                
                                hd.motor_run(1, "CW", right_speed)  # 右轮
                                hd.motor_run(2, "CW", left_speed)   # 左轮
                                print(f"前进 - 左轮: {left_speed}, 右轮: {right_speed}")
                                
                            elif y_analog < -10:  # 后退
                                # 差速转向：一个轮子快，一个轮子慢
                                left_speed = int(base_speed * (1 + turn_factor))
                                right_speed = int(base_speed * (1 - turn_factor))
                                
                                # 限制速度在0-255范围内
                                left_speed = max(0, min(255, left_speed))
                                right_speed = max(0, min(255, right_speed))
                                
                                hd.motor_run(1, "CCW", right_speed)  # 右轮
                                hd.motor_run(2, "CCW", left_speed)   # 左轮
                                print(f"后退 - 左轮: {left_speed}, 右轮: {right_speed}")
                                
                            else:  # 原地转向（只有X轴输入）
                                turn_speed = abs(x_analog) * 2
                                if x_analog > 10:  # 原地右转
                                    hd.motor_run(1, "CCW", min(turn_speed, 255))
                                    hd.motor_run(2, "CW", min(turn_speed, 255))
                                    print(f"原地右转 - 速度: {turn_speed}")
                                elif x_analog < -10:  # 原地左转
                                    hd.motor_run(1, "CW", min(turn_speed, 255))
                                    hd.motor_run(2, "CCW", min(turn_speed, 255))
                                    print(f"原地左转 - 速度: {turn_speed}")
                                    
                    except (ValueError, IndexError):
                        print(f"模拟量指令解析错误: {command}")
                        
                else:
                    # 方向指令 - 原有的逻辑保持不变
                    if command == "Forward":
                        hd.motor_run(1, "CW", 255)
                        hd.motor_run(2, "CW", 255)
                        print("车辆前进")
                    elif command == "Backward":
                        hd.motor_run(1, "CCW", 255)
                        hd.motor_run(2, "CCW", 255)
                        print("车辆后退")
                    elif command == "Left":
                        hd.motor_run(1, "CW", 255)
                        hd.motor_run(2, "CCW", 255)
                        print("车辆左转")
                    elif command == "Right":
                        hd.motor_run(1, "CCW", 255)
                        hd.motor_run(2, "CW", 255)
                        print("车辆右转")
                    elif command == "Stop":
                        hd.motor_stop("ALL")
                        print("车辆停止")

            sleep_ms(10)
        except Exception as ex:
            print(f"接收错误: {ex}")
            sleep_ms(100)

def main():
    """主函数：初始化系统并启动消息接收"""
    try:
        # ed.fill(0x0000)
        receive_messages()  # 启动接收循环
    except KeyboardInterrupt:
        print("程序被中断")
    except Exception as ex:
        print(f"错误: {ex}")
    finally:
        # 清理资源
        e.active(False)
        sta.active(False)
        ed.fill(0x0000)
        ed.text("已停止", 50, 60)

if __name__ == "__main__":
    main()
