#!/usr/bin/env python3
"""检查GPS数据是否有异常跳变"""
import sys
import math

log_file = sys.argv[1] if len(sys.argv) > 1 else "calibration_20251204_171502.log"

# 读取所有巡航状态数据
cruise_states = ['FWD_VLOW_C', 'FWD_LOW_C', 'FWD_MID_C', 'FWD_HIGH_C', 'FWD_VHIGH_C', 'FWD_MAX_C']

for state_name in cruise_states:
    data = []
    with open(log_file, 'r', encoding='utf-8', errors='ignore') as f:
        for line in f:
            if state_name in line and line.startswith('$CAL'):
                parts = line.strip().split(',')
                if len(parts) >= 12:
                    data.append({
                        'time': int(parts[2]),
                        'pwm': int(parts[4]),
                        'x': float(parts[6]),
                        'y': float(parts[7]),
                        'hdg': float(parts[9])
                    })
    
    if len(data) < 10:
        continue
    
    print(f"\n========== {state_name} (PWM {data[0]['pwm']}) ==========")
    
    # 检查跳变
    max_jump = 0
    max_speed = 0
    for i in range(1, len(data)):
        dx = data[i]['x'] - data[i-1]['x']
        dy = data[i]['y'] - data[i-1]['y']
        dt = (data[i]['time'] - data[i-1]['time']) / 1000.0
        
        dist = math.sqrt(dx*dx + dy*dy)
        if dt > 0:
            speed = dist / dt
            if speed > max_speed:
                max_speed = speed
            if dist > max_jump:
                max_jump = dist
    
    # 计算总体平均速度
    total_dx = data[-1]['x'] - data[0]['x']
    total_dy = data[-1]['y'] - data[0]['y']
    total_dist = math.sqrt(total_dx*total_dx + total_dy*total_dy)
    total_time = (data[-1]['time'] - data[0]['time']) / 1000.0
    avg_speed = total_dist / total_time if total_time > 0 else 0
    
    print(f"总体: 距离{total_dist:.2f}m, 时间{total_time:.1f}s, 平均速度{avg_speed:.3f} m/s")
    print(f"单步最大跳变: {max_jump:.2f}m, 最大瞬时速度: {max_speed:.3f} m/s")
    
    if max_jump > 2.0 or max_speed > 10.0:
        print(f"⚠️ 警告：存在GPS跳变！数据不可信！")
    elif avg_speed > 3.0:
        print(f"⚠️ 警告：平均速度异常高！")