#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/action/execute_controller_evaluation.hpp"
#include "calibration_control_interfaces/msg/acceleration_deceleration_task.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_control_interfaces/msg/controller_evaluation_task_type.hpp"
#include "calibration_control_interfaces/msg/controller_parameter_set.hpp"
#include "calibration_control_interfaces/msg/stop_accuracy_task.hpp"
#include "calibration_control_interfaces/msg/trajectory_point.hpp"
#include "calibration_control_interfaces/msg/trajectory_tracking_task.hpp"
#include "calibration_control_interfaces/msg/velocity_step_task.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_calibration_telemetry.hpp"
#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp"
#include "calibration_vehicle_profile_interfaces/msg/controller_algorithm_type.hpp"
#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp"
#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp"

namespace control_calibration_service
{

using ExecuteTask = calibration_control_interfaces::action::ExecuteControllerEvaluation;
using ControllerAlgorithmType = calibration_vehicle_profile_interfaces::msg::ControllerAlgorithmType;

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

  // 车辆画像：包含控制器画像、控制轴支持算法、默认算法、车辆基础信息等。
  calibration_vehicle_profile_interfaces::msg::VehicleProfile vehicle_profile;

  // 控制评估任务请求：包含 request_id、任务目的、任务类型、各任务 payload、来源迭代号。
  ExecuteTask::Goal request;

  // 本轮需要进入的控制算法类型：用于把任务分发到 PID / MPC / LQR / Pure Pursuit 等专属模板。
  ControllerAlgorithmType controller_algorithm;

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

  // ===== 任务拆解视图 =====
  // 轨迹跟踪任务的原始 payload。适用于 MPC、LQR、Pure Pursuit 等需要整条参考轨迹的算法。
  calibration_control_interfaces::msg::TrajectoryTrackingTask trajectory_tracking_task;

  // 速度阶跃任务的原始 payload。适用于纵向 PID / MPC 的动态响应评估。
  calibration_control_interfaces::msg::VelocityStepTask velocity_step_task;

  // 加减速任务的原始 payload。适用于纵向平顺性、加速度响应和 jerk 约束分析。
  calibration_control_interfaces::msg::AccelerationDecelerationTask acceleration_deceleration_task;

  // 停车精度任务的原始 payload。适用于停车误差与末端姿态控制算法评估。
  calibration_control_interfaces::msg::StopAccuracyTask stop_accuracy_task;

  // 将轨迹任务里的 path 单独拆出来，方便算法直接访问参考轨迹点序列。
  std::vector<calibration_control_interfaces::msg::TrajectoryPoint> reference_trajectory;

  // 将常用参考量单独展开，避免算法工程师总是回到原始 payload 中取值。
  double reference_target_velocity_ms{0.0};
  double reference_start_velocity_ms{0.0};
  double reference_target_accel_ms2{0.0};
  double reference_hold_time_sec{0.0};
  double reference_timeout_sec{0.0};
  double reference_stop_x_m{0.0};
  double reference_stop_y_m{0.0};
  double reference_stop_yaw_rad{0.0};
  bool reference_stop_at_end{false};

  // ===== 控制域反馈 =====
  // 控制服务就绪状态：轨迹执行器、反馈链路、输出链路、急停、安全移动等是否满足执行条件。
  calibration_control_interfaces::msg::ControlReadinessResponse control_readiness;

  // 当前控制工作模式：调参准备、参数注入、评估、验证等。
  calibration_control_interfaces::msg::ControlWorkMode control_work_mode;

  // 当前已生效的控制参数：横向/纵向参数集合，是本轮调参的基线输入。
  calibration_control_interfaces::msg::ActiveControllerParametersResponse active_controller_parameters;

  // 当前算法准备写回的候选参数集。
  // 推荐做法是：算法先在这个结构里组装新参数，再统一回填到 output.response.result.estimated_parameter_set。
  calibration_control_interfaces::msg::ControllerParameterSet candidate_parameter_set;

