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

// 引入自定义 ROS 2 接口
#include "win_ubuntu_bridge/msg/camera_intrinsic.hpp"
#include "win_ubuntu_bridge/msg/sensor_extrinsic.hpp"
#include "win_ubuntu_bridge/srv/move_to_observation_pose.hpp"
#include "win_ubuntu_bridge/srv/trigger_sync_capture.hpp"
#include "win_ubuntu_bridge/srv/commit_calibration_results.hpp"
#include "win_ubuntu_bridge/action/download_sensor_data.hpp"

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

// 文件 I/O 与多线程
#include <fstream>
#include <filesystem>
#include <thread>

using namespace agv::calibration::sensor;
namespace fs = std::filesystem;

namespace win_ubuntu_bridge {

class SensorGatewayNode : public rclcpp::Node {
public:
    using DownloadAction = win_ubuntu_bridge::action::DownloadSensorData;
    using GoalHandleDownload = rclcpp_action::ServerGoalHandle<DownloadAction>;

    explicit SensorGatewayNode(const rclcpp::NodeOptions & options)
    : Node("sensor_gateway", options) {
        
        RCLCPP_INFO(this->get_logger(), "📷 传感器 gRPC 网关插件启动中... 准备接管大文件流式拉取引擎！");

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

        // 1. 挂载 3 个瞬间动作服务 (Service)
        srv_move_ = this->create_service<win_ubuntu_bridge::srv::MoveToObservationPose>(
            "~/move_to_pose", std::bind(&SensorGatewayNode::cb_move, this, std::placeholders::_1, std::placeholders::_2));
            
        srv_trigger_ = this->create_service<win_ubuntu_bridge::srv::TriggerSyncCapture>(
            "~/trigger_sync_capture", std::bind(&SensorGatewayNode::cb_trigger, this, std::placeholders::_1, std::placeholders::_2));

        srv_commit_ = this->create_service<win_ubuntu_bridge::srv::CommitCalibrationResults>(
            "~/commit_calibration_results", std::bind(&SensorGatewayNode::cb_commit, this, std::placeholders::_1, std::placeholders::_2));

        // 2. 🌟 挂载 1 个耗时动作服务器 (Action Server) —— 专治大文件流
        action_download_ = rclcpp_action::create_server<DownloadAction>(
            this,
            "~/download_sensor_data",
            std::bind(&SensorGatewayNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&SensorGatewayNode::handle_cancel, this, std::placeholders::_1),
            std::bind(&SensorGatewayNode::handle_accepted, this, std::placeholders::_1));
    }

private:
    std::shared_ptr<grpc::Channel> channel_;
    std::unique_ptr<SensorCalibrationService::Stub> stub_;

    rclcpp::Service<win_ubuntu_bridge::srv::MoveToObservationPose>::SharedPtr srv_move_;
    rclcpp::Service<win_ubuntu_bridge::srv::TriggerSyncCapture>::SharedPtr srv_trigger_;
    rclcpp::Service<win_ubuntu_bridge::srv::CommitCalibrationResults>::SharedPtr srv_commit_;
    rclcpp_action::Server<DownloadAction>::SharedPtr action_download_;

    // ==========================================================
    // 瞬时服务翻译 (Service)
    // ==========================================================
    void cb_move(const std::shared_ptr<win_ubuntu_bridge::srv::MoveToObservationPose::Request> req,
                 std::shared_ptr<win_ubuntu_bridge::srv::MoveToObservationPose::Response> res) {
        PoseRequest grpc_req;
        grpc_req.set_target_x_m(req->target_x_m); grpc_req.set_target_y_m(req->target_y_m);
        grpc_req.set_target_yaw_deg(req->target_yaw_deg); grpc_req.set_is_relative(req->is_relative);
        StandardResponse grpc_reply; grpc::ClientContext context; 
        context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(60)); // 走位可能需要较长时间
        grpc::Status status = stub_->MoveToObservationPose(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
    }

    void cb_trigger(const std::shared_ptr<win_ubuntu_bridge::srv::TriggerSyncCapture::Request> req,
                    std::shared_ptr<win_ubuntu_bridge::srv::TriggerSyncCapture::Response> res) {
        CaptureRequest grpc_req;
        for (const auto& id : req->sensor_ids) { grpc_req.add_sensor_ids(id); }
        CaptureResponse grpc_reply; grpc::ClientContext context; 
        context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(3));
        grpc::Status status = stub_->TriggerSyncCapture(&context, grpc_req, &grpc_reply);
        res->success = status.ok() && grpc_reply.success(); 
        res->capture_timestamp_us = grpc_reply.capture_timestamp_us(); // 🚨 取回取件码
        res->error_message = status.ok() ? grpc_reply.error_message() : status.error_message();
    }

