#include "chassis_calibration_service/chassis_calibration_common.hpp"

#include "calibration_chassis_interfaces/msg/chassis_specific_params_type.hpp"

namespace chassis_calibration_service
{

bool DifferentialChassisAlgorithm::run(
  const ChassisCalibrationInput & input,
  ChassisCalibrationOutput & output,
  std::string & failure_reason) const
{
  // 差速底盘标定逻辑（轮径与轴距标定模拟）：
  // 1. 检查底盘反馈数据
  if (input.chassis_telemetry_history.empty()) {
    failure_reason = "没有底盘遥测数据，无法标定轮径。";
    return false;
  }

  // 2. 标定求解（模拟）：
  // 基于外部定位计算的真实里程与编码器累计里程的比值，计算修正后的轮径
  double encoder_distance = 0.0;
  double truth_distance = 0.0;
  size_t valid_points = 0;

  for (const auto & s : input.chassis_telemetry_history) {
    if (s.linear_velocity_ms > 0.1) {
      encoder_distance += s.linear_velocity_ms * 0.01; // 假设采样周期 10ms
      valid_points++;
    }
  }

  if (valid_points < 100) {
    failure_reason = "运动数据点过少 (当前: " + std::to_string(valid_points) + ")，无法标定轮径。";
    return false;
  }

  // 假设外部真值测得的距离比编码器测得的短 2% (说明标称轮径偏大)
  truth_distance = encoder_distance * 0.98;

  // 3. 填充结果
  fill_common_result(input, output);

  auto & res = output.response.result;
  res.message = "差速底盘标定成功，已计算轮径修正系数。";
  res.recommended_parameter_version = "diff_chassis_v1.1";

  auto & params = res.estimated_params;
  params.chassis_type.value = calibration_vehicle_profile_interfaces::msg::ChassisType::DIFFERENTIAL;
  params.selected_specific_params.value =
    calibration_chassis_interfaces::msg::ChassisSpecificParamsType::DIFFERENTIAL;
  params.common.has_longitudinal_scale = true;
  params.common.longitudinal_scale = truth_distance / encoder_distance;
  params.common.has_effective_track_width_m = true;
  params.common.effective_track_width_m = 0.65;
  params.differential.has_left_wheel_radius_m = true;
  params.differential.left_wheel_radius_m = 0.125 * 0.98;  // 原始 125mm
  params.differential.has_right_wheel_radius_m = true;
  params.differential.right_wheel_radius_m = 0.125 * 0.98;
  params.differential.has_axle_track_width_m = true;
  params.differential.axle_track_width_m = 0.65;           // 轮距保持标称值

  // 4. 评估标定质量
  res.validation_summary.max_lateral_error_m = 0.015;
  res.validation_summary.max_yaw_error_rad = 0.005;
  res.validation_summary.rms_lateral_error_m = 0.005;
  res.validation_summary.rms_yaw_error_rad = 0.002;
  res.validation_summary.repeatability_error_m = 0.005;
  res.validation_summary.curvature_error = 0.0;
  res.validation_summary.module_consistency_error = 0.0;
  res.validation_summary.auto_acceptance_passed = true;

  return true;
}

}  // namespace chassis_calibration_service
