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

// 🚨 引入自定义的 ROS 2 接口头文件 (由 win_ubuntu_bridge 包生成)
#include "win_ubuntu_bridge/msg/hardware_state.hpp"
#include "win_ubuntu_bridge/srv/set_diagnostic_mode.hpp"
#include "win_ubuntu_bridge/srv/execute_raw_drive.hpp"
#include "win_ubuntu_bridge/srv/execute_raw_steer.hpp"
#include "win_ubuntu_bridge/srv/commit_kinematics.hpp"
#include <std_srvs/srv/trigger.hpp> // 紧急刹车可以直接用标准库的 Trigger

// 引入 gRPC 自动生成的契约头文件
#include <grpcpp/grpcpp.h>
#include "agv_calib_chassis.grpc.pb.h"

#include <thread>
#include <atomic>

using namespace agv::calibration::chassis;

namespace win_ubuntu_bridge {

class ChassisGatewayNode : public rclcpp::Node {
public:
    explicit ChassisGatewayNode(const rclcpp::NodeOptions & options)
    : Node("chassis_gateway", options), stream_running_(false) {
        
        RCLCPP_INFO(this->get_logger(), "🔌 底盘 gRPC 网关插件启动中... 正在挂载 6 大完整接口！");

        // 1. 拨号连接 Windows 车端
        std::string target_ip = this->declare_parameter("agv_ip", "127.0.0.1:50051");
        channel_ = grpc::CreateChannel(target_ip, grpc::InsecureChannelCredentials());
        stub_ = AgvCalibChassisService::NewStub(channel_);

        // ==========================================================
        // 2. 注册 5 个 ROS 2 服务 (拦截内部行为树的请求，翻译给车端)
        // ==========================================================
        
        // 接口 1：夺权
        srv_set_mode_ = this->create_service<win_ubuntu_bridge::srv::SetDiagnosticMode>(
            "~/set_diagnostic_mode", std::bind(&ChassisGatewayNode::cb_set_mode, this, std::placeholders::_1, std::placeholders::_2));
        
        // 接口 2：最高优急停 (无参数，用 Trigger)
        srv_estop_ = this->create_service<std_srvs::srv::Trigger>(
            "~/hardware_emergency_brake", std::bind(&ChassisGatewayNode::cb_emergency_brake, this, std::placeholders::_1, std::placeholders::_2));

        // 接口 3：开环直行 (测轮径)
        srv_raw_drive_ = this->create_service<win_ubuntu_bridge::srv::ExecuteRawDrive>(
            "~/execute_raw_drive", std::bind(&ChassisGatewayNode::cb_raw_drive, this, std::placeholders::_1, std::placeholders::_2));

        // 接口 4：开环转向 (测死区/零位)
        srv_raw_steer_ = this->create_service<win_ubuntu_bridge::srv::ExecuteRawSteer>(
            "~/execute_raw_steer", std::bind(&ChassisGatewayNode::cb_raw_steer, this, std::placeholders::_1, std::placeholders::_2));

        // 接口 5：物理参数定稿固化
        srv_commit_kinematics_ = this->create_service<win_ubuntu_bridge::srv::CommitKinematics>(
            "~/commit_kinematics", std::bind(&ChassisGatewayNode::cb_commit_kinematics, this, std::placeholders::_1, std::placeholders::_2));

        // ==========================================================
        // 3. 注册 1 个 ROS 2 话题 (发布从车端抽上来的 50Hz 流数据)
        // ==========================================================
        pub_telemetry_ = this->create_publisher<win_ubuntu_bridge::msg::HardwareState>("~/hardware_telemetry", 50);
        
        // 4. 启动异步抽水机线程
        stream_running_ = true;
        stream_thread_ = std::thread(&ChassisGatewayNode::grpc_stream_to_ros2_topic, this);
    }

