#pragma once

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

#include "calibration_chassis_interfaces/action/execute_motion_primitive.hpp"
#include "calibration_chassis_interfaces/msg/applied_chassis_calibration_parameters_response.hpp"
#include "calibration_chassis_interfaces/msg/chassis_capability_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/msg/applied_sensor_calibration_parameters_response.hpp"
#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_telemetry.hpp"
#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp"
#include "calibration_vehicle_profile_interfaces/msg/chassis_type.hpp"
#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp"

namespace chassis_calibration_service
{

using ExecuteTask = calibration_chassis_interfaces::action::ExecuteMotionPrimitive;
using ChassisType = calibration_vehicle_profile_interfaces::msg::ChassisType;

struct ChassisCalibrationInput
{
  // ===== 会话与静态画像 =====
  // 当前车间会话：可用于读取本轮阶段计划、人工审批状态、会话级参数等。
  calibration_workshop_orchestration_interfaces::msg::WorkshopSession workshop_session;

  // 车辆画像快照：包含底盘、传感器、控制器、URDF、启用阶段、能力清单等静态信息。
  calibration_vehicle_profile_interfaces::msg::VehicleProfile vehicle_profile;

  // 标定任务请求：包含 request_id、任务目的、原语类型、原语参数、超时、是否执行完毕后刹停等。
  ExecuteTask::Goal request;

  // 当前底盘类型：用于把任务路由到对应的算法模板。
  ChassisType chassis_type;

  // ===== 底盘域反馈 =====
  // 底盘能力：先判断当前车到底支持哪些动作原语，再决定算法是否可以进入执行。
  calibration_chassis_interfaces::msg::ChassisCapabilityResponse chassis_capability;

  // 底盘就绪状态：驱动是否在线、急停是否释放、是否允许移动、是否可回传遥测。
  calibration_chassis_interfaces::msg::ChassisReadinessResponse chassis_readiness;

  // 当前工作模式：标定准备、直接执行、验证等，算法可据此决定工作流。
  calibration_chassis_interfaces::msg::ChassisWorkMode chassis_work_mode;

  // 当前已生效的底盘参数：算法通常要读取现有参数，和本轮结果做对比或增量更新。
  calibration_chassis_interfaces::msg::AppliedChassisCalibrationParametersResponse applied_chassis_parameters;

  // 最新一帧底盘遥测：轮速、里程计、舵角、IMU、驱动错误码等。
  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_sensor_interfaces::msg::SensorReadinessResponse sensor_readiness;

  // 当前已生效传感器参数：相机内参、IMU 内参、外参、手眼参数等。
  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;

  // ===== 外部定位 / 真值源反馈 =====
  // 外部定位就绪状态：是否适合作为参考源、时间同步是否达标、质量摘要等。
  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;

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

  // 底盘原始运动数据文件：例如 wheel/steering/odom/driver_state CSV、JSON 或 rosbag。
  std::vector<calibration_common_interfaces::msg::FileReference> chassis_motion_data_files;

  // 控制命令与执行器反馈文件：例如 cmd_vel、Ackermann 命令、驱动器反馈、制动状态。
  std::vector<calibration_common_interfaces::msg::FileReference> actuator_command_files;

  // 外部真值轨迹文件：例如 external pose 轨迹、地面真值、测量轨迹，用于和底盘里程计对齐。
  std::vector<calibration_common_interfaces::msg::FileReference> truth_trajectory_files;

  // 说明：如果后续算法还需要更多原始反馈，可以继续在这里扩展。
  // 常见补充项包括：GNSS、SLAM 里程计、地面真值、控制器内部状态、制动状态、故障码、机械臂反馈等。
};

struct ChassisCalibrationOutput
{
  // 标定算法的输出：成功/失败、错误码、验证摘要、建议参数、产物引用等都写到这里。
  ExecuteTask::Result response;
};

class ChassisCalibrationAlgorithm
{
public:
  virtual ~ChassisCalibrationAlgorithm() = default;
  virtual bool run(
    const ChassisCalibrationInput & input,
    ChassisCalibrationOutput & output,
    std::string & failure_reason) const = 0;
};

class AckermannChassisAlgorithm final : public ChassisCalibrationAlgorithm
{
public:
  bool run(
    const ChassisCalibrationInput & input,
    ChassisCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class DifferentialChassisAlgorithm final : public ChassisCalibrationAlgorithm
{
public:
  bool run(
    const ChassisCalibrationInput & input,
    ChassisCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class SingleSteerWheelChassisAlgorithm final : public ChassisCalibrationAlgorithm
{
public:
  bool run(
    const ChassisCalibrationInput & input,
    ChassisCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class MultiSteerWheelChassisAlgorithm final : public ChassisCalibrationAlgorithm
{
public:
  bool run(
    const ChassisCalibrationInput & input,
    ChassisCalibrationOutput & output,
    std::string & failure_reason) const override;
};

}  // namespace chassis_calibration_service