    void cb_commit(const std::shared_ptr<win_ubuntu_bridge::srv::CommitCalibrationResults::Request> req,
                   std::shared_ptr<win_ubuntu_bridge::srv::CommitCalibrationResults::Response> res) {
        CalibrationPayload grpc_req;
        grpc_req.set_task_id(req->task_id);
        
        // 映射结构体数组
        for (const auto& ros_in : req->updated_intrinsics) {
            auto* grpc_in = grpc_req.add_updated_intrinsics();
            grpc_in->set_camera_id(ros_in.camera_id); grpc_in->set_fx(ros_in.fx); grpc_in->set_fy(ros_in.fy);
            grpc_in->set_cx(ros_in.cx); grpc_in->set_cy(ros_in.cy);
            for (auto d : ros_in.dist_coeffs) grpc_in->add_dist_coeffs(d);
        }
        for (const auto& ros_ex : req->updated_extrinsics) {
            auto* grpc_ex = grpc_req.add_updated_extrinsics();
            grpc_ex->set_source_frame(ros_ex.source_frame); grpc_ex->set_target_frame(ros_ex.target_frame);
            grpc_ex->set_trans_x_mm(ros_ex.trans_x_mm); grpc_ex->set_trans_y_mm(ros_ex.trans_y_mm); grpc_ex->set_trans_z_mm(ros_ex.trans_z_mm);
            grpc_ex->set_roll_deg(ros_ex.roll_deg); grpc_ex->set_pitch_deg(ros_ex.pitch_deg); grpc_ex->set_yaw_deg(ros_ex.yaw_deg);
        }

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

    // ==========================================================
    // 🚨 灵魂逻辑：大文件流式下载 转换为 ROS 2 Action 落盘
    // ==========================================================
    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const DownloadAction::Goal> goal) {
        (void)uuid;
        RCLCPP_INFO(this->get_logger(), "📥 收到大文件下载工单! 凭证: %ld, 传感器: %s", goal->capture_timestamp_us, goal->sensor_id.c_str());
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleDownload> goal_handle) {
        (void)goal_handle;
        RCLCPP_WARN(this->get_logger(), "🛑 行为树请求强行取消下载");
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted(const std::shared_ptr<GoalHandleDownload> goal_handle) {
        // 🚨 必须在独立线程中执行阻塞的网络流读取，否则会死锁整个 ROS 2 容器！
        std::thread{std::bind(&SensorGatewayNode::execute_download, this, std::placeholders::_1), goal_handle}.detach();
    }

    // 后台真实下载线程
    void execute_download(const std::shared_ptr<GoalHandleDownload> goal_handle) {
        const auto goal = goal_handle->get_goal();
        auto result = std::make_shared<DownloadAction::Result>();
        auto feedback = std::make_shared<DownloadAction::Feedback>();

        DataFetchRequest grpc_req;
        grpc_req.set_capture_timestamp_us(goal->capture_timestamp_us);
        grpc_req.set_sensor_id(goal->sensor_id);

        grpc::ClientContext context;
        context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(60)); // 大文件给足 60 秒传输时间

        std::unique_ptr<grpc::ClientReader<FileChunk>> reader;
        if (goal->data_type == DownloadAction::Goal::DATA_TYPE_IMAGE) {
            reader = stub_->DownloadImage(&context, grpc_req);
        } else {
            reader = stub_->DownloadPointCloud(&context, grpc_req);
        }

        FileChunk chunk;
        std::ofstream outfile;
        std::string final_file_path;
        uint64_t total_bytes = 0;
        bool is_first_chunk = true;

        RCLCPP_INFO(this->get_logger(), "🚀 开始从车端流式拉取传感器大文件...");

        // 🌊 像蚂蚁搬家一样，收到一块，就用二进制追加写入硬盘一块！
        while (reader->Read(&chunk) && rclcpp::ok()) {
            
            // 1. 检查行为树是否发出了取消指令
            if (goal_handle->is_canceling()) {
                context.TryCancel(); // 物理打断 gRPC 传输
                if (outfile.is_open()) outfile.close();
                if (!final_file_path.empty() && fs::exists(final_file_path)) fs::remove(final_file_path); // 删掉下了一半的残次品
                result->success = false; result->error_message = "下载被系统强制中止";
                goal_handle->canceled(result);
                return;
            }

            // 2. 如果是第一包数据，创建目录并打开硬盘文件
            if (is_first_chunk) {
                if (!fs::exists(goal->save_directory)) { fs::create_directories(goal->save_directory); }
                
                std::string ext = chunk.format_ext().empty() ? "bin" : chunk.format_ext(); // 动态获取文件后缀
                final_file_path = goal->save_directory + "/" + goal->sensor_id + "_" + std::to_string(goal->capture_timestamp_us) + "." + ext;
                
                // 🚨 以纯二进制 (binary) 和截断清空 (trunc) 模式打开硬盘文件！
                outfile.open(final_file_path, std::ios::out | std::ios::binary | std::ios::trunc);
                if (!outfile.is_open()) {
                    result->success = false; result->error_message = "无法在 Ubuntu 硬盘创建文件！权限不足？";
                    goal_handle->abort(result);
                    return;
                }
                is_first_chunk = false;
            }

            // 3. 把网络收到的内存碎块直接写入硬盘！(内存占用极低，绝对不会爆内存)
            outfile.write(chunk.chunk_data().data(), chunk.chunk_data().size());
            total_bytes += chunk.chunk_data().size();

            // 4. 每收到一块，通过 Action Feedback 汇报一次进度
            feedback->downloaded_bytes = total_bytes;
            goal_handle->publish_feedback(feedback);
            
            if (chunk.is_last_chunk()) break;
        }

        grpc::Status status = reader->Finish();
        if (outfile.is_open()) outfile.close();

        // 5. 善后汇报
        if (status.ok() && !is_first_chunk) {
            RCLCPP_INFO(this->get_logger(), "✅ 下载落盘成功！共计 %.2f MB -> %s", total_bytes / 1024.0 / 1024.0, final_file_path.c_str());
            result->success = true;
            result->saved_file_path = final_file_path; // 🚨 核心：将绝对路径交还给行为树！
            goal_handle->succeed(result);
        } else {
            if (!final_file_path.empty() && fs::exists(final_file_path)) fs::remove(final_file_path); // 失败清理垃圾
            RCLCPP_ERROR(this->get_logger(), "❌ 下载大文件失败: %s", status.error_message().c_str());
            result->success = false;
            result->error_message = "网络传输中断: " + status.error_message();
            goal_handle->abort(result);
        }
    }
};

} // namespace agv_calib_core

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