#include <rclcpp/rclcpp.hpp>
#include <grpcpp/grpcpp.h>
// 包含刚才 CMake 自动生成的头文件
#include "agv_calib_control.grpc.pb.h" 

using namespace agv::calibration::control;

class BrainNode : public rclcpp::Node {
public:
    BrainNode() : Node("calib_brain_node") {
        // 1. 拨号连接到 Windows 车端 (请替换为车端在 Wi-Fi 6 下的真实 IP)
        std::string target_ip = "192.168.1.100:50051";
        
        // 🚨 Wi-Fi 防断联核心：强制开启 gRPC 保活机制 (KeepAlive)
        grpc::ChannelArguments args;
        args.SetInt(GRPC_ARG_KEEPALIVE_TIME_MS, 2000);    // 每 2 秒发一次 TCP 心跳探活
        args.SetInt(GRPC_ARG_KEEPALIVE_TIMEOUT_MS, 1000); // 1 秒没回音即认为掉线
        args.SetInt(GRPC_ARG_KEEPALIVE_PERMIT_WITHOUT_CALLS, 1);
        
        auto channel = grpc::CreateCustomChannel(target_ip, grpc::InsecureChannelCredentials(), args);
        stub_ = AgvCalibControlService::NewStub(channel);

        RCLCPP_INFO(this->get_logger(), "连接车端 %s 中...", target_ip.c_str());
        
        // 2. 发起夺权指令测试
        send_control_mode_request();
    }

private:
    std::unique_ptr<AgvCalibControlService::Stub> stub_;

    void send_control_mode_request() {
        ModeRequest request;
        request.set_target_mode(ModeRequest::OPEN_LOOP_MODE); // 下发开环提线木偶模式

        StandardResponse response;
        grpc::ClientContext context;
        
        // 【网络防卡死】设置单次通信的超时时间为 2 秒，防止死锁 ROS 2 主线程
        context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));

        grpc::Status status = stub_->SetControlMode(&context, request, &response);

        if (status.ok() && response.success()) {
            RCLCPP_INFO(this->get_logger(), "✅ 夺权成功！车端回复: %s", response.message().c_str());
        } else {
            RCLCPP_ERROR(this->get_logger(), "❌ 调用失败: %d - %s", status.error_code(), status.error_message().c_str());
        }
    }
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<BrainNode>());
    rclcpp::shutdown();
    return 0;
}