#pragma once

#include <behaviortree_cpp_v3/action_node.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

// 引入底层 win_ubuntu_bridge 接口
#include "win_ubuntu_bridge/srv/set_diagnostic_mode.hpp"
#include "win_ubuntu_bridge/srv/trigger_sync_capture.hpp"
#include "win_ubuntu_bridge/action/download_sensor_data.hpp"

namespace agv_calib_core {

class SetChassisModeNode : public BT::SyncActionNode {
public:
    SetChassisModeNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node* node)
        : BT::SyncActionNode(name, config), node_(node) {
        client_ = node_->create_client<win_ubuntu_bridge::srv::SetDiagnosticMode>("/chassis_gateway/set_diagnostic_mode");
    }

    static BT::PortsList providedPorts() { return { BT::InputPort<int>("target_mode") }; }

    BT::NodeStatus tick() override {
        int mode; if (!getInput("target_mode", mode)) return BT::NodeStatus::FAILURE;
        RCLCPP_INFO(node_->get_logger(), "🌲 [BT] 下发底盘夺权指令，模式: %d", mode);
        if (!client_->wait_for_service(std::chrono::seconds(2))) return BT::NodeStatus::FAILURE;

        auto req = std::make_shared<win_ubuntu_bridge::srv::SetDiagnosticMode::Request>(); req->target_mode = mode;
        auto future = client_->async_send_request(req);
        
        // 🚨 这里阻塞等待完全没问题！因为外层 BT 跑在独立线程，根本不影响 ROS 2 Executor 的回调！
        if (future.wait_for(std::chrono::seconds(3)) == std::future_status::ready) {
            auto res = future.get();
            if (res->success) return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
private:
    rclcpp::Node* node_; rclcpp::Client<win_ubuntu_bridge::srv::SetDiagnosticMode>::SharedPtr client_;
};

class TriggerCaptureNode : public BT::SyncActionNode {
public:
    TriggerCaptureNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node* node)
        : BT::SyncActionNode(name, config), node_(node) {
        client_ = node_->create_client<win_ubuntu_bridge::srv::TriggerSyncCapture>("/sensor_gateway/trigger_sync_capture");
    }

    static BT::PortsList providedPorts() {
        return { BT::InputPort<std::string>("sensor_id"), BT::OutputPort<int64_t>("capture_code_out") };
    }

    BT::NodeStatus tick() override {
        std::string sensor_id; getInput("sensor_id", sensor_id);
        RCLCPP_INFO(node_->get_logger(), "📷 [BT] 冻结 %s 数据...", sensor_id.c_str());
        if (!client_->wait_for_service(std::chrono::seconds(2))) return BT::NodeStatus::FAILURE;

        auto req = std::make_shared<win_ubuntu_bridge::srv::TriggerSyncCapture::Request>(); req->sensor_ids.push_back(sensor_id);
        auto future = client_->async_send_request(req);
        if (future.wait_for(std::chrono::seconds(3)) == std::future_status::ready) {
            auto res = future.get();
            if (res->success) { setOutput("capture_code_out", res->capture_timestamp_us); return BT::NodeStatus::SUCCESS; }
        }
        return BT::NodeStatus::FAILURE;
    }
private:
    rclcpp::Node* node_; rclcpp::Client<win_ubuntu_bridge::srv::TriggerSyncCapture>::SharedPtr client_;
};

class DownloadDataNode : public BT::StatefulActionNode {
public:
    DownloadDataNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node* node)
        : BT::StatefulActionNode(name, config), node_(node) {
        action_client_ = rclcpp_action::create_client<win_ubuntu_bridge::action::DownloadSensorData>(node_, "/sensor_gateway/download_sensor_data");
    }

    static BT::PortsList providedPorts() {
        return { BT::InputPort<std::string>("sensor_id"), BT::InputPort<int64_t>("capture_code_in"),
                 BT::InputPort<std::string>("save_dir"), BT::OutputPort<std::string>("saved_path_out") };
    }

    BT::NodeStatus onStart() override {
        int64_t code; std::string sensor; std::string save_dir;
        if (!getInput("capture_code_in", code) || !getInput("sensor_id", sensor) || !getInput("save_dir", save_dir)) return BT::NodeStatus::FAILURE;

        RCLCPP_INFO(node_->get_logger(), "📥 [BT] 挂起下载任务，取件码: %ld", code);
        if (!action_client_->wait_for_action_server(std::chrono::seconds(2))) return BT::NodeStatus::FAILURE;

        auto goal_msg = win_ubuntu_bridge::action::DownloadSensorData::Goal();
        goal_msg.capture_timestamp_us = code; goal_msg.sensor_id = sensor;
        goal_msg.data_type = win_ubuntu_bridge::action::DownloadSensorData::Goal::DATA_TYPE_IMAGE; goal_msg.save_directory = save_dir;

        auto send_goal_options = rclcpp_action::Client<win_ubuntu_bridge::action::DownloadSensorData>::SendGoalOptions();
        send_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle<win_ubuntu_bridge::action::DownloadSensorData>::WrappedResult & result) {
            if (result.code == rclcpp_action::ResultCode::SUCCEEDED && result.result->success) {
                RCLCPP_INFO(node_->get_logger(), "✅ [BT] 落盘成功！路径: %s", result.result->saved_file_path.c_str());
                setOutput("saved_path_out", result.result->saved_file_path); done_ = true; success_ = true;
            } else { done_ = true; success_ = false; }
        };
        action_client_->async_send_goal(goal_msg, send_goal_options); done_ = false; return BT::NodeStatus::RUNNING; 
    }
    BT::NodeStatus onRunning() override { if (done_) return success_ ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; return BT::NodeStatus::RUNNING; }
    void onHalted() override { }
private:
    rclcpp::Node* node_; rclcpp_action::Client<win_ubuntu_bridge::action::DownloadSensorData>::SharedPtr action_client_;
    bool done_ = false; bool success_ = false;
};

} // namespace agv_calib_core