    ~ChassisGatewayNode() {
        stream_running_ = false;
        if (stream_context_) stream_context_->TryCancel(); // 唤醒阻塞的 gRPC
        if (stream_thread_.joinable()) stream_thread_.join(); 
    }

private:
    std::shared_ptr<grpc::Channel> channel_;
    std::unique_ptr<AgvCalibChassisService::Stub> stub_;
    std::unique_ptr<grpc::ClientContext> stream_context_;
    
    // ROS 2 接口句柄
    rclcpp::Service<win_ubuntu_bridge::srv::SetDiagnosticMode>::SharedPtr srv_set_mode_;
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_estop_;
    rclcpp::Service<win_ubuntu_bridge::srv::ExecuteRawDrive>::SharedPtr srv_raw_drive_;
    rclcpp::Service<win_ubuntu_bridge::srv::ExecuteRawSteer>::SharedPtr srv_raw_steer_;
    rclcpp::Service<win_ubuntu_bridge::srv::CommitKinematics>::SharedPtr srv_commit_kinematics_;
    rclcpp::Publisher<win_ubuntu_bridge::msg::HardwareState>::SharedPtr pub_telemetry_;
    
    std::thread stream_thread_;
    std::atomic<bool> stream_running_;

    // ----------------------------------------------------------------------
    // ⬇️ 以下全是完美的 1:1 翻译逻辑 (ROS Request -> gRPC -> ROS Response) ⬇️
    // ----------------------------------------------------------------------

