#include "chassis_calibration_service/chassis_calibration_algorithm_template.hpp"

namespace chassis_calibration_service
{

bool ChassisCalibrationAlgorithmTemplate::validate_input(
  const Input & input,
  std::string & failure_reason) const
{
  if (input.request.goal.header.request_id.empty()) {
    failure_reason = "chassis 任务 request_id 不能为空。";
    return false;
  }

  if (input.chassis_type.value == ChassisType::CHASSIS_TYPE_UNSPECIFIED) {
    failure_reason = "底盘类型不能为空，必须明确是阿克曼、差速、单舵轮或多舵轮。";
    return false;
  }

  if (input.request.goal.selected_primitive.value !=
      calibration_chassis_interfaces::msg::ChassisMotionPrimitiveType::STRAIGHT_LINE) {
    failure_reason = "当前模板只演示 straight_line 动作原语；如果要支持转弯、原地旋转、横移等原语，需要在这里扩展分发规则。";
    return false;
  }

  if (input.request.goal.straight_line.target_distance_m <= 0.0) {
    failure_reason = "straight_line.target_distance_m 必须大于 0。";
    return false;
  }
  if (input.request.goal.straight_line.target_speed_ms <= 0.0) {
    failure_reason = "straight_line.target_speed_ms 必须大于 0。";
    return false;
  }

  return true;
}

const ChassisCalibrationAlgorithm * ChassisCalibrationAlgorithmTemplate::resolve_algorithm(
  const ChassisType & chassis_type) const
{
  switch (chassis_type.value) {
    case ChassisType::ACKERMANN:
      return &ackermann_algorithm_;
    case ChassisType::DIFFERENTIAL:
      return &differential_algorithm_;
    case ChassisType::SINGLE_STEER_WHEEL:
      return &single_steer_wheel_algorithm_;
    case ChassisType::MULTI_STEER_WHEEL:
      return &multi_steer_wheel_algorithm_;
    default:
      return nullptr;
  }
}

bool ChassisCalibrationAlgorithmTemplate::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.chassis_type);
  if (algorithm == nullptr) {
    failure_reason = "不支持的底盘类型。";
    return false;
  }

  // 这里是标定算法的统一入口：先按底盘类型分发，再由对应模板文件实现具体算法。
  // 算法工程师通常会先检查：
  // - input.workshop_session / input.vehicle_profile：会话级上下文和车辆静态画像
  // - input.chassis_*：底盘能力、就绪状态、工作模式、参数、遥测
  // - input.control_*：控制链路、控制参数、控制误差和输出
  // - input.sensor_*：传感器就绪状态、已生效参数、采集质量
  // - input.external_localization_*：真值源就绪状态和外部定位观测
  // 再进入具体拟合与参数求解。
  return algorithm->run(input, output, failure_reason);
}

}  // namespace chassis_calibration_service
