#!/usr/bin/env python3
"""基于可信数据段提取参数"""
import numpy as np

# 手动提取的可信数据点（前进测试，GPS正常）
pwm_speed_pairs = [
    (1400, 0.065),  # FWD_VLOW_C: 0.32m/5.0s
    (1300, 0.298),  # FWD_LOW_C: 1.48m/5.0s
    (1100, 0.493),  # FWD_HIGH_C: 2.44m/5.0s (跳过1200和1050因为有GPS跳变)
]

print("========== 基于可信数据的线性拟合 ==========\n")
print("使用数据点:")
for pwm, v in pwm_speed_pairs:
    print(f"  PWM {pwm}: {v:.3f} m/s")

# 线性拟合：v = k × (1500 - pwm) + bias
pwms = np.array([p[0] for p in pwm_speed_pairs])
speeds = np.array([p[1] for p in pwm_speed_pairs])

x = 1500 - pwms
y = speeds

coef = np.polyfit(x, y, 1)
k = coef[0]
bias = coef[1]

print(f"\n========== 拟合结果 ==========")
print(f"模型：v = {k:.6f} × (1500 - pwm) + ({bias:.6f})")
print(f"\n验证:")
for pwm, v_measured in pwm_speed_pairs:
    v_pred = k * (1500 - pwm) + bias
    print(f"  PWM {pwm}: 实测 {v_measured:.3f} m/s, 预测 {v_pred:.3f} m/s, 误差 {abs(v_measured-v_pred):.3f} m/s")

# 预测极限
v_1000 = k * (1500 - 1000) + bias
v_1500 = bias

print(f"\n========== 预测 ==========")
print(f"PWM 1500 (停止): {v_1500:.3f} m/s (应接近0)")
print(f"PWM 1000 (最大前进): {v_1000:.3f} m/s")

print(f"\n========== 推荐的C代码参数 ==========")
print(f"#define MC_CFG_FORWARD_K         ({k:.6f}f)")
print(f"#define MC_CFG_FORWARD_BIAS      ({bias:.6f}f)")
print(f"#define MC_CFG_MAX_FORWARD_MPS   ({v_1000:.3f}f)")

# 转向数据（从之前的分析结果手动提取，只用前期可信数据）
print(f"\n\n========== 转向参数（基于部分数据）==========")
print("由于后期GPS失锁，转向数据不完整")
print("建议先用保守值：")
print("#define MC_CFG_STEERING_K_LEFT   (0.00010f)  // 待更精确校准")
print("#define MC_CFG_STEERING_K_RIGHT  (0.00010f)  // 待更精确校准")
print("#define MC_CFG_STEERING_TRIM     (10)        // 向右补偿10PWM")