#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
M01激光测距模块测试脚本 - 修正版
基于文档中的完整命令示例
"""

from maix import uart, pinmap, time
import binascii

# ==================== 配置 ====================
UART_PORT = "/dev/ttyS1"
BAUDRATE = 9600

# 初始化串口
try:
    pinmap.set_pin_function("A18", "UART1_RX")
    pinmap.set_pin_function("A19", "UART1_TX")
    laser_uart = uart.UART(UART_PORT, BAUDRATE)
    print("✅ 硬件初始化完成")
except Exception as e:
    print(f"❌ 初始化失败: {e}")
    exit(1)

# ==================== 根据文档的完整命令集 ====================
# 1. 激光开关（文档2.3.10，已验证可用）
LASER_ON_CMD = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x01, 0xC1])
LASER_OFF_CMD = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x00, 0xC0])

# 2. 尝试不同的测距命令格式
TEST_COMMANDS = [
    # 格式1：文档2.3.12的单次测量（您测试失败的）
    {
        "name": "单次测量 (0x0020)",
        "cmd": bytes([0xAA, 0x00, 0x00, 0x20, 0x00, 0x01, 0x00, 0x00, 0x21]),
        "desc": "文档2.3.12 示例命令"
    },
    # 格式2：文档2.3.7的读取测量结果
    {
        "name": "读取测量结果 (0x0022)",
        "cmd": bytes([0xAA, 0x80, 0x00, 0x22, 0xA2]),
        "desc": "文档2.3.7 读取测量结果"
    },
    # 格式3：文档2.3.13的快速测量
    {
        "name": "快速测量 (0x0022带数据)",
        "cmd": bytes([0xAA, 0x00, 0x00, 0x22, 0x00, 0x01, 0x00, 0x00, 0x23]),
        "desc": "文档2.3.13 快速测量"
    },
    # 格式4：连续测量模式
    {
        "name": "连续测量模式 (0x0021)",
        "cmd": bytes([0xAA, 0x00, 0x00, 0x21, 0x00, 0x01, 0x00, 0x00, 0x22]),
        "desc": "文档2.3.14 连续测量"
    }
]

def clear_buffer():
    """清空串口缓冲区"""
    try:
        data = laser_uart.read(-1)
        if data:
            print(f"清空: {len(data)}字节")
    except:
        pass

def send_and_wait(cmd, name, wait_time=2000):
    """发送命令并等待响应"""
    print(f"\n📤 发送: {name}")
    print(f"   命令: {cmd.hex()}")
    
    clear_buffer()
    
    try:
        laser_uart.write(cmd)
        print(f"   已发送 {len(cmd)} 字节")
    except Exception as e:
        print(f"   ❌ 发送失败: {e}")
        return None
    
    # 等待响应
    start_time = time.ticks_ms()
    response = b""
    
    while time.ticks_ms() - start_time < wait_time:
        try:
            chunk = laser_uart.read(1)
            if chunk:
                response += chunk
                # 完整响应通常是9或13字节
                if len(response) >= 9:
                    # 检查是否完整帧
                    if response[0] in [0xAA, 0xEE]:
                        if len(response) >= 13:  # 测距完整响应
                            break
                        elif response[0] == 0xEE:  # 错误响应
                            break
        except:
            break
        
        time.sleep_ms(10)
    
    if response:
        print(f"   📥 响应: {response.hex()}")
        print(f"   长度: {len(response)} 字节")
        
        # 解析错误码
        if response[0] == 0xEE and len(response) >= 9:
            err_code = (response[7] << 8) | response[8]
            error_mapping = {
                0x0000: "无错误",
                0x0001: "硬件错误",
                0x0002: "无输出数据", 
                0x0003: "反射信号太弱",
                0x0004: "反射信号太强",
                0x0005: "温度太高(>40℃)",
                0x0006: "温度太低(<-10℃)",
                0x0007: "电源电压低(<2.5V)",
                0x0008: "超出量程",
                0x0009: "读通讯错误",
                0x000A: "写通讯错误",
                0x000B: "地址错误"
            }
            err_msg = error_mapping.get(err_code, f"未知错误: 0x{err_code:04X}")
            print(f"   ❌ 模块错误: {err_msg}")
    else:
        print("   ⚠️ 无响应")
    
    return response

def parse_distance_data(response):
    """解析距离数据"""
    if not response or len(response) < 13:
        return None
    
    if response[0] != 0xAA or response[3] not in [0x20, 0x21, 0x22]:
        return None
    
    # 解析4字节BCD码
    bcd_bytes = response[6:10]
    distance_int = 0
    
    for byte in bcd_bytes:
        high = (byte >> 4) & 0x0F
        low = byte & 0x0F
        
        if high > 9 or low > 9:
            return None
        
        distance_int = distance_int * 100 + high * 10 + low
    
    distance_m = distance_int / 1000.0
    
    # 信号质量
    signal = 0
    if len(response) >= 12:
        signal = (response[10] << 8) | response[11]
    
    return {
        'meters': distance_m,
        'millimeters': distance_m * 1000,
        'signal': signal,
        'raw': response.hex()
    }

# ==================== 主测试 ====================
print("\n" + "="*50)
print("M01激光测距模块详细测试")
print("="*50)

try:
    # 1. 测试基本连接
    print("\n1. 测试模块连接...")
    version_cmd = bytes([0xAA, 0x80, 0x00, 0x0A, 0x8A])
    resp = send_and_wait(version_cmd, "读取硬件版本")
    
    if resp and resp[0] == 0xAA and resp[3] == 0x0A:
        print(f"✅ 模块正常，版本: {resp[6]:02X}{resp[7]:02X}")
    else:
        print("❌ 模块连接测试失败")
        exit(1)
    
    # 2. 开启激光
    print("\n2. 开启激光...")
    resp = send_and_wait(LASER_ON_CMD, "开启激光", 1000)
    if resp and resp.hex() == "aa0001be00010001c1":
        print("✅ 激光已开启")
    
    print("   等待激光稳定...")
    time.sleep(2)  # 重要等待时间
    
    # 3. 尝试不同的测距命令
    print("\n3. 测试不同测距命令...")
    
    for i, test_cmd in enumerate(TEST_COMMANDS):
        print(f"\n{'='*30}")
        print(f"测试 {i+1}: {test_cmd['name']}")
        print(f"{test_cmd['desc']}")
        print(f"{'='*30}")
        
        resp = send_and_wait(test_cmd['cmd'], test_cmd['name'], 3000)
        
        if resp:
            if resp[0] == 0xAA and len(resp) >= 13:
                result = parse_distance_data(resp)
                if result:
                    print(f"✅ 测距成功!")
                    print(f"   距离: {result['meters']:.3f} m")
                    print(f"   距离: {result['millimeters']:.1f} mm")
                    print(f"   信号质量: {result['signal']}")
                    break
                else:
                    print("❌ 无法解析距离数据")
            elif resp[0] == 0xEE:
                print("❌ 命令执行错误")
            else:
                print("❌ 无效响应格式")
        else:
            print("❌ 无响应")
        
        time.sleep(1)  # 命令间间隔
    
    # 4. 关闭激光
    print("\n4. 关闭激光...")
    send_and_wait(LASER_OFF_CMD, "关闭激光", 1000)
    
    print("\n" + "="*50)
    print("🏁 测试完成")
    print("="*50)
    
    print("\n📋 测试总结：")
    print("1. 模块通信: ✅ 正常")
    print("2. 激光控制: ✅ 正常")
    print("3. 测距功能: ❌ 有问题")
    print("\n建议：")
    print("1. 检查激光是否实际发光（在暗处观察红点）")
    print("2. 确保测量目标在有效范围内（0.2-60米）")
    print("3. 确保目标有足够反射率（白色平面最佳）")
    print("4. 如果所有测距命令都返回ERR_ADDR，可能是固件版本问题")
    
except KeyboardInterrupt:
    print("\n\n🛑 用户中断")
    laser_uart.write(LASER_OFF_CMD)
    print("✅ 已发送关闭指令")
except Exception as e:
    print(f"\n❌ 测试出错: {e}")