#include "control_calibration_service/control_calibration_algorithm_template.hpp"

#include "calibration_control_interfaces/msg/controller_evaluation_task_type.hpp"

namespace control_calibration_service
{

// 统一入口校验。
// 这里主要检查：
// 1. 这次控制评估任务是不是一个合法请求；
// 2. node 是否已经推导出明确的控制算法类型；
// 3. 当前任务类型是否已经明确。
// 更细的业务校验（例如轨迹点数量、速度范围、停车目标是否有效）
// 推荐在具体算法文件中再按任务类型继续做。
bool ControlCalibrationAlgorithmTemplate::validate_input(
  const Input & input,
  std::string & failure_reason) const
{
  if (input.request.goal.header.request_id.empty()) {
    failure_reason = "control 任务 request_id 不能为空。";
    return false;
  }

  if (input.controller_algorithm.value == ControllerAlgorithmType::CONTROLLER_ALGORITHM_UNSPECIFIED) {
    failure_reason = "控制算法类型不能为空，必须明确是 PID、MPC、LQR 或 PURE_PURSUIT。";
    return false;
  }

  if (input.request.goal.selected_task.value ==
      calibration_control_interfaces::msg::ControllerEvaluationTaskType::CONTROL_EVALUATION_TASK_UNSPECIFIED) {
    failure_reason = "selected_task 不能为空，必须明确控制评估任务类型。";
    return false;
  }

  return true;
}

const ControlCalibrationAlgorithm * ControlCalibrationAlgorithmTemplate::resolve_algorithm(
  const ControllerAlgorithmType & controller_algorithm) const
{
  switch (controller_algorithm.value) {
    case ControllerAlgorithmType::PID:
      return &pid_algorithm_;
    case ControllerAlgorithmType::MPC:
      return &mpc_algorithm_;
    case ControllerAlgorithmType::LQR:
      return &lqr_algorithm_;
    case ControllerAlgorithmType::PURE_PURSUIT:
      return &pure_pursuit_algorithm_;
    default:
      return nullptr;
  }
}

bool ControlCalibrationAlgorithmTemplate::run(
  const Input & input,
  Output & output,
  std::string & failure_reason) const
{
  if (!validate_input(input, failure_reason)) {
    return false;
  }

  const auto * algorithm = resolve_algorithm(input.controller_algorithm);
  if (algorithm == nullptr) {
    failure_reason = "不支持的控制算法类型。";
    return false;
  }

  // 这里是控制标定/评估算法的统一入口：
  // 1. 先按 controller_algorithm 分发到对应算法模板；
  // 2. 各算法模板再结合 task_type、轨迹/速度/停车任务 payload、控制遥测、底盘反馈、真值源反馈进入求解；
  // 3. 最终统一把结果写回 output.response.result。
  // 算法工程师通常会重点读取：
  // - input.vehicle_profile：控制轴支持算法、默认算法、车辆静态画像
  // - input.control_*：当前控制参数、控制误差、控制输出、饱和状态、工作模式
  // - input.chassis_*：底盘运动状态和执行器反馈
  // - input.sensor_*：姿态/速度/定位等观测质量
  // - input.external_localization_*：真值位姿、同步偏差、质量评分
  return algorithm->run(input, output, failure_reason);
}

}  // namespace control_calibration_service
