#pragma once

#include <memory>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "calibration_common_interfaces/msg/file_reference.hpp"
#include "calibration_chassis_interfaces/action/execute_motion_primitive.hpp"
#include "calibration_chassis_interfaces/srv/get_chassis_readiness.hpp"
#include "calibration_vehicle_profile_interfaces/srv/get_vehicle_profile.hpp"

#include "chassis_calibration_service/chassis_calibration_algorithm_template.hpp"

namespace chassis_calibration_service
{

// 底盘标定服务节点：
// 1) 对外提供 readiness 接口；
// 2) 对外提供动作原语 action；
// 3) 后续把真实底盘算法接进来时，算法入口也放在这个节点内部。
class ChassisCalibrationServiceNode : public rclcpp::Node
{
public:
  explicit ChassisCalibrationServiceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

private:
  using ReadinessSrv = calibration_chassis_interfaces::srv::GetChassisReadiness;
  using GetVehicleProfileSrv = calibration_vehicle_profile_interfaces::srv::GetVehicleProfile;
  using ExecuteTask = calibration_chassis_interfaces::action::ExecuteMotionPrimitive;
  using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle<ExecuteTask>;

  struct DataInputConfig
  {
    int64_t data_window_start_timestamp_us{0};
    int64_t data_window_end_timestamp_us{0};
    std::vector<calibration_common_interfaces::msg::FileReference> chassis_motion_data_files;
    std::vector<calibration_common_interfaces::msg::FileReference> actuator_command_files;
    std::vector<calibration_common_interfaces::msg::FileReference> truth_trajectory_files;
  };

  // readiness 服务：让编排器确认当前底盘是否具备执行条件。
  rclcpp::Service<ReadinessSrv>::SharedPtr readiness_service_;
  // 车辆画像客户端：从 vehicle_profile_manager 取当前车辆的底盘类型。
  rclcpp::Client<GetVehicleProfileSrv>::SharedPtr get_vehicle_profile_client_;
  // action server：接收底盘动作原语任务，并在后台线程里执行。
  rclcpp_action::Server<ExecuteTask>::SharedPtr execute_task_action_server_;
  DataInputConfig data_input_config_;

  DataInputConfig load_data_input_config();

  // readiness 回调：把底盘运行状态翻译成统一的 readiness 响应。
  void handle_readiness(
    const std::shared_ptr<ReadinessSrv::Request> request,
    std::shared_ptr<ReadinessSrv::Response> response);
  // goal 回调：在真正执行之前先做一次最小合法性检查。
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const ExecuteTask::Goal> goal);
  // cancel 回调：当前最小版本直接接受取消，真正的中止逻辑后面再接。
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
  // goal 接收后回调：把长任务放到后台线程，避免卡住 ROS 回调线程。
  void handle_accepted(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
  // 从车辆画像服务里按 vehicle_id 查询底盘类型。
  bool fetch_chassis_type(
    const std::string & vehicle_id,
    ChassisCalibrationAlgorithmTemplate::ChassisType & chassis_type,
    std::string & failure_reason);
  // 后台执行入口：这里不直接写算法细节，而是调用底盘算法模板。
  // 以后真实标定算法只需要替换模板内部实现，节点层仍然保持稳定。
  void execute_goal(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);

  // 底盘标定算法模板：负责承接输入校验、底盘类型分发与结果填充。
  ChassisCalibrationAlgorithmTemplate algorithm_;
};

}  // namespace chassis_calibration_service
