#pragma once
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <string>
#include <vector>
#include <functional>
#include <mutex>
#include <atomic>
#include <thread>
#include <condition_variable>

#define RechargeTopic "rt/aruco_cmd"
#define RechargeStateTopic "rt/aruco_state"

namespace custom {
    class Recharge {
    public:
        Recharge(double timeout = 60.0);
        
        bool Init();

        std::string StartRecharge(int repeat = 1, double period_sec = 1.0);

        bool StopRecharge(int repeat = 1, double period_sec = 1.0);

        void RegisterStateCallback(std::function<void(const std::string&)> callback);

        std::string GetLastState() const;

    private:
        void OnStateMessage(const void* message);

        bool Publish(const std::string& data, int repeat = 1, double period_sec = 1.0);

        double timeout_;
        mutable std::mutex state_mutex_;
        std::string last_state_;
        std::vector<std::function<void(const std::string&)>> callbacks_;
        std::condition_variable cv_;
        
        unitree::robot::ChannelPublisherPtr<std_msgs::msg::dds_::String_> cmd_pub_;
        unitree::robot::ChannelSubscriberPtr<std_msgs::msg::dds_::String_> state_sub_;
    };
}