#include "external_localization_service/external_localization_algorithm_template.hpp"

namespace external_localization_service
{

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

const ExternalLocalizationAlgorithm * ExternalLocalizationAlgorithmTemplate::resolve_algorithm(
  const ExternalTaskType & task_type) const
{
  switch (task_type.value) {
    case ExternalTaskType::REFERENCE_POSE_COLLECTION:
      return &reference_pose_collection_algorithm_;
    case ExternalTaskType::MARKER_ALIGNMENT:
      return &marker_alignment_algorithm_;
    case ExternalTaskType::TRUTH_SOURCE_VALIDATION:
      return &truth_source_validation_algorithm_;
    default:
      return nullptr;
  }
}

bool ExternalLocalizationAlgorithmTemplate::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);
  if (algorithm == nullptr) {
    failure_reason = "不支持的外部定位任务类型。";
    return false;
  }

  // 这里是外部定位算法的统一入口：先按任务类型分发，再由对应模板文件实现具体算法。
  // 算法工程师通常会先检查：
  // - input.workshop_session / input.vehicle_profile：会话级上下文和车辆静态画像
  // - input.external_localization_*：能力、就绪状态、遥测、历史窗口、真值源诊断
  // - input.chassis_*：车辆运动状态，便于做短运动段验证和时序对齐
  // - input.control_*：控制闭环输出，便于和真值源做对比
  // - input.sensor_*：图像 / IMU / LiDAR 等采样质量，辅助判断真值数据是否可信
  // 再进入具体采集、对齐、稳定性分析和结果求解。
  return algorithm->run(input, output, failure_reason);
}

}  // namespace external_localization_service
