#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/action/execute_external_localization_task.hpp"
#include "calibration_external_localization_interfaces/msg/external_localization_calibration_result.hpp"
#include "calibration_external_localization_interfaces/msg/external_localization_capability_response.hpp"
#include "calibration_external_localization_interfaces/msg/external_localization_readiness_response.hpp"
#include "calibration_external_localization_interfaces/msg/external_localization_task_payload_type.hpp"
#include "calibration_external_localization_interfaces/msg/external_localization_telemetry.hpp"
#include "calibration_external_localization_interfaces/msg/marker_alignment_task.hpp"
#include "calibration_external_localization_interfaces/msg/reference_pose_collection_task.hpp"
#include "calibration_external_localization_interfaces/msg/truth_source_validation_task.hpp"
#include "calibration_sensor_interfaces/msg/applied_sensor_calibration_parameters_response.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_telemetry.hpp"
#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_work_mode.hpp"
#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp"
#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp"

namespace external_localization_service
{

using ExecuteTask = calibration_external_localization_interfaces::action::ExecuteExternalLocalizationTask;
using ExternalTaskType = calibration_external_localization_interfaces::msg::ExternalLocalizationTaskPayloadType;

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

  // 车辆画像：可用于读取车辆基础信息、传感器安装信息、底盘/控制/传感器使能状态等静态配置。
  calibration_vehicle_profile_interfaces::msg::VehicleProfile vehicle_profile;

  // 外部定位任务请求：包含 request_id、任务目的、任务类型、各任务 payload、来源迭代号、外部定位源 ID、工位 ID。
  ExecuteTask::Goal request;

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

  // ===== 任务拆解视图 =====
  // 参考位姿采集任务：适用于静止重复性分析、参考位姿基线建立、初始化基准采样等。
  calibration_external_localization_interfaces::msg::ReferencePoseCollectionTask reference_pose_collection_task;

  // 标靶对齐任务：适用于建立 workshop frame 与 localization frame 之间的坐标关系。
  calibration_external_localization_interfaces::msg::MarkerAlignmentTask marker_alignment_task;

  // 真值源验证任务：适用于验证时间同步、稳定性、覆盖范围、丢失率、可作为真值源的可行性。
  calibration_external_localization_interfaces::msg::TruthSourceValidationTask truth_source_validation_task;

  // 常用参考量展开：便于算法工程师直接访问，不需要每次都回原始 payload 中解析。
  uint32_t required_reference_pose_sample_count{0};
  uint32_t required_marker_observation_count{0};
  uint32_t required_static_sample_count{0};
  uint32_t required_dynamic_sample_count{0};
  bool require_vehicle_static{false};
  bool require_short_motion_segment{false};
  double reference_timeout_sec{0.0};
  double min_sample_interval_sec{0.0};
  double accepted_max_position_stddev_m{0.0};
  double accepted_max_yaw_stddev_rad{0.0};
  double accepted_max_tracking_loss_ratio{0.0};
  double accepted_max_time_sync_offset_ms{0.0};
  std::string reference_board_id;
  std::string localization_source_id;
  std::string workcell_zone_id;

  // ===== 外部定位域反馈 =====
  // 外部定位能力：算法可据此知道当前源支持哪些任务、是否支持持久化、是否支持真值验证等。
  calibration_external_localization_interfaces::msg::ExternalLocalizationCapabilityResponse external_localization_capability;

  // 外部定位就绪状态：服务本身是否就绪、是否允许进入真值验证、当前验证摘要如何。
  calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse external_localization_readiness;

  // 当前算法准备写回的候选结果。
  // 推荐做法是：算法先在这个结构里组装结果，再统一回填到 output.response.result.result。
  calibration_external_localization_interfaces::msg::ExternalLocalizationCalibrationResult candidate_calibration_result;

  // 最新一帧外部定位遥测：当前观测位姿、标准差、跟踪丢失率、时间同步偏差、质量分数等。
  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;

  // 标靶检测与对齐诊断扩展：例如标靶检测失败、有效角点不足、位姿解算不稳定等。
  std::vector<std::string> marker_alignment_diagnostics;

