#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>

// 引入自定义的 ROS 2 接口头文件
#include "win_ubuntu_bridge/msg/trajectory_point.hpp"
#include "win_ubuntu_bridge/msg/control_telemetry.hpp"
#include "win_ubuntu_bridge/srv/set_control_mode.hpp"
#include "win_ubuntu_bridge/srv/execute_open_loop_cmd.hpp"
#include "win_ubuntu_bridge/srv/follow_test_trajectory.hpp"
#include "win_ubuntu_bridge/srv/execute_step_response.hpp"
#include "win_ubuntu_bridge/srv/inject_tuning_params.hpp"
#include <std_srvs/srv/trigger.hpp>

// 引入 gRPC 网络契约
#include <grpcpp/grpcpp.h>
#include "agv_calib_control.grpc.pb.h"

#include <thread>
#include <atomic>

using namespace agv::calibration::control;

namespace win_ubuntu_bridge {

class ControlGatewayNode : public rclcpp::Node {
public:
    explicit ControlGatewayNode(const rclcpp::NodeOptions & options)
    : Node("control_gateway", options), stream_running_(false) {
        
        RCLCPP_INFO(this->get_logger(), "🧠 运控 gRPC 网关插件启动中... 正在挂载闭环调优 7 大接口！");

        std::string target_ip = this->declare_parameter("agv_ip", "127.0.0.1:50051");
        channel_ = grpc::CreateChannel(target_ip, grpc::InsecureChannelCredentials());
        stub_ = AgvCalibControlService::NewStub(channel_);

        // ==========================================================
        // 1. 挂载 7 个 ROS 2 翻译服务
        // ==========================================================
        srv_set_mode_ = this->create_service<win_ubuntu_bridge::srv::SetControlMode>(
            "~/set_control_mode", std::bind(&ControlGatewayNode::cb_set_mode, this, std::placeholders::_1, std::placeholders::_2));
            
        srv_estop_ = this->create_service<std_srvs::srv::Trigger>(
            "~/emergency_stop", std::bind(&ControlGatewayNode::cb_estop, this, std::placeholders::_1, std::placeholders::_2));

        srv_open_loop_ = this->create_service<win_ubuntu_bridge::srv::ExecuteOpenLoopCmd>(
            "~/execute_open_loop_cmd", std::bind(&ControlGatewayNode::cb_open_loop, this, std::placeholders::_1, std::placeholders::_2));

        srv_follow_traj_ = this->create_service<win_ubuntu_bridge::srv::FollowTestTrajectory>(
            "~/follow_test_trajectory", std::bind(&ControlGatewayNode::cb_follow_traj, this, std::placeholders::_1, std::placeholders::_2));

        srv_step_resp_ = this->create_service<win_ubuntu_bridge::srv::ExecuteStepResponse>(
            "~/execute_step_response", std::bind(&ControlGatewayNode::cb_step_resp, this, std::placeholders::_1, std::placeholders::_2));

        srv_inject_params_ = this->create_service<win_ubuntu_bridge::srv::InjectTuningParams>(
            "~/inject_tuning_params", std::bind(&ControlGatewayNode::cb_inject_params, this, std::placeholders::_1, std::placeholders::_2));

        srv_commit_params_ = this->create_service<std_srvs::srv::Trigger>(
            "~/commit_control_parameters", std::bind(&ControlGatewayNode::cb_commit_params, this, std::placeholders::_1, std::placeholders::_2));

        // ==========================================================
        // 2. 挂载 1 个 ROS 2 遥测发布者 (50Hz)
        // ==========================================================
        pub_telemetry_ = this->create_publisher<win_ubuntu_bridge::msg::ControlTelemetry>("~/control_telemetry", 50);
        
        // 3. 启动独立推流抽水机线程
        stream_running_ = true;
        stream_thread_ = std::thread(&ControlGatewayNode::grpc_stream_to_ros2_topic, this);
    }

    ~ControlGatewayNode() {
        stream_running_ = false;
        if (stream_context_) stream_context_->TryCancel();
        if (stream_thread_.joinable()) stream_thread_.join(); 
    }

private:
    std::shared_ptr<grpc::Channel> channel_;
    std::unique_ptr<AgvCalibControlService::Stub> stub_;
    std::unique_ptr<grpc::ClientContext> stream_context_;
    
