#pragma once

#include <cstdint>
#include <string>
#include <vector>

#include "calibration_chassis_interfaces/msg/applied_chassis_calibration_parameters_response.hpp"
#include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp"
#include "calibration_chassis_interfaces/msg/chassis_telemetry.hpp"
#include "calibration_chassis_interfaces/msg/chassis_work_mode.hpp"
#include "calibration_common_interfaces/msg/file_reference.hpp"
#include "calibration_control_interfaces/msg/active_controller_parameters_response.hpp"
#include "calibration_control_interfaces/msg/control_readiness_response.hpp"
#include "calibration_control_interfaces/msg/control_telemetry.hpp"
#include "calibration_control_interfaces/msg/control_work_mode.hpp"
#include "calibration_external_localization_interfaces/msg/external_localization_readiness_response.hpp"
#include "calibration_external_localization_interfaces/msg/external_localization_telemetry.hpp"
#include "calibration_sensor_interfaces/action/execute_sensor_calibration_task.hpp"
#include "calibration_sensor_interfaces/msg/applied_sensor_calibration_parameters_response.hpp"
#include "calibration_sensor_interfaces/msg/camera_intrinsic_calibration_task.hpp"
#include "calibration_sensor_interfaces/msg/hand_eye_calibration_task.hpp"
#include "calibration_sensor_interfaces/msg/imu_intrinsic_calibration_task.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_parameter_set.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_task_subtype.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_task_type.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_telemetry.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_work_mode.hpp"
#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp"
#include "calibration_sensor_interfaces/msg/sensor_to_base_extrinsic_calibration_task.hpp"
#include "calibration_vehicle_profile_interfaces/msg/sensor_type.hpp"
#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp"
#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp"

namespace sensor_calibration_service
{

using ExecuteTask = calibration_sensor_interfaces::action::ExecuteSensorCalibrationTask;
using SensorCalibrationTaskSubtype = calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype;
using SensorCalibrationTaskType = calibration_sensor_interfaces::msg::SensorCalibrationTaskType;
using SensorType = calibration_vehicle_profile_interfaces::msg::SensorType;

struct SensorCalibrationInput
{
  // ===== 会话与静态画像 =====
  // 当前车间会话：算法可读取本轮 session 的阶段上下文、人工确认状态、操作者信息等。
  calibration_workshop_orchestration_interfaces::msg::WorkshopSession workshop_session;

  // 车辆画像：包含传感器清单、安装方式、base_link、控制器和启用阶段等静态信息。
  calibration_vehicle_profile_interfaces::msg::VehicleProfile vehicle_profile;

  // 传感器标定任务请求：包含 request_id、任务目的、任务类型、各 payload、来源迭代号。
  ExecuteTask::Goal request;

  // 当前任务类型快照：与 request.goal.selected_task 一致，但单独展开更方便算法工程师阅读。
  SensorCalibrationTaskType task_type;

  // 当前任务子类型：用于区分前视/下视相机、IMU 外参、2D/3D 激光雷达、眼在手内/外等。
  SensorCalibrationTaskSubtype task_subtype;

  // 当前目标传感器 ID：从具体 payload 中提取并展开。
  std::string target_sensor_id;

  // 当前目标传感器类型：上层若已知，可在进入算法前写入；未知时可保持未指定。
  SensorType target_sensor_type;

  // ===== 任务拆解视图 =====
  calibration_sensor_interfaces::msg::CameraIntrinsicCalibrationTask camera_intrinsic_task;
  calibration_sensor_interfaces::msg::IMUIntrinsicCalibrationTask imu_intrinsic_task;
  calibration_sensor_interfaces::msg::SensorToBaseExtrinsicCalibrationTask sensor_to_base_extrinsic_task;
  calibration_sensor_interfaces::msg::HandEyeCalibrationTask hand_eye_task;

  // 常用展开参考量：便于算法直接取值，而不用总回到原始 payload 中解析。
  uint32_t required_image_count{0};
  uint32_t required_static_segment_count{0};
  uint32_t required_motion_segment_count{0};
  uint32_t required_sample_count{0};
  uint32_t required_pose_count{0};
  double reference_timeout_sec{0.0};
  std::string reference_board_id;
  std::string reference_base_frame_id;
  std::string reference_arm_id;

  // ===== 传感器域反馈 =====
  // 传感器服务就绪状态：采集链路、存储链路、遥测链路、车辆安全、机械臂就绪等。
  calibration_sensor_interfaces::msg::SensorReadinessResponse sensor_readiness;

  // 当前传感器工作模式：调参准备、采集、求解、验证等。
  calibration_sensor_interfaces::msg::SensorCalibrationWorkMode sensor_work_mode;

  // 当前已生效的传感器参数：相机内参、IMU 内参、外参、手眼等。
  calibration_sensor_interfaces::msg::AppliedSensorCalibrationParametersResponse applied_sensor_parameters;

  // 当前算法准备写回的候选参数集。
  calibration_sensor_interfaces::msg::SensorCalibrationParameterSet candidate_parameter_set;

  // 最新一帧传感器遥测：当前采样数、目标检测状态、质量评分、当前任务 ID 等。
  calibration_sensor_interfaces::msg::SensorCalibrationTelemetry latest_sensor_telemetry;

