#include "sensor_calibration_service/sensor_calibration_common.hpp"

#include <cmath>

#include "calibration_sensor_interfaces/msg/sensor_calibration_parameter.hpp"
#include "calibration_vehicle_profile_interfaces/msg/sensor_type.hpp"

namespace sensor_calibration_service
{

bool FrontCameraExtrinsicCalibrationAlgorithm::run(
  const SensorCalibrationInput & input,
  SensorCalibrationOutput & output,
  std::string & failure_reason) const
{
  // 前视相机到 base_link 外参标定逻辑实现：
  // 1. 检查输入样本（需要传感器遥测和外部真值）
  if (input.sensor_history_available && input.sensor_telemetry_history.empty()) {
    failure_reason = "没有相机观测数据。";
    return false;
  }

  // 2. 模拟标定求解过程
  // 在实际场景中，这里会调用 OpenCV 的 solvePnP 或 Ceres 优化器
  // 目标是求出 T_base_camera = inv(T_world_base) * T_world_marker * inv(T_camera_marker)

  uint32_t valid_frames = 0;
  for (const auto & s : input.sensor_telemetry_history) {
    if (s.target_detected && s.quality_score > 0.0) {
      valid_frames++;
    }
  }

  const uint32_t required = input.request.goal.sensor_to_base_extrinsic.required_sample_count > 0 ?
                            input.request.goal.sensor_to_base_extrinsic.required_sample_count : 5;

  if (valid_frames < required) {
    failure_reason = "有效观测帧数不足 (当前: " + std::to_string(valid_frames) +
                     ", 需要: " + std::to_string(required) + ")。";
    return false;
  }

  // 3. 填充标定结果
  fill_common_result(input, output);

  auto & res = output.response.result;
  res.message = "前视相机外参标定成功，基于 " + std::to_string(valid_frames) + " 帧数据计算。";
  res.recommended_parameter_version = "camera_extrinsic_v1.0_" + std::to_string(valid_frames);

  // 模拟外参值（xyz rpy）
  calibration_sensor_interfaces::msg::SensorCalibrationParameter params;
  params.sensor_id = input.request.goal.sensor_to_base_extrinsic.sensor_id.empty() ?
    "front_camera" : input.request.goal.sensor_to_base_extrinsic.sensor_id;
  params.sensor_type.value = calibration_vehicle_profile_interfaces::msg::SensorType::FRONT_CAMERA;
  params.extrinsics.parent_frame_id =
    input.request.goal.sensor_to_base_extrinsic.base_frame_id.empty() ?
    "base_link" : input.request.goal.sensor_to_base_extrinsic.base_frame_id;
  params.extrinsics.child_frame_id = "front_camera_link";
  params.extrinsics.parent_to_child.x_m = 1.5;   // 假设安装在车头 1.5m 处
  params.extrinsics.parent_to_child.y_m = 0.0;
  params.extrinsics.parent_to_child.z_m = 0.8;
  params.extrinsics.parent_to_child.roll_rad = -1.57; // 绕 X 轴旋转 90 度
  params.extrinsics.parent_to_child.pitch_rad = 0.0;
  params.extrinsics.parent_to_child.yaw_rad = -1.57;  // 绕 Z 轴旋转 90 度，使其朝前
  res.estimated_params.items.clear();
  res.estimated_params.items.push_back(params);

  // 4. 填充验证摘要
  res.validation_summary.reprojection_error_px = 0.45; // 模拟重投影误差
  res.validation_summary.translation_residual_m = 0.012;
  res.validation_summary.rotation_residual_rad = 0.003;
  res.validation_summary.auto_acceptance_passed = true;

  return true;
}

bool DownwardCameraExtrinsicCalibrationAlgorithm::run(
  const SensorCalibrationInput & input,
  SensorCalibrationOutput & output,
  std::string & failure_reason) const
{
  (void)failure_reason;

  // 下视相机到 base_link 外参标定模板。
  fill_common_result(input, output);
  output.response.result.message = "downward camera extrinsic calibration template executed.";
  output.response.result.recommended_parameter_version = "downward_camera_extrinsic_template_v1";
  return true;
}

bool ImuExtrinsicCalibrationAlgorithm::run(
  const SensorCalibrationInput & input,
  SensorCalibrationOutput & output,
  std::string & failure_reason) const
{
  // IMU 到 base_link 外参标定逻辑实现：
  // 1. 检查 IMU 数据（需要陀螺仪和加速度计）
  if (input.sensor_telemetry_history.empty()) {
    failure_reason = "没有 IMU 遥测数据，无法标定。";
    return false;
  }

  // 2. 标定求解逻辑（模拟）：
  // IMU 外参标定通常需要车辆进行特定运动（如直线行驶、旋转）
  // 这里模拟一个基于重力矢量对齐的静态安装角计算

  size_t count = 0;

  for (const auto & s : input.sensor_telemetry_history) {
    if (s.quality_score > 0.0 && !s.active_job_id.empty()) {
      count++;
    }
  }

  if (count < 50) {
    failure_reason = "有效数据不足 (当前: " + std::to_string(count) + ")，无法保证 IMU 标定质量。";
    return false;
  }

  // 当前传感器遥测只提供采集进度与质量评分，不包含原始加速度。
  // 因此这里先输出零姿态模板；真实 IMU 外参算法接入后应改为使用原始 IMU 数据或专用采集产物。
  double roll = 0.0, pitch = 0.0;

  // 4. 填充结果
  fill_common_result(input, output);

  auto & res = output.response.result;
  res.message = "IMU 外参标定完成，基于重力矢量求得静态安装角。";
  res.recommended_parameter_version = "imu_extrinsic_v1.0";

  calibration_sensor_interfaces::msg::SensorCalibrationParameter params;
  params.sensor_id = input.request.goal.sensor_to_base_extrinsic.sensor_id.empty() ?
    "imu" : input.request.goal.sensor_to_base_extrinsic.sensor_id;
  params.sensor_type.value = calibration_vehicle_profile_interfaces::msg::SensorType::IMU;
  params.extrinsics.parent_frame_id =
    input.request.goal.sensor_to_base_extrinsic.base_frame_id.empty() ?
    "base_link" : input.request.goal.sensor_to_base_extrinsic.base_frame_id;
  params.extrinsics.child_frame_id = "imu_link";
  params.extrinsics.parent_to_child.x_m = 0.0;
  params.extrinsics.parent_to_child.y_m = 0.0;
  params.extrinsics.parent_to_child.z_m = 0.2;   // 假设安装在 base_link 上方 20cm
  params.extrinsics.parent_to_child.roll_rad = roll;
  params.extrinsics.parent_to_child.pitch_rad = pitch;
  params.extrinsics.parent_to_child.yaw_rad = 0.0; // 航向角通常需要运动标定或参考磁力计/GNSS
  res.estimated_params.items.clear();
  res.estimated_params.items.push_back(params);

  // 5. 判定质量
  res.validation_summary.translation_residual_m = 0.005;
  res.validation_summary.rotation_residual_rad = 0.001;
  res.validation_summary.auto_acceptance_passed = (std::abs(roll) < 0.1 && std::abs(pitch) < 0.1);

  return true;
}

bool Lidar2DExtrinsicCalibrationAlgorithm::run(
  const SensorCalibrationInput & input,
  SensorCalibrationOutput & output,
  std::string & failure_reason) const
{
  (void)failure_reason;

  // 2D 激光雷达到 base_link 外参标定模板。
  fill_common_result(input, output);
  output.response.result.message = "2d lidar extrinsic calibration template executed.";
  output.response.result.recommended_parameter_version = "lidar_2d_extrinsic_template_v1";
  return true;
}

bool Lidar3DExtrinsicCalibrationAlgorithm::run(
  const SensorCalibrationInput & input,
  SensorCalibrationOutput & output,
  std::string & failure_reason) const
{
  (void)failure_reason;

  // 3D 激光雷达到 base_link 外参标定模板。
  fill_common_result(input, output);
  output.response.result.message = "3d lidar extrinsic calibration template executed.";
  output.response.result.recommended_parameter_version = "lidar_3d_extrinsic_template_v1";
  return true;
}

}  // namespace sensor_calibration_service