    // Service & Publisher 句柄
    rclcpp::Service<win_ubuntu_bridge::srv::SetControlMode>::SharedPtr srv_set_mode_;
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_estop_;
    rclcpp::Service<win_ubuntu_bridge::srv::ExecuteOpenLoopCmd>::SharedPtr srv_open_loop_;
    rclcpp::Service<win_ubuntu_bridge::srv::FollowTestTrajectory>::SharedPtr srv_follow_traj_;
    rclcpp::Service<win_ubuntu_bridge::srv::ExecuteStepResponse>::SharedPtr srv_step_resp_;
    rclcpp::Service<win_ubuntu_bridge::srv::InjectTuningParams>::SharedPtr srv_inject_params_;
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_commit_params_;
    
    rclcpp::Publisher<win_ubuntu_bridge::msg::ControlTelemetry>::SharedPtr pub_telemetry_;
    
    std::thread stream_thread_;
    std::atomic<bool> stream_running_;

    // ----------------------------------------------------------------------
    // ⬇️ ROS 2 to gRPC 核心翻译逻辑 ⬇️
    // ----------------------------------------------------------------------

    void cb_set_mode(const std::shared_ptr<win_ubuntu_bridge::srv::SetControlMode::Request> req,
                     std::shared_ptr<win_ubuntu_bridge::srv::SetControlMode::Response> res) {
        ModeRequest grpc_req;
        grpc_req.set_target_mode(static_cast<ModeRequest::Mode>(req->target_mode));
        StandardResponse grpc_reply;
        grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
        grpc::Status status = stub_->SetControlMode(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    void cb_estop(const std::shared_ptr<std_srvs::srv::Trigger::Request> req, std::shared_ptr<std_srvs::srv::Trigger::Response> res) {
        (void)req; Empty grpc_req; StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(1));
        grpc::Status status = stub_->EmergencyStop(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    void cb_open_loop(const std::shared_ptr<win_ubuntu_bridge::srv::ExecuteOpenLoopCmd::Request> req, std::shared_ptr<win_ubuntu_bridge::srv::ExecuteOpenLoopCmd::Response> res) {
        OpenLoopRequest grpc_req;
        grpc_req.set_left_motor_cmd(req->left_motor_cmd); grpc_req.set_right_motor_cmd(req->right_motor_cmd);
        grpc_req.set_steering_angle(req->steering_angle); grpc_req.set_duration_sec(req->duration_sec);
        StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
        grpc::Status status = stub_->ExecuteOpenLoopCmd(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    // 🚨 核心难点 1：翻译不定长数组 (轨迹考卷下发) + 超长超时时间
    void cb_follow_traj(const std::shared_ptr<win_ubuntu_bridge::srv::FollowTestTrajectory::Request> req,
                        std::shared_ptr<win_ubuntu_bridge::srv::FollowTestTrajectory::Response> res) {
        TrajectoryRequest grpc_req;
        grpc_req.set_test_case_id(req->test_case_id);

        // 遍历 ROS 2 的变长数组，依次 Add 进 gRPC 结构体 (使用 proto 的 add_ 方法分配内存)
        for (const auto& ros_pt : req->path) {
            TrajectoryPoint* grpc_pt = grpc_req.add_path();
            grpc_pt->set_x_m(ros_pt.x_m);
            grpc_pt->set_y_m(ros_pt.y_m);
            grpc_pt->set_yaw_rad(ros_pt.yaw_rad);
            grpc_pt->set_target_speed_ms(ros_pt.target_speed_ms);
            grpc_pt->set_curvature(ros_pt.curvature);
        }

        RCLCPP_INFO(this->get_logger(), "正在下发包含 %zu 个轨迹点的考卷...", req->path.size());

        StandardResponse grpc_reply; 
        grpc::ClientContext context; 
        
        // 🚨 [架构师级防线]：车辆跑一圈可能需要 120 秒！这里的超时时间决不能像其他指令那样设为 2 秒，否则必死锁报错！
        context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(180));
        
        grpc::Status status = stub_->FollowTestTrajectory(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    void cb_step_resp(const std::shared_ptr<win_ubuntu_bridge::srv::ExecuteStepResponse::Request> req, std::shared_ptr<win_ubuntu_bridge::srv::ExecuteStepResponse::Response> res) {
        StepResponseRequest grpc_req;
        grpc_req.set_target_velocity_ms(req->target_velocity_ms); grpc_req.set_duration_sec(req->duration_sec);
        StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(10)); // 阶跃可能耗时几秒
        grpc::Status status = stub_->ExecuteStepResponse(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    // 🚨 核心难点 2：解决 Optional 热更新映射
    void cb_inject_params(const std::shared_ptr<win_ubuntu_bridge::srv::InjectTuningParams::Request> req, std::shared_ptr<win_ubuntu_bridge::srv::InjectTuningParams::Response> res) {
        ControlParams grpc_req;
        
        // 只有当 ROS 2 里的 has_ 标志位为 true 时，才向 gRPC 里 set 值
        if (req->has_wheel_radius_left_ratio) grpc_req.set_wheel_radius_left_ratio(req->wheel_radius_left_ratio);
        if (req->has_wheel_radius_right_ratio) grpc_req.set_wheel_radius_right_ratio(req->wheel_radius_right_ratio);
        if (req->has_effective_track_width_m) grpc_req.set_effective_track_width_m(req->effective_track_width_m);
        if (req->has_steering_zero_offset_deg) grpc_req.set_steering_zero_offset_deg(req->steering_zero_offset_deg);
        
        if (req->has_pid_kp_lateral) grpc_req.set_pid_kp_lateral(req->pid_kp_lateral);
        if (req->has_pid_ki_lateral) grpc_req.set_pid_ki_lateral(req->pid_ki_lateral);
        if (req->has_pid_kd_lateral) grpc_req.set_pid_kd_lateral(req->pid_kd_lateral);
        
        if (req->has_pid_kp_heading) grpc_req.set_pid_kp_heading(req->pid_kp_heading);
        if (req->has_pid_ki_heading) grpc_req.set_pid_ki_heading(req->pid_ki_heading);
        if (req->has_pid_kd_heading) grpc_req.set_pid_kd_heading(req->pid_kd_heading);
        
        if (req->has_pure_pursuit_lookahead_m) grpc_req.set_pure_pursuit_lookahead_m(req->pure_pursuit_lookahead_m);
        if (req->has_mpc_weight_q_lateral) grpc_req.set_mpc_weight_q_lateral(req->mpc_weight_q_lateral);
        if (req->has_mpc_weight_r_steering) grpc_req.set_mpc_weight_r_steering(req->mpc_weight_r_steering);

        StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
        grpc::Status status = stub_->InjectTuningParameters(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    void cb_commit_params(const std::shared_ptr<std_srvs::srv::Trigger::Request> req, std::shared_ptr<std_srvs::srv::Trigger::Response> res) {
        (void)req; Empty grpc_req; StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(3));
        grpc::Status status = stub_->CommitControlParameters(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    // ==========================================================
    // 🌊 异步抽水机：读取 50Hz 运控状态 -> 转化为 ROS 2 Topic
    // ==========================================================
    void grpc_stream_to_ros2_topic() {
        while (stream_running_ && rclcpp::ok()) {
            stream_context_ = std::make_unique<grpc::ClientContext>();
            Empty grpc_req;
            auto reader = stub_->StreamTelemetry(stream_context_.get(), grpc_req);
            
            win_ubuntu_bridge::msg::ControlTelemetry ros_msg;
            TelemetryData grpc_state;

            RCLCPP_INFO(this->get_logger(), "🌊 车端运控 50Hz ODOM 与体感数据流已建立，开始广播...");

            while (stream_running_ && reader->Read(&grpc_state) && rclcpp::ok()) {
                // 将 gRPC Odom 与速度赋值给 ROS msg
                ros_msg.hardware_timestamp_us = grpc_state.hardware_timestamp_us();
                ros_msg.odom_x_m = grpc_state.odom_x_m();
                ros_msg.odom_y_m = grpc_state.odom_y_m();
                ros_msg.odom_yaw_rad = grpc_state.odom_yaw_rad();
                ros_msg.feedback_linear_vel_ms = grpc_state.feedback_linear_vel_ms();
                ros_msg.feedback_angular_vel_rads = grpc_state.feedback_angular_vel_rads();
                ros_msg.left_motor_current_amp = grpc_state.left_motor_current_amp();
                ros_msg.right_motor_current_amp = grpc_state.right_motor_current_amp();
                ros_msg.steering_motor_current_amp = grpc_state.steering_motor_current_amp();
                ros_msg.cmd_steering_output = grpc_state.cmd_steering_output();
                
                // 广播供行为树和 AI 节点白嫖！
                pub_telemetry_->publish(ros_msg);
            }
            
            if (stream_running_) {
                RCLCPP_WARN(this->get_logger(), "⚠️ 运控推流中断！1秒后重连...");
                std::this_thread::sleep_for(std::chrono::seconds(1));
            }
        }
    }
};

} // namespace agv_calib_core

// 🚨 注册为 ROS 2 组件
RCLCPP_COMPONENTS_REGISTER_NODE(win_ubuntu_bridge::ControlGatewayNode)