  // 历史传感器遥测窗口：用于观测采集进度、质量趋势、稳定性和异常剔除。
  std::vector<calibration_sensor_interfaces::msg::SensorCalibrationTelemetry> sensor_telemetry_history;

  // 采集质量诊断扩展：例如棋盘格检测失败、LiDAR 反光不足、IMU 饱和、图像模糊等。
  std::vector<std::string> capture_quality_diagnostics;

  // 存储与产物诊断扩展：例如落盘失败、目录权限、磁盘空间不足、文件损坏等。
  std::vector<std::string> storage_diagnostics;

  // ===== 底盘域反馈 =====
  calibration_chassis_interfaces::msg::ChassisReadinessResponse chassis_readiness;
  calibration_chassis_interfaces::msg::ChassisWorkMode chassis_work_mode;
  calibration_chassis_interfaces::msg::AppliedChassisCalibrationParametersResponse applied_chassis_parameters;
  calibration_chassis_interfaces::msg::ChassisTelemetry latest_chassis_telemetry;
  std::vector<calibration_chassis_interfaces::msg::ChassisTelemetry> chassis_telemetry_history;

  // ===== 控制域反馈 =====
  calibration_control_interfaces::msg::ControlReadinessResponse control_readiness;
  calibration_control_interfaces::msg::ControlWorkMode control_work_mode;
  calibration_control_interfaces::msg::ActiveControllerParametersResponse active_controller_parameters;
  calibration_control_interfaces::msg::ControlTelemetry latest_control_telemetry;
  std::vector<calibration_control_interfaces::msg::ControlTelemetry> control_telemetry_history;

  // ===== 外部定位 / 真值源反馈 =====
  calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse external_localization_readiness;
  calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry latest_external_localization_telemetry;
  std::vector<calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry> external_localization_telemetry_history;
  std::vector<std::string> truth_source_diagnostics;

  // ===== 时间同步 / 数据窗口有效性 =====
  double estimated_sensor_to_chassis_time_offset_sec{0.0};
  double estimated_sensor_to_control_time_offset_sec{0.0};
  double estimated_sensor_to_truth_time_offset_sec{0.0};
  bool sensor_history_available{false};
  bool chassis_history_available{false};
  bool control_history_available{false};
  bool truth_history_available{false};

  // ===== 算法数据输入窗口 =====
  // 当前采集任务对应的数据时间窗口。0 表示上层尚未提供明确边界。
  int64_t data_window_start_timestamp_us{0};
  int64_t data_window_end_timestamp_us{0};

  // 图像样本文件：适用于前视/下视相机内参、相机外参、手眼标定。
  // 文件可为单帧图片、图片列表、rosbag、mcap 或算法约定的数据集索引。
  std::vector<calibration_common_interfaces::msg::FileReference> image_sample_files;

  // IMU 原始样本文件：适用于 IMU 内参、IMU 外参、运动段对齐。
  std::vector<calibration_common_interfaces::msg::FileReference> imu_sample_files;

  // 3D 点云样本文件：适用于 3D LiDAR 外参或点云配准类算法。
  std::vector<calibration_common_interfaces::msg::FileReference> pointcloud_sample_files;

  // 2D 激光扫描样本文件：适用于 2D LiDAR 外参、角落/平面靶标拟合。
  std::vector<calibration_common_interfaces::msg::FileReference> laser_scan_sample_files;

  // 标定目标检测结果文件：例如棋盘格/ChArUco/AprilTag 角点、PnP 初值、LiDAR 平面/角点提取结果。
  std::vector<calibration_common_interfaces::msg::FileReference> target_detection_files;

  // 机械臂或平台位姿文件：适用于手眼标定、传感器到 base_link 外参联合求解。
  std::vector<calibration_common_interfaces::msg::FileReference> robot_pose_sample_files;

  // 已同步的数据集索引：推荐作为真实算法的首选入口，内部可引用图像、IMU、点云、真值、检测结果。
  std::vector<calibration_common_interfaces::msg::FileReference> synchronized_dataset_files;

  // 说明：如果后续某类传感器算法还需要更细原始数据入口，可继续在这里补充。
  // 常见补充项包括：图像帧缓存、IMU 原始包、点云帧列表、机械臂位姿序列、标定板检测结果等。
};

struct SensorCalibrationOutput
{
  // 传感器标定算法统一输出：成功/失败、错误码、验证摘要、估计参数、产物引用都写到这里。
  ExecuteTask::Result response;
};

class SensorCalibrationAlgorithm
{
public:
  virtual ~SensorCalibrationAlgorithm() = default;

  virtual bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const = 0;
};

class FrontCameraIntrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class DownwardCameraIntrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class ImuIntrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class FrontCameraExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class DownwardCameraExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class ImuExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class Lidar2DExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class Lidar3DExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class EyeInHandCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class EyeToHandCalibrationAlgorithm final : public SensorCalibrationAlgorithm
{
public:
  bool run(
    const SensorCalibrationInput & input,
    SensorCalibrationOutput & output,
    std::string & failure_reason) const override;
};

}  // namespace sensor_calibration_service
