#pragma once

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

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

#include "control_calibration_service/control_calibration_algorithm_template.hpp"

#include "calibration_common_interfaces/msg/file_reference.hpp"
#include "calibration_control_interfaces/action/execute_controller_evaluation.hpp"
#include "calibration_control_interfaces/srv/get_control_readiness.hpp"

namespace control_calibration_service
{

// 控制标定服务节点：
// 1. 负责暴露 readiness service 和 execute action，作为控制专项对外的 ROS 接口入口。
// 2. 负责把 ExecuteControllerEvaluation::Goal 组装成 ControlCalibrationInput。
// 3. 负责在进入算法前，先做最小任务合法性检查，并推导默认控制算法类型。
// 4. 负责调用控制算法门面，再把输出统一回填到 Action Result。
// 5. 这里不直接写具体控制算法，避免算法工程师在修改 PID/MPC/LQR/PP 时碰 ROS 通信代码。
class ControlCalibrationServiceNode : public rclcpp::Node
{
public:
  explicit ControlCalibrationServiceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

private:
  using ReadinessSrv = calibration_control_interfaces::srv::GetControlReadiness;
  using ExecuteTask = calibration_control_interfaces::action::ExecuteControllerEvaluation;
  using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle<ExecuteTask>;
  using ControllerAlgorithmType = calibration_vehicle_profile_interfaces::msg::ControllerAlgorithmType;

  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> control_evaluation_data_files;
    std::vector<calibration_common_interfaces::msg::FileReference> reference_signal_files;
    std::vector<calibration_common_interfaces::msg::FileReference> chassis_response_files;
    std::vector<calibration_common_interfaces::msg::FileReference> truth_trajectory_files;
  };

  rclcpp::Service<ReadinessSrv>::SharedPtr readiness_service_;
  rclcpp_action::Server<ExecuteTask>::SharedPtr execute_task_action_server_;
  ControlCalibrationAlgorithmTemplate algorithm_;
  DataInputConfig data_input_config_;

  DataInputConfig load_data_input_config();

  void handle_readiness(
    const std::shared_ptr<ReadinessSrv::Request> request,
    std::shared_ptr<ReadinessSrv::Response> response);
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const ExecuteTask::Goal> goal);
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
  void handle_accepted(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
  void execute_goal(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);

  ControllerAlgorithmType resolve_controller_algorithm(const ExecuteTask::Goal & goal) const;
};

}  // namespace control_calibration_service
