#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
GPS/IMU PythonLink 协议验证脚本

功能:
 1. 串口实时读取帧 (live 模式) 验证 GPS 帧 checksum 与结构解包
 2. 构造本地示例帧 (sample 模式) 做自测

协议: Frame = AA 55 | Type(1) | Len(2 LE) | Payload | Checksum(2 LE) | 0D 0A
Checksum = (sum of Type + Len(2B) + Payload bytes) & 0xFFFF (小端存储)
GPS Type = 0x01, Payload = 44 字节: 
  double lat, double lon,
  float heading,
  float vel_east, float vel_north, float vel_up,
  float altitude,
  uint32 utc_time,
  uint8 pos_quality, uint8 sat_count, 2 bytes reserved

使用示例:
  python tools/verify_gps.py --port COM28 --baud 921600 --mode live
  python tools/verify_gps.py --mode sample

按 Ctrl+C 退出实时模式。
"""
import argparse
import struct
import sys
import time
from typing import Optional, Tuple

try:
    import serial  # pyserial
except ImportError:
    serial = None

HEADER1 = 0xAA
HEADER2 = 0x55
FOOTER1 = 0x0D
FOOTER2 = 0x0A
TYPE_GPS = 0x01
# TYPE_IMU = 0x02  # 备用
GPS_PAYLOAD_LEN = 44
FRAME_HEADER_LEN = 4  # 0xAA 0x55 Type Len(2)
FRAME_FOOTER_LEN = 4  # Checksum(2) 0x0D 0x0A

GPS_STRUCT_FMT = '<ddfffffI2B2x'
# 验证结构尺寸
assert struct.calcsize(GPS_STRUCT_FMT) == GPS_PAYLOAD_LEN, 'GPS struct size mismatch'

class GPSFrame:
    def __init__(self, lat, lon, heading, vel_e, vel_n, vel_u, altitude, utc_time, pos_q, sat_cnt):
        self.lat = lat
        self.lon = lon
        self.heading = heading
        self.vel_e = vel_e
        self.vel_n = vel_n
        self.vel_u = vel_u
        self.altitude = altitude
        self.utc_time = utc_time
        self.pos_q = pos_q
        self.sat_cnt = sat_cnt

    def __str__(self):
        return (f'GPSFrame(lat={self.lat:.8f}, lon={self.lon:.8f}, heading={self.heading:.2f}, '
                f'vel_e={self.vel_e:.2f}, vel_n={self.vel_n:.2f}, vel_u={self.vel_u:.2f}, '
                f'alt={self.altitude:.2f}, utc={self.utc_time}, posQ={self.pos_q}, sats={self.sat_cnt})')

def calc_checksum(type_byte: int, length_le: bytes, payload: bytes) -> int:
    s = type_byte + sum(length_le) + sum(payload)
    return s & 0xFFFF

def parse_gps_payload(payload: bytes) -> GPSFrame:
    (lat, lon, heading, vel_e, vel_n, vel_u, altitude, utc_time, pos_q, sat_cnt) = struct.unpack(GPS_STRUCT_FMT, payload)
    return GPSFrame(lat, lon, heading, vel_e, vel_n, vel_u, altitude, utc_time, pos_q, sat_cnt)

def parse_frame(buf: bytes) -> Tuple[Optional[GPSFrame], str]:
    # 最小长度检查
    if len(buf) < FRAME_HEADER_LEN + FRAME_FOOTER_LEN:
        return None, 'buffer too short'
    if buf[0] != HEADER1 or buf[1] != HEADER2:
        return None, 'header mismatch'
    type_byte = buf[2]
    length = buf[3] | (buf[4] << 8)
    total_needed = FRAME_HEADER_LEN + length + FRAME_FOOTER_LEN
    if len(buf) < total_needed:
        return None, 'incomplete frame'
    payload = buf[5:5+length]
    checksum_recv = buf[5+length] | (buf[6+length] << 8)
    footer1 = buf[7+length]
    footer2 = buf[8+length]
    checksum_calc = calc_checksum(type_byte, buf[3:5], payload)
    if checksum_recv != checksum_calc:
        return None, f'checksum error recv=0x{checksum_recv:04X} calc=0x{checksum_calc:04X}'
    if footer1 != FOOTER1 or footer2 != FOOTER2:
        return None, f'footer error got=[0x{footer1:02X},0x{footer2:02X}]'
    if type_byte != TYPE_GPS:
        return None, f'unsupported type 0x{type_byte:02X}'
    if length != GPS_PAYLOAD_LEN:
        return None, f'payload length mismatch {length} != {GPS_PAYLOAD_LEN}'
    gps = parse_gps_payload(payload)
    return gps, 'ok'

def run_live(port: str, baud: int, timeout: float):
    if serial is None:
        print('pyserial 未安装，先执行: pip install pyserial', file=sys.stderr)
        sys.exit(1)
    ser = serial.Serial(port=port, baudrate=baud, timeout=0.05)
    print(f'[Live] 打开串口 {port} @ {baud}')
    buffer = bytearray()
    stats_ok = 0
    stats_err = 0
    last_print = time.time()
    start = time.time()
    try:
        while True:
            chunk = ser.read(4096)
            if chunk:
                buffer.extend(chunk)
            # 搜索帧头
            while True:
                idx = buffer.find(bytes([HEADER1, HEADER2]))
                if idx < 0:
                    # 没有头，丢弃旧数据防止无限膨胀
                    if len(buffer) > 8192:
                        del buffer[:-2]
                    break
                # 丢弃头前数据
                if idx > 0:
                    del buffer[:idx]
                if len(buffer) < FRAME_HEADER_LEN:
                    break
                # 长度字段
                if len(buffer) >= 5:
                    length = buffer[3] | (buffer[4] << 8)
                    total_len = FRAME_HEADER_LEN + length + FRAME_FOOTER_LEN
                    if len(buffer) < total_len:
                        break  # 等更多字节
                    frame_bytes = bytes(buffer[:total_len])
                    del buffer[:total_len]
                    gps, status = parse_frame(frame_bytes)
                    if gps is not None:
                        stats_ok += 1
                        print(f'[OK] {gps}')
                    else:
                        stats_err += 1
                        print(f'[ERR] {status}')
                else:
                    break
            if timeout > 0 and (time.time() - start) >= timeout:
                print('达到超时时间, 退出')
                break
            if time.time() - last_print > 5:
                last_print = time.time()
                print(f'[Stats] OK={stats_ok} ERR={stats_err} RunTime={time.time()-start:.1f}s')
    except KeyboardInterrupt:
        print('\n用户中断')
    finally:
        ser.close()
        print(f'结束: OK={stats_ok} ERR={stats_err}')

def run_sample():
    # 构造一个示例 GPS payload
    lat = 31.12345678
    lon = 121.98765432
    heading = 45.25
    vel_e = 0.12
    vel_n = -0.05
    vel_u = 0.01
    altitude = 12.34
    utc_time = 123456789
    pos_q = 4
    sat_cnt = 18
    payload = struct.pack(GPS_STRUCT_FMT, lat, lon, heading, vel_e, vel_n, vel_u, altitude, utc_time, pos_q, sat_cnt)
    length_le = struct.pack('<H', len(payload))
    checksum = calc_checksum(TYPE_GPS, length_le, payload)
    frame = bytearray()
    frame.append(HEADER1)
    frame.append(HEADER2)
    frame.append(TYPE_GPS)
    frame.extend(length_le)
    frame.extend(payload)
    frame.extend(struct.pack('<H', checksum))
    frame.append(FOOTER1)
    frame.append(FOOTER2)
    gps, status = parse_frame(bytes(frame))
    print(f'Sample frame parse status: {status}')
    if gps:
        print(gps)
    print('Raw frame hex:', frame.hex())

def main():
    ap = argparse.ArgumentParser(description='PythonLink GPS 验证工具')
    ap.add_argument('--port', '-p', default='COM28', help='串口号 (live 模式需要)')
    ap.add_argument('--baud', '-b', type=int, default=921600, help='波特率')
    ap.add_argument('--mode', '-m', choices=['live', 'sample'], default='live', help='运行模式: live=实时串口, sample=本地示例')
    ap.add_argument('--timeout', '-t', type=float, default=0, help='live 模式运行秒数(0=无限直到Ctrl+C)')
    args = ap.parse_args()
    if args.mode == 'sample':
        run_sample()
    else:
        run_live(args.port, args.baud, args.timeout)

if __name__ == '__main__':
    main()