    // [翻译] 接口 1: SetDiagnosticMode
    void cb_set_mode(const std::shared_ptr<win_ubuntu_bridge::srv::SetDiagnosticMode::Request> req,
                     std::shared_ptr<win_ubuntu_bridge::srv::SetDiagnosticMode::Response> res) {
        DiagnosticModeRequest grpc_req;
        grpc_req.set_target_mode(req->target_mode == 1 ? DiagnosticModeRequest::DIRECT_RAW_DRIVE : DiagnosticModeRequest::NORMAL_KINEMATICS);
        StandardResponse grpc_reply;
        grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
        grpc::Status status = stub_->SetDiagnosticMode(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success();
        res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    // [翻译] 接口 2: HardwareEmergencyBrake
    void cb_emergency_brake(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)); // 急停超时设为1秒
        grpc::Status status = stub_->HardwareEmergencyBrake(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success();
        res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    // [翻译] 接口 3: ExecuteRawDriveCommand
    void cb_raw_drive(const std::shared_ptr<win_ubuntu_bridge::srv::ExecuteRawDrive::Request> req,
                      std::shared_ptr<win_ubuntu_bridge::srv::ExecuteRawDrive::Response> res) {
        RawDriveRequest grpc_req;
        grpc_req.set_test_case_id(req->test_case_id);
        grpc_req.set_fl_motor_rpm(req->fl_motor_rpm);
        grpc_req.set_fr_motor_rpm(req->fr_motor_rpm);
        grpc_req.set_rl_motor_rpm(req->rl_motor_rpm);
        grpc_req.set_rr_motor_rpm(req->rr_motor_rpm);
        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_->ExecuteRawDriveCommand(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success();
        res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    // [翻译] 接口 4: ExecuteRawSteerCommand
    void cb_raw_steer(const std::shared_ptr<win_ubuntu_bridge::srv::ExecuteRawSteer::Request> req,
                      std::shared_ptr<win_ubuntu_bridge::srv::ExecuteRawSteer::Response> res) {
        RawSteerRequest grpc_req;
        grpc_req.set_test_case_id(req->test_case_id);
        grpc_req.set_front_steer_angle_deg(req->front_steer_angle_deg);
        grpc_req.set_rear_steer_angle_deg(req->rear_steer_angle_deg);
        grpc_req.set_duration_sec(req->duration_sec);
        
        // 处理 proto 的 optional 字段 (依靠 ROS 2 传来的 bool 判断)
        if (req->has_sweep) {
            grpc_req.set_sweep_amplitude_deg(req->sweep_amplitude_deg);
            grpc_req.set_sweep_frequency_hz(req->sweep_frequency_hz);
        }

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

    // [翻译] 接口 5: CommitKinematicParameters
    void cb_commit_kinematics(const std::shared_ptr<win_ubuntu_bridge::srv::CommitKinematics::Request> req,
                              std::shared_ptr<win_ubuntu_bridge::srv::CommitKinematics::Response> res) {
        KinematicParams grpc_req;
        // ROS 2 请求中依靠 bool 标志位来判断是否 set 进 grpc (实现真正的 optional)
        if (req->has_wheel_radius_fl) grpc_req.set_wheel_radius_fl_m(req->wheel_radius_fl_m);
        if (req->has_wheel_radius_fr) grpc_req.set_wheel_radius_fr_m(req->wheel_radius_fr_m);
        if (req->has_wheel_radius_rl) grpc_req.set_wheel_radius_rl_m(req->wheel_radius_rl_m);
        if (req->has_wheel_radius_rr) grpc_req.set_wheel_radius_rr_m(req->wheel_radius_rr_m);
        if (req->has_steer_zero_offset_front) grpc_req.set_steer_zero_offset_front_deg(req->steer_zero_offset_front_deg);
        if (req->has_steer_zero_offset_rear) grpc_req.set_steer_zero_offset_rear_deg(req->steer_zero_offset_rear_deg);
        if (req->has_effective_track_width) grpc_req.set_effective_track_width_m(req->effective_track_width_m);
        if (req->has_effective_wheel_base) grpc_req.set_effective_wheel_base_m(req->effective_wheel_base_m);
        if (req->has_icr_offset_x) grpc_req.set_icr_offset_x_m(req->icr_offset_x_m);
        if (req->has_icr_offset_y) grpc_req.set_icr_offset_y_m(req->icr_offset_y_m);

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

    // ----------------------------------------------------------------------
    // 🌊 接口 6 [异步水龙头]: 后台读取 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_->StreamHardwareTelemetry(stream_context_.get(), grpc_req);
            win_ubuntu_bridge::msg::HardwareState ros_msg;
            HardwareState grpc_state;

            RCLCPP_INFO(this->get_logger(), "🌊 车端底盘 50Hz 裸数据流连接已建立，开始广播 ROS 2 Topic...");

            // 只要网不断，Read() 就会以 50Hz 的频率一直被触发
            while (stream_running_ && reader->Read(&grpc_state) && rclcpp::ok()) {
                // 完美 1:1 结构体拷贝
                ros_msg.hardware_timestamp_us = grpc_state.hardware_timestamp_us();
                ros_msg.encoder_ticks_fl = grpc_state.encoder_ticks_fl();
                ros_msg.encoder_ticks_fr = grpc_state.encoder_ticks_fr();
                ros_msg.encoder_ticks_rl = grpc_state.encoder_ticks_rl();
                ros_msg.encoder_ticks_rr = grpc_state.encoder_ticks_rr();
                ros_msg.actual_steer_angle_front_deg = grpc_state.actual_steer_angle_front_deg();
                ros_msg.actual_steer_angle_rear_deg = grpc_state.actual_steer_angle_rear_deg();
                ros_msg.current_fl_amp = grpc_state.current_fl_amp();
                ros_msg.current_fr_amp = grpc_state.current_fr_amp();
                ros_msg.current_rl_amp = grpc_state.current_rl_amp();
                ros_msg.current_rr_amp = grpc_state.current_rr_amp();
                ros_msg.current_steer_front_amp = grpc_state.current_steer_front_amp();
                ros_msg.driver_error_code = grpc_state.driver_error_code();
                
                // 发布到 ROS 2 网络中！
                pub_telemetry_->publish(ros_msg);
            }
            
            // 如果走到这里，说明网络断了或者发生了异常，稍等 1 秒后自动重试
            if (stream_running_) {
                RCLCPP_WARN(this->get_logger(), "⚠️ gRPC 到底端的流意外中断！尝试重连...");
                std::this_thread::sleep_for(std::chrono::seconds(1));
            }
        }
    }
};

} // namespace agv_calib_core

RCLCPP_COMPONENTS_REGISTER_NODE(win_ubuntu_bridge::ChassisGatewayNode)