#pragma once

#include <string>

#include "calibration_common_interfaces/msg/error_code.hpp"
#include "external_localization_service/external_localization_algorithms.hpp"

namespace external_localization_service
{

// 这里放外部定位算法模板共用的默认结果填充逻辑。
// 这样每个具体算法 cpp 不需要从零开始拼装 ExecuteExternalLocalizationTask::Result，
// 只需要在此基础上补充本算法真正求解出的关键结果即可。
inline void fill_external_localization_common_success(
  const ExternalLocalizationInput & input,
  ExternalLocalizationOutput & output,
  const std::string & success_message,
  const std::string & parameter_version)
{
  output.response.result.success = true;
  output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::OK;
  output.response.result.message = success_message;
  output.response.result.job_id = input.request.goal.header.request_id;
  output.response.result.task_purpose = input.request.goal.task_purpose;
  output.response.result.data_quality_passed = true;
  output.response.result.suitable_for_commit = true;
  output.response.result.recommended_parameter_version = parameter_version;

  output.response.result.validation_summary.time_sync_ok = true;
  output.response.result.validation_summary.coverage_ok = true;
  output.response.result.validation_summary.quality_ok = true;
  output.response.result.validation_summary.tracking_stable = true;
  output.response.result.validation_summary.recommended_as_truth_source = true;
  output.response.result.validation_summary.position_stddev_m = 0.0;
  output.response.result.validation_summary.yaw_stddev_rad = 0.0;
  output.response.result.validation_summary.tracking_loss_ratio = 0.0;
  output.response.result.validation_summary.time_sync_offset_ms = 0.0;
  output.response.result.validation_summary.issues.clear();

  output.response.result.result = input.candidate_calibration_result;
  output.response.result.result.localization_source_id = input.localization_source_id;
  output.response.result.result.workcell_zone_id = input.workcell_zone_id;
}

inline void fill_external_localization_common_failure(
  ExternalLocalizationOutput & output,
  const std::string & failure_reason)
{
  output.response.result.success = false;
  output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::INVALID_ARGUMENT;
  output.response.result.message = failure_reason;
  output.response.result.data_quality_passed = false;
  output.response.result.suitable_for_commit = false;
  output.response.result.recommended_parameter_version.clear();
}

}  // namespace external_localization_service
