#include "control_calibration_service/control_calibration_common.hpp"

#include <cmath>

#include "calibration_control_interfaces/msg/controller_parameter_kind.hpp"
#include "calibration_control_interfaces/msg/controller_parameter_pack.hpp"
#include "calibration_vehicle_profile_interfaces/msg/control_axis_type.hpp"

namespace control_calibration_service
{

bool PurePursuitControlCalibrationAlgorithm::run(
  const ControlCalibrationInput & input,
  ControlCalibrationOutput & output,
  std::string & failure_reason) const
{
  // Pure Pursuit 控制标定逻辑（模拟）：
  // 1. 检查控制反馈数据
  if (input.control_telemetry_history.empty()) {
    failure_reason = "没有控制遥测数据，无法评估控制效果。";
    return false;
  }

  // 2. 统计评估（模拟）：
  // 对历史观测中的横向误差进行均方根（RMS）统计
  double sum_sq_lat_err = 0.0;
  double sum_sq_heading_err = 0.0;
  double sum_sq_speed_err = 0.0;
  size_t saturation_points = 0;
  size_t valid_points = 0;

  for (const auto & s : input.control_telemetry_history) {
    if (s.active_job_id.empty()) {
      continue;
    }
    sum_sq_lat_err += (s.lateral_error_m * s.lateral_error_m);
    sum_sq_heading_err += (s.heading_error_rad * s.heading_error_rad);
    sum_sq_speed_err += (s.speed_error_ms * s.speed_error_ms);
    if (s.saturation_flag) {
      saturation_points++;
    }
    valid_points++;
  }

  if (valid_points < 50) {
    failure_reason = "有效跟踪点过少 (当前: " + std::to_string(valid_points) + ")，无法评估控制质量。";
    return false;
  }

  const double inv_points = 1.0 / static_cast<double>(valid_points);
  const double rms_lat_err = std::sqrt(sum_sq_lat_err * inv_points);
  const double rms_heading_err = std::sqrt(sum_sq_heading_err * inv_points);
  const double rms_speed_err = std::sqrt(sum_sq_speed_err * inv_points);

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

  auto & res = output.response.result;
  res.message = "Pure Pursuit 控制标定/评估完成。";
  res.recommended_parameter_version = "pp_params_v2.0";

  calibration_control_interfaces::msg::ControllerParameterPack params;
  params.control_axis.value =
    calibration_vehicle_profile_interfaces::msg::ControlAxisType::LATERAL_CONTROL;
  params.algorithm_type.value =
    calibration_vehicle_profile_interfaces::msg::ControllerAlgorithmType::PURE_PURSUIT;
  params.selected_params.value =
    calibration_control_interfaces::msg::ControllerParameterKind::PURE_PURSUIT;
  params.pure_pursuit.lookahead_m = 1.2;
  params.pure_pursuit.has_min_lookahead_m = true;
  params.pure_pursuit.min_lookahead_m = 0.5;
  params.pure_pursuit.has_max_lookahead_m = true;
  params.pure_pursuit.max_lookahead_m = 2.5;
  params.pure_pursuit.has_curvature_gain = true;
  params.pure_pursuit.curvature_gain = 1.0;
  params.pure_pursuit.has_steering_limit_deg = true;
  params.pure_pursuit.steering_limit_deg = 32.0;
  res.estimated_parameter_set.items.clear();
  res.estimated_parameter_set.items.push_back(params);

  // 4. 评估控制质量
  res.validation_summary.rms_lateral_error_m = rms_lat_err;
  res.validation_summary.rms_heading_error_rad = rms_heading_err;
  res.validation_summary.rms_speed_error_ms = rms_speed_err;
  res.validation_summary.saturation_ratio =
    static_cast<double>(saturation_points) / static_cast<double>(valid_points);
  res.validation_summary.auto_acceptance_passed = (rms_lat_err < 0.05); // 示例验收标准：5cm RMS

  return true;
}

}  // namespace control_calibration_service
