#ifndef LOW_CONTROLLER_HPP
#define LOW_CONTROLLER_HPP

#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/idl/go2/LowCmd_.hpp>
#include <unitree/common/thread/thread.hpp>
#include <atomic>

#define TOPIC_LOWCMD "rt/lowcmd"
#define UT_CPU_ID_NONE  -1

constexpr double PosStopF = (2.146E+9f);
constexpr double VelStopF = (16000.0f);
namespace custom {

class LowController {
public:
    LowController();
    ~LowController();

    bool initialize();
    bool start();
    bool stop();
    void shutdown() { stop(); }
    bool isRunning() const { return running_; }

    void requestAutoCharge(bool enable);
    void powerOff();

private:
    void publishLoop();
    void initLowCmd();
    static uint32_t crc32_core(uint32_t* ptr, uint32_t len);

    unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher_;
    unitree_go::msg::dds_::LowCmd_ low_cmd_{};
    unitree::common::ThreadPtr publish_thread_;
    std::atomic<bool> running_{false};
};

} // namespace custom

#endif // LOW_CONTROLLER_HPP