#include "vehicle_agent_gateway/control_bridge_node.hpp"

#include <thread>

#include "calibration_common_interfaces/msg/error_code.hpp"
#include "calibration_common_interfaces/msg/job_state.hpp"
#include "calibration_common_interfaces/msg/job_status.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "vehicle_agent_gateway/gateway_codec.hpp"

namespace vehicle_agent_gateway
{

using calibration_common_interfaces::msg::ErrorCode;
using calibration_common_interfaces::msg::JobState;

ControlBridgeNode::ControlBridgeNode(const rclcpp::NodeOptions & options)
: Node("control_bridge_node", options)
{
  // 节点参数：Windows 车端统一 WiFi6 gateway IP 与端口，方便通过 launch 文件或命令行覆盖。
  control_host_ = declare_parameter<std::string>("control_host", "192.168.1.100");
  control_port_ = static_cast<uint16_t>(declare_parameter<int>("control_port", 9000));
  timeout_ms_   = declare_parameter<int>("control_timeout_ms", 5000);

  readiness_service_ = create_service<ReadinessSrv>(
    "/control/get_readiness",
    std::bind(
      &ControlBridgeNode::handle_readiness, this,
      std::placeholders::_1, std::placeholders::_2));

  execute_task_server_ = rclcpp_action::create_server<ExecuteTask>(
    this,
    "/control/execute_controller_evaluation",
    std::bind(&ControlBridgeNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
    std::bind(&ControlBridgeNode::handle_cancel, this, std::placeholders::_1),
    std::bind(&ControlBridgeNode::handle_accepted, this, std::placeholders::_1));

  RCLCPP_INFO(get_logger(), "ControlBridgeNode 启动，目标: %s:%u", control_host_.c_str(), control_port_);
}

void ControlBridgeNode::handle_readiness(
  const std::shared_ptr<ReadinessSrv::Request> request,
  std::shared_ptr<ReadinessSrv::Response> response)
{
  const std::string req_payload = codec::encode_control_readiness_request(request->request);

  TcpClient client(control_host_, control_port_, timeout_ms_);
  std::string rsp_payload;
  const bool ok = client.send_recv(
    MsgType::CONTROL_GET_READINESS_REQ, req_payload,
    MsgType::CONTROL_GET_READINESS_RSP, rsp_payload);

  if (!ok || rsp_payload.empty()) {
    response->response.success                  = false;
    response->response.error_code.code          = ErrorCode::NETWORK_LOSS;
    response->response.message                  = "运控网关：无法连接 Windows 车端。";
    response->response.agent_ready              = false;
    response->response.trajectory_executor_ready = false;
    response->response.vehicle_feedback_ready    = false;
    response->response.control_output_ready      = false;
    response->response.estop_released            = false;
    response->response.vehicle_safe_to_move      = false;
    return;
  }

  codec::decode_control_readiness_response(rsp_payload, response->response);
}

rclcpp_action::GoalResponse ControlBridgeNode::handle_goal(
  const rclcpp_action::GoalUUID & /*uuid*/,
  std::shared_ptr<const ExecuteTask::Goal> goal)
{
  if (goal->goal.header.request_id.empty()) {
    return rclcpp_action::GoalResponse::REJECT;
  }
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

rclcpp_action::CancelResponse ControlBridgeNode::handle_cancel(
  const std::shared_ptr<GoalHandle> /*goal_handle*/)
{
  return rclcpp_action::CancelResponse::ACCEPT;
}

void ControlBridgeNode::handle_accepted(const std::shared_ptr<GoalHandle> goal_handle)
{
  std::thread(std::bind(&ControlBridgeNode::execute_goal, this, goal_handle)).detach();
}

void ControlBridgeNode::execute_goal(const std::shared_ptr<GoalHandle> goal_handle)
{
  // 先回一帧 RUNNING feedback，与 control_calibration_service 行为保持一致。
  auto feedback = std::make_shared<ExecuteTask::Feedback>();
  feedback->feedback.state.state = JobState::RUNNING;
  goal_handle->publish_feedback(feedback);

  TcpClient client(control_host_, control_port_, timeout_ms_);
  std::string rsp_payload;
  const std::string req_payload =
    codec::encode_controller_evaluation_request(goal_handle->get_goal()->goal);
  const bool ok = client.send_recv(
    MsgType::CONTROL_EVALUATION_REQ, req_payload,
    MsgType::CONTROL_EVALUATION_RSP, rsp_payload);

  auto result = std::make_shared<ExecuteTask::Result>();
  result->result.job_id = goal_handle->get_goal()->goal.header.request_id;
  if (ok && !rsp_payload.empty()) {
    codec::decode_control_job_result(rsp_payload, result->result);
    if (result->result.success) {
      goal_handle->succeed(result);
    } else {
      goal_handle->abort(result);
    }
  } else {
    result->result.success = false;
    result->result.error_code.code = ErrorCode::NETWORK_LOSS;
    result->result.message = "运控网关：无法连接 Windows 车端，控制评估执行失败。";
    result->result.data_quality_passed = false;
    result->result.suitable_for_commit = false;
    goal_handle->abort(result);
  }
}

}  // namespace vehicle_agent_gateway

RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_agent_gateway::ControlBridgeNode)