  // 数据采集与落盘诊断扩展：例如原始位姿采集窗口不足、采样频率不满足、落盘失败等。
  std::vector<std::string> acquisition_diagnostics;

  // ===== 算法数据输入窗口 =====
  // 当前算法可使用的数据时间窗口。0 表示上层尚未提供明确边界。
  int64_t data_window_start_timestamp_us{0};
  int64_t data_window_end_timestamp_us{0};

  // 外部定位原始观测文件：例如 CSV/JSON/rosbag，内容应至少包含时间戳、位姿、质量、协方差或标准差。
  std::vector<calibration_common_interfaces::msg::FileReference> external_observation_files;

  // 标靶观测或检测结果文件：例如 marker/AprilTag/反光柱观测日志、角点检测结果、对齐中间结果。
  std::vector<calibration_common_interfaces::msg::FileReference> marker_observation_files;

  // 时间同步诊断文件：例如 PTP/NTP 偏差统计、车端与车间主机时钟对齐日志。
  std::vector<calibration_common_interfaces::msg::FileReference> time_sync_diagnostic_files;

  // ===== 底盘域反馈 =====
  // 外部定位验证往往需要结合车辆真实运动状态，因此保留底盘域反馈接口。
  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;

  // ===== 传感器域反馈 =====
  // 外部定位作为真值源时，也常需要结合相机 / IMU / LiDAR 的采样质量判断观测是否可信。
  calibration_sensor_interfaces::msg::SensorReadinessResponse sensor_readiness;
  calibration_sensor_interfaces::msg::SensorCalibrationWorkMode sensor_work_mode;
  calibration_sensor_interfaces::msg::AppliedSensorCalibrationParametersResponse applied_sensor_parameters;
  calibration_sensor_interfaces::msg::SensorCalibrationTelemetry latest_sensor_telemetry;
  std::vector<calibration_sensor_interfaces::msg::SensorCalibrationTelemetry> sensor_telemetry_history;

  // 传感器质量诊断扩展：例如相机丢帧、IMU 饱和、LiDAR 数据稀疏、时间戳抖动等。
  std::vector<std::string> sensor_quality_diagnostics;

  // ===== 时间同步 / 数据窗口有效性 =====
  // 各域之间的估计时间偏移：后续如需跨域误差对齐，可由上层在进入算法前完成填充。
  double estimated_external_to_chassis_time_offset_sec{0.0};
  double estimated_external_to_control_time_offset_sec{0.0};
  double estimated_external_to_sensor_time_offset_sec{0.0};

  // 历史数据窗口是否可用：便于算法先做前置判定，再决定是否执行拟合或验证。
  bool external_history_available{false};
  bool chassis_history_available{false};
  bool control_history_available{false};
  bool sensor_history_available{false};

  // 说明：如果后续外部定位算法还需要更细的数据入口，可继续在这里补充。
  // 常见补充项包括：原始观测包、标靶检测结果序列、时间同步统计、点云/图像辅助特征、参考轨迹片段等。
};

struct ExternalLocalizationOutput
{
  // 外部定位算法统一输出：成功/失败、错误码、验证摘要、结果内容、产物引用都写到这里。
  ExecuteTask::Result response;
};

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

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

class ReferencePoseCollectionAlgorithm final : public ExternalLocalizationAlgorithm
{
public:
  bool run(
    const ExternalLocalizationInput & input,
    ExternalLocalizationOutput & output,
    std::string & failure_reason) const override;
};

class MarkerAlignmentAlgorithm final : public ExternalLocalizationAlgorithm
{
public:
  bool run(
    const ExternalLocalizationInput & input,
    ExternalLocalizationOutput & output,
    std::string & failure_reason) const override;
};

class TruthSourceValidationAlgorithm final : public ExternalLocalizationAlgorithm
{
public:
  bool run(
    const ExternalLocalizationInput & input,
    ExternalLocalizationOutput & output,
    std::string & failure_reason) const override;
};

}  // namespace external_localization_service
