#include "external_localization_service/external_localization_common.hpp"

namespace external_localization_service
{

bool TruthSourceValidationAlgorithm::run(
  const ExternalLocalizationInput & input,
  ExternalLocalizationOutput & output,
  std::string & failure_reason) const
{
  // 真值源验证逻辑实现：
  // 1. 检查历史观测数据
  if (input.external_localization_telemetry_history.empty()) {
    failure_reason = "没有观测数据可供验证。";
    return false;
  }

  uint32_t valid_count = 0;
  double total_loss_ratio = 0;
  double total_time_offset = 0;
  double max_pos_stddev = 0;
  double max_yaw_stddev = 0;

  for (const auto & sample : input.external_localization_telemetry_history) {
    if (sample.pose_valid) {
      valid_count++;
      max_pos_stddev = std::max(max_pos_stddev, sample.position_stddev_m);
      max_yaw_stddev = std::max(max_yaw_stddev, sample.yaw_stddev_rad);
    }
    total_loss_ratio += sample.tracking_loss_ratio;
    total_time_offset += std::abs(sample.time_sync_offset_ms);
  }

  const double count_inv = 1.0 / input.external_localization_telemetry_history.size();
  const double avg_loss_ratio = total_loss_ratio * count_inv;
  const double avg_time_offset = total_time_offset * count_inv;
  const double availability = static_cast<double>(valid_count) * count_inv;

  // 2. 判定真值源质量
  bool quality_ok = (availability > 0.95) && (avg_loss_ratio < 0.05) && (avg_time_offset < 50.0);

  if (availability < 0.5) {
    failure_reason = "外部定位可用性过低 (当前: " + std::to_string(availability * 100.0) + "%)";
    return false;
  }

  // 3. 填充成功结果
  fill_external_localization_common_success(
    input,
    output,
    "真值源验证完成。可用性: " + std::to_string(availability * 100.0) + "%",
    "truth_validation_v1.0");

  auto & summary = output.response.result.validation_summary;
  summary.time_sync_ok = (avg_time_offset < 50.0);
  summary.coverage_ok = (availability > 0.9);
  summary.quality_ok = quality_ok;
  summary.tracking_stable = (max_pos_stddev < 0.1);
  summary.recommended_as_truth_source = quality_ok;
  summary.tracking_loss_ratio = avg_loss_ratio;
  summary.time_sync_offset_ms = avg_time_offset;

  auto & res = output.response.result.result;
  res.tracking_loss_ratio = avg_loss_ratio;
  res.time_sync_offset_ms = avg_time_offset;
  res.validated_as_truth_source = quality_ok;

  return true;
}

}  // namespace external_localization_service