  // 最新一帧控制遥测：横向误差、航向误差、速度误差、控制输出、饱和状态、当前算法版本等。
  calibration_control_interfaces::msg::ControlTelemetry latest_control_telemetry;

  // 历史控制遥测窗口：用于误差统计、响应曲线拟合、超调/收敛时间分析、稳定性分析。
  std::vector<calibration_control_interfaces::msg::ControlTelemetry> control_telemetry_history;

  // 控制内部诊断扩展：为后续接入真实控制器内部状态预留。
  // 例如：积分项、观测器状态、预测窗口代价、约束激活信息、状态机状态等。
  std::vector<std::string> control_internal_diagnostics;

  // 执行器饱和原因扩展：用于区分是转向打满、油门受限、制动限幅还是安全逻辑介入。
  std::vector<std::string> actuator_saturation_reasons;

  // ===== 底盘域反馈 =====
  // 底盘就绪状态：控制算法通常需要确认底盘驱动链路是否在线、急停是否释放、车辆是否可移动。
  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_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;

  // 传感器质量诊断扩展：例如丢帧、视觉目标丢失、IMU 饱和、时间戳抖动等。
  std::vector<std::string> sensor_quality_diagnostics;

  // ===== 外部定位 / 真值源反馈 =====
  // 外部定位就绪状态：用于判断真值源是否可以参与控制评估与自动验收。
  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_control_to_chassis_time_offset_sec{0.0};
  double estimated_control_to_sensor_time_offset_sec{0.0};
  double estimated_control_to_truth_time_offset_sec{0.0};

  // 数据窗口有效性标记：上层可在进入算法前先完成预检，再把不满足项写到这里。
  bool control_history_available{false};
  bool chassis_history_available{false};
  bool sensor_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};

  // 控制评估原始数据文件：例如控制误差、控制输出、饱和状态、控制器内部状态 CSV/JSON/rosbag。
  std::vector<calibration_common_interfaces::msg::FileReference> control_evaluation_data_files;

  // 参考轨迹或参考速度曲线文件：当 reference_trajectory 不足以表达完整任务时使用。
  std::vector<calibration_common_interfaces::msg::FileReference> reference_signal_files;

  // 底盘响应文件：例如底盘里程计、轮速、舵角、执行器状态，用于分析车辆实际响应。
  std::vector<calibration_common_interfaces::msg::FileReference> chassis_response_files;

  // 外部真值轨迹文件：例如 external pose 轨迹、地面真值、定位质量日志。
  std::vector<calibration_common_interfaces::msg::FileReference> truth_trajectory_files;

  // 说明：如果后续控制算法还需要更细反馈，可继续在这里补充。
  // 常见补充项包括：规划轨迹、参考速度曲线、控制器内部状态、执行器饱和原因、故障码、时间同步诊断信息等。
}
;
struct ControlCalibrationOutput
{
  // 控制评估算法的统一输出：成功/失败、错误码、验证摘要、建议参数、产物引用都写到这里。
  ExecuteTask::Result response;
};

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

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

class PidControlCalibrationAlgorithm final : public ControlCalibrationAlgorithm
{
public:
  bool run(
    const ControlCalibrationInput & input,
    ControlCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class MpcControlCalibrationAlgorithm final : public ControlCalibrationAlgorithm
{
public:
  bool run(
    const ControlCalibrationInput & input,
    ControlCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class LqrControlCalibrationAlgorithm final : public ControlCalibrationAlgorithm
{
public:
  bool run(
    const ControlCalibrationInput & input,
    ControlCalibrationOutput & output,
    std::string & failure_reason) const override;
};

class PurePursuitControlCalibrationAlgorithm final : public ControlCalibrationAlgorithm
{
public:
  bool run(
    const ControlCalibrationInput & input,
    ControlCalibrationOutput & output,
    std::string & failure_reason) const override;
};

}  // namespace control_calibration_service
