#include "external_localization_service/external_localization_common.hpp"

#include <cmath>

namespace external_localization_service
{

bool ReferencePoseCollectionAlgorithm::run(
  const ExternalLocalizationInput & input,
  ExternalLocalizationOutput & output,
  std::string & failure_reason) const
{
  // 参考位姿采集逻辑实现：
  // 1. 检查输入样本
  if (input.external_localization_telemetry_history.empty()) {
    failure_reason = "没有外部定位观测数据，无法进行参考位姿采集。";
    return false;
  }

  // 2. 检查车辆静止状态（如果任务要求）
  if (input.require_vehicle_static) {
    bool moving = false;
    for (const auto & chassis : input.chassis_telemetry_history) {
      if (std::abs(chassis.linear_velocity_ms) > 0.01 || std::abs(chassis.angular_velocity_rads) > 0.01) {
        moving = true;
        break;
      }
    }
    if (moving) {
      failure_reason = "车辆未处于静止状态，无法采集高精度参考位姿。";
      return false;
    }
  }

  const uint32_t required_count = input.required_reference_pose_sample_count > 0 ?
                                  input.required_reference_pose_sample_count : 10;

  std::vector<calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry> valid_samples;
  for (const auto & sample : input.external_localization_telemetry_history) {
    if (sample.pose_valid && sample.quality_score > 0.6) {
      valid_samples.push_back(sample);
    }
  }

  if (valid_samples.size() < required_count) {
    failure_reason = "有效样本数不足 (当前: " + std::to_string(valid_samples.size()) +
                     ", 需要: " + std::to_string(required_count) + ")。";
    return false;
  }

  // 3. 计算统计结果
  double sum_x = 0, sum_y = 0, sum_yaw = 0;
  for (const auto & s : valid_samples) {
    sum_x += s.workshop_pose.x_m;
    sum_y += s.workshop_pose.y_m;
    sum_yaw += s.workshop_pose.yaw_rad;
  }
  const double count_inv = 1.0 / valid_samples.size();
  const double avg_x = sum_x * count_inv;
  const double avg_y = sum_y * count_inv;
  const double avg_yaw = sum_yaw * count_inv;

  double var_pos = 0, var_yaw = 0;
  for (const auto & s : valid_samples) {
    double dx = s.workshop_pose.x_m - avg_x;
    double dy = s.workshop_pose.y_m - avg_y;
    double dyaw = s.workshop_pose.yaw_rad - avg_yaw;
    var_pos += (dx * dx + dy * dy);
    var_yaw += (dyaw * dyaw);
  }

  // 4. 填充结果
  fill_external_localization_common_success(
    input,
    output,
    "参考位姿采集完成，已采集 " + std::to_string(valid_samples.size()) + " 个有效样本。",
    "reference_collection_v1.0");

  auto & res = output.response.result.result;
  res.workshop_frame_id = "workshop";
  res.localization_frame_id = "localization";
  res.position_repeatability_m = std::sqrt(var_pos * count_inv);
  res.yaw_repeatability_rad = std::sqrt(var_yaw * count_inv);

  // 验证结果是否可信
  res.validated_as_truth_source = (res.position_repeatability_m < 0.02); // 静态重复性优于 2cm

  return true;
}

}  // namespace external_localization_service
