#!/usr/bin/env python3
"""找出GPS大跳变的位置"""
import sys
import math

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

data = []
with open(log_file, 'r', encoding='utf-8', errors='ignore') as f:
    for line in f:
        if line.startswith('$CAL'):
            parts = line.strip().split(',')
            if len(parts) >= 12:
                data.append({
                    'seq': int(parts[1]),
                    'state': parts[3],
                    'x': float(parts[6]),
                    'y': float(parts[7])
                })

print(f"共 {len(data)} 条数据\n")
print("检查单步位移超过1米的跳变：\n")

jump_count = 0
for i in range(1, len(data)):
    dx = data[i]['x'] - data[i-1]['x']
    dy = data[i]['y'] - data[i-1]['y']
    dist = math.sqrt(dx*dx + dy*dy)
    
    if dist > 1.0:  # 单步超过1米认为是跳变
        jump_count += 1
        print(f"跳变#{jump_count}: seq {data[i-1]['seq']}→{data[i]['seq']}, 状态 {data[i-1]['state']}→{data[i]['state']}")
        print(f"  位置: ({data[i-1]['x']:.2f}, {data[i-1]['y']:.2f}) → ({data[i]['x']:.2f}, {data[i]['y']:.2f})")
        print(f"  跳变距离: {dist:.2f}m\n")

if jump_count == 0:
    print("✅ 未发现明显GPS跳变（单步<1米）")
else:
    print(f"⚠️ 共发现 {jump_count} 次GPS跳变，数据质量差！")