#pragma once

#include <memory>
#include <string>

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

#include "calibration_control_interfaces/action/execute_controller_evaluation.hpp"
#include "calibration_control_interfaces/srv/get_control_readiness.hpp"

#include "vehicle_agent_gateway/tcp_client.hpp"

namespace vehicle_agent_gateway
{

// 运控网关节点：
// 1. 暴露与 control_calibration_service 完全相同的 ROS2 接口，orchestrator 零感知。
// 2. 收到请求后通过 TcpClient 发帧到 Windows 车端，等待响应后回填 ROS2 结果。
// 3. 当前 payload 为空占位；后续接入 protobuf 序列化时只需修改序列化/反序列化部分。
class ControlBridgeNode : public rclcpp::Node
{
public:
  explicit ControlBridgeNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

private:
  using ReadinessSrv = calibration_control_interfaces::srv::GetControlReadiness;
  using ExecuteTask  = calibration_control_interfaces::action::ExecuteControllerEvaluation;
  using GoalHandle   = rclcpp_action::ServerGoalHandle<ExecuteTask>;

  rclcpp::Service<ReadinessSrv>::SharedPtr readiness_service_;
  rclcpp_action::Server<ExecuteTask>::SharedPtr execute_task_server_;

  std::string control_host_;
  uint16_t control_port_;
  int timeout_ms_;

  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<GoalHandle> goal_handle);

  void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle);

  void execute_goal(const std::shared_ptr<GoalHandle> goal_handle);
};

}  // namespace vehicle_agent_gateway
