#!/usr/bin/env python3
"""分析每个前进档位的实际速度"""
import math

log_file = "python/tools/calibration_20251204_171502.log"

# 解析所有$CAL数据
data_points = []
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_points.append({
                    'seq': int(parts[1]),
                    'time_ms': int(parts[2]),
                    'state': parts[3],
                    'throttle': int(parts[4]),
                    'steering': int(parts[5]),
                    'x': float(parts[6]),
                    'y': float(parts[7])
                })

print("=" * 80)
print("前进测试各档位平均速度分析")
print("=" * 80)

# 定义需要分析的cruise状态
cruise_states = [
    'FWD_VLOW_C',
    'FWD_LOW_C', 
    'FWD_MID_C',
    'FWD_HIGH_C',
    'FWD_VHIGH_C',
    'FWD_MAX_C'
]

for state in cruise_states:
    # 提取该状态的所有数据点
    state_data = [d for d in data_points if d['state'] == state]
    
    if len(state_data) < 2:
        print(f"\n{state:15s}: 数据不足")
        continue
    
    # 获取起止点
    start = state_data[0]
    end = state_data[-1]
    
    # 计算总距离
    dx = end['x'] - start['x']
    dy = end['y'] - start['y']
    distance = math.sqrt(dx*dx + dy*dy)
    
    # 计算总时间
    time_s = (end['time_ms'] - start['time_ms']) / 1000.0
    
    # 计算平均速度
    avg_speed = distance / time_s if time_s > 0 else 0
    
    # 获取PWM值（应该是恒定的）
    pwm = start['throttle']
    
    # 检查是否有GPS跳变
    max_jump = 0
    for i in range(1, len(state_data)):
        dx_step = state_data[i]['x'] - state_data[i-1]['x']
        dy_step = state_data[i]['y'] - state_data[i-1]['y']
        jump = math.sqrt(dx_step*dx_step + dy_step*dy_step)
        if jump > max_jump:
            max_jump = jump
    
    # 标记数据质量
    quality = "✓" if max_jump < 1.0 else "⚠ GPS跳变"
    
    print(f"\n{state:15s} (PWM={pwm}):")
    print(f"  起点: ({start['x']:6.2f}, {start['y']:6.2f}) m")
    print(f"  终点: ({end['x']:6.2f}, {end['y']:6.2f}) m")
    print(f"  总距离: {distance:6.2f} m")
    print(f"  总时间: {time_s:5.1f} s")
    print(f"  平均速度: {avg_speed:5.3f} m/s  {quality}")
    print(f"  最大单步跳变: {max_jump:5.2f} m")

print("\n" + "=" * 80)
print("数据汇总（仅可信数据）")
print("=" * 80)
print(f"{'PWM':>6s}  {'状态':>15s}  {'速度(m/s)':>10s}  {'质量':>8s}")
print("-" * 80)

for state in cruise_states:
    state_data = [d for d in data_points if d['state'] == state]
    if len(state_data) < 2:
        continue
    
    start = state_data[0]
    end = state_data[-1]
    
    dx = end['x'] - start['x']
    dy = end['y'] - start['y']
    distance = math.sqrt(dx*dx + dy*dy)
    time_s = (end['time_ms'] - start['time_ms']) / 1000.0
    avg_speed = distance / time_s if time_s > 0 else 0
    pwm = start['throttle']
    
    # 检查最大跳变
    max_jump = 0
    for i in range(1, len(state_data)):
        dx_step = state_data[i]['x'] - state_data[i-1]['x']
        dy_step = state_data[i]['y'] - state_data[i-1]['y']
        jump = math.sqrt(dx_step*dx_step + dy_step*dy_step)
        if jump > max_jump:
            max_jump = jump
    
    quality = "可信" if max_jump < 1.0 else "跳变"
    marker = "  ✓" if max_jump < 1.0 else "  ✗"
    
    print(f"{pwm:6d}  {state:>15s}  {avg_speed:10.3f}  {quality:>8s}{marker}")

print("=" * 80)