#include "sensor_calibration_service/sensor_calibration_algorithm_template.hpp"

namespace sensor_calibration_service
{

// 统一入口校验。
// 这里主要检查：
// 1. 这次传感器标定任务是不是一个合法请求；
// 2. 当前任务类型是否已经明确；
// 3. 对于大多数任务，目标传感器 ID 是否已经解析出来。
// 更细的业务校验（例如图像数、采样数、机械臂位姿数是否合法）
// 推荐在具体算法文件中再按任务类型继续做。
bool SensorCalibrationAlgorithmTemplate::validate_input(
  const Input & input,
  std::string & failure_reason) const
{
  if (input.request.goal.header.request_id.empty()) {
    failure_reason = "sensor 任务 request_id 不能为空。";
    return false;
  }

  if (input.task_type.value == TaskType::SENSOR_CALIBRATION_TASK_TYPE_UNSPECIFIED) {
    failure_reason = "传感器标定任务类型不能为空。";
    return false;
  }

  if (input.task_subtype.value == TaskSubtype::SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED) {
    failure_reason = "传感器标定任务子类型不能为空。";
    return false;
  }

  if (input.task_type.value != TaskType::IMU_INTRINSIC && input.target_sensor_id.empty()) {
    failure_reason = "目标传感器 ID 不能为空。";
    return false;
  }

  return true;
}

const SensorCalibrationAlgorithm * SensorCalibrationAlgorithmTemplate::resolve_algorithm(
  const TaskType & task_type,
  const TaskSubtype & task_subtype) const
{
  switch (task_type.value) {
    case TaskType::CAMERA_INTRINSIC:
      switch (task_subtype.value) {
        case TaskSubtype::FRONT_CAMERA_INTRINSIC:
          return &front_camera_intrinsic_algorithm_;
        case TaskSubtype::DOWNWARD_CAMERA_INTRINSIC:
          return &downward_camera_intrinsic_algorithm_;
        default:
          return nullptr;
      }
    case TaskType::IMU_INTRINSIC:
      switch (task_subtype.value) {
        case TaskSubtype::IMU_INTRINSIC:
          return &imu_intrinsic_algorithm_;
        default:
          return nullptr;
      }
    case TaskType::SENSOR_TO_BASE_EXTRINSIC:
      switch (task_subtype.value) {
        case TaskSubtype::FRONT_CAMERA_EXTRINSIC:
          return &front_camera_extrinsic_algorithm_;
        case TaskSubtype::DOWNWARD_CAMERA_EXTRINSIC:
          return &downward_camera_extrinsic_algorithm_;
        case TaskSubtype::IMU_EXTRINSIC:
          return &imu_extrinsic_algorithm_;
        case TaskSubtype::LIDAR_2D_EXTRINSIC:
          return &lidar_2d_extrinsic_algorithm_;
        case TaskSubtype::LIDAR_3D_EXTRINSIC:
          return &lidar_3d_extrinsic_algorithm_;
        default:
          return nullptr;
      }
    case TaskType::HAND_EYE:
      switch (task_subtype.value) {
        case TaskSubtype::EYE_IN_HAND:
          return &eye_in_hand_algorithm_;
        case TaskSubtype::EYE_TO_HAND:
          return &eye_to_hand_algorithm_;
        default:
          return nullptr;
      }
    default:
      return nullptr;
  }
}

bool SensorCalibrationAlgorithmTemplate::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.task_type, input.task_subtype);
  if (algorithm == nullptr) {
    failure_reason = "当前 task_type 与 task_subtype 组合不受支持。";
    return false;
  }

  // 这里是传感器标定/评估算法的统一入口：
  // 1. 先按 task_type 分发到相机内参、IMU 内参、外参、手眼的专属模板；
  // 2. 各算法模板再结合遥测、底盘反馈、控制反馈、真值源反馈进入求解；
  // 3. 最终统一把结果写回 output.response.result。
  return algorithm->run(input, output, failure_reason);
}

}  // namespace sensor_calibration_service
