#include "external_localization_service/external_localization_common.hpp"

#include <cmath>

namespace external_localization_service
{

bool MarkerAlignmentAlgorithm::run(
  const ExternalLocalizationInput & input,
  ExternalLocalizationOutput & output,
  std::string & failure_reason) const
{
  // 标靶对齐逻辑实现：
  // 1. 检查输入样本是否充足
  if (input.external_localization_telemetry_history.empty()) {
    failure_reason = "没有外部定位观测数据，无法进行标靶对齐。";
    return false;
  }

  const uint32_t required_count = input.marker_alignment_task.min_valid_observation_count > 0 ?
                                  input.marker_alignment_task.min_valid_observation_count : 1;

  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.5) {
      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;
  }

  // 2. 计算平均位姿（简化实现：取均值）
  double sum_x = 0, sum_y = 0, sum_z = 0;
  double sum_roll = 0, sum_pitch = 0, sum_yaw = 0;

  for (const auto & sample : valid_samples) {
    sum_x += sample.workshop_pose.x_m;
    sum_y += sample.workshop_pose.y_m;
    sum_z += sample.workshop_pose.z_m;
    sum_roll += sample.workshop_pose.roll_rad;
    sum_pitch += sample.workshop_pose.pitch_rad;
    sum_yaw += sample.workshop_pose.yaw_rad;
  }

  const double count_inv = 1.0 / valid_samples.size();

  // 3. 填充成功结果
  fill_external_localization_common_success(
    input,
    output,
    "标靶对齐成功，基于 " + std::to_string(valid_samples.size()) + " 个样本计算。",
    "marker_alignment_v1.0");

  auto & res = output.response.result.result;
  res.workshop_frame_id = "workshop";
  res.localization_frame_id = "localization";

  // 假设观测到的位姿即为对齐后的变换（实际生产中需配合标靶真值计算 T_w_l = T_w_marker * inv(T_l_marker)）
  res.workshop_to_localization.x_m = sum_x * count_inv;
  res.workshop_to_localization.y_m = sum_y * count_inv;
  res.workshop_to_localization.z_m = sum_z * count_inv;
  res.workshop_to_localization.roll_rad = sum_roll * count_inv;
  res.workshop_to_localization.pitch_rad = sum_pitch * count_inv;
  res.workshop_to_localization.yaw_rad = sum_yaw * count_inv;

  // 4. 计算残差（标准差）
  double var_pos = 0;
  double var_yaw = 0;
  for (const auto & sample : valid_samples) {
    double dx = sample.workshop_pose.x_m - res.workshop_to_localization.x_m;
    double dy = sample.workshop_pose.y_m - res.workshop_to_localization.y_m;
    double dyaw = sample.workshop_pose.yaw_rad - res.workshop_to_localization.yaw_rad;
    var_pos += (dx * dx + dy * dy);
    var_yaw += (dyaw * dyaw);
  }

  res.residual_error_m = std::sqrt(var_pos * count_inv);
  res.residual_error_rad = std::sqrt(var_yaw * count_inv);
  res.position_repeatability_m = res.residual_error_m;
  res.yaw_repeatability_rad = res.residual_error_rad;

  res.validated_as_truth_source = (res.residual_error_m < 0.05); // 示例验收标准：5cm

  return true;
}

}  // namespace external_localization_service
