#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <chrono>
#include <mutex>

using namespace std::chrono_literals;
namespace sweeperMsg = sweeper_interfaces::msg;

class ArbitrationNode : public rclcpp::Node
{
public:
    ArbitrationNode()
        : Node("control_arbitrator")
    {
        timeout_ms_ = 200; // 200毫秒超时阈值，可调整

        publisher_ = this->create_publisher<sweeperMsg::McCtrl>("/mc_ctrl", 10);

        sub_radio_ = this->create_subscription<sweeperMsg::McCtrl>(
            "/radio_mc_ctrl", 10,
            [this](const sweeperMsg::McCtrl::SharedPtr msg)
            {
                std::lock_guard<std::mutex> lock(mutex_);
                radio_msg_ = *msg;
                radio_last_time_ = this->now();
                radio_valid_ = true;
            });

        sub_remote_ = this->create_subscription<sweeperMsg::McCtrl>(
            "/remote_mc_ctrl", 10,
            [this](const sweeperMsg::McCtrl::SharedPtr msg)
            {
                std::lock_guard<std::mutex> lock(mutex_);
                remote_msg_ = *msg;
                remote_last_time_ = this->now();
                remote_valid_ = true;
            });

        sub_auto_ = this->create_subscription<sweeperMsg::McCtrl>(
            "/auto_mc_ctrl", 10,
            [this](const sweeperMsg::McCtrl::SharedPtr msg)
            {
                std::lock_guard<std::mutex> lock(mutex_);
                auto_msg_ = *msg;
                auto_last_time_ = this->now();
                auto_valid_ = true;
            });

        timer_ = this->create_wall_timer(20ms, [this]()
                                         { this->arbitrateAndPublish(); });
        RCLCPP_INFO(this->get_logger(), "ArbitrationNode started, waiting for control sources...");
    }

private:
    void arbitrateAndPublish()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        rclcpp::Time now = this->now();

        // 检查每个控制源是否在线
        if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000)
        {
            publisher_->publish(radio_msg_);
            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using RADIO control");
            return;
        }
        if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000)
        {
            publisher_->publish(auto_msg_);
            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using AUTO control");
            return;
        }
        if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000)
        {
            publisher_->publish(remote_msg_);
            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using REMOTE control");
            return;
        }

        // 所有控制源均失联，发布安全默认指令
        sweeperMsg::McCtrl safe_msg;
        safe_msg.brake = 1;
        safe_msg.gear = 0;
        safe_msg.rpm = 0;
        safe_msg.angle = 0;
        safe_msg.angle_speed = 120;

        safe_msg.roll_brush_suction_direction = 0;
        safe_msg.roll_brush_suction = 0;
        safe_msg.side_brush = 0;
        safe_msg.dust_shake = 0;
        safe_msg.brush_deck_pusher = 0;
        safe_msg.suction_squeeqee_pusher = 0;
        safe_msg.water_spray = 0;

        publisher_->publish(safe_msg);
        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                             "[ARBITER] All sources timeout, publishing FAILSAFE control");
    }

    rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr publisher_;

    rclcpp::Subscription<sweeperMsg::McCtrl>::SharedPtr sub_radio_, sub_remote_, sub_auto_;
    rclcpp::TimerBase::SharedPtr timer_;

    sweeperMsg::McCtrl radio_msg_, remote_msg_, auto_msg_;
    rclcpp::Time radio_last_time_, remote_last_time_, auto_last_time_;
    bool radio_valid_ = false;
    bool remote_valid_ = false;
    bool auto_valid_ = false;

    std::mutex mutex_;
    int timeout_ms_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ArbitrationNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}