#pragma once

#include "controller.hpp"
#include "config.hpp"
#include "logger.hpp"

#include <memory>
#include <atomic>
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>

namespace custom {

class CustomRobot {
public:
    explicit CustomRobot();
    ~CustomRobot();

    bool initialize();
    bool start();
    
    // Robot state methods
    bool GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList);
    bool SwitchService(const std::string& serviceName, bool enable);
    bool SetReportFreq(int32_t interval, int32_t duration);

private:
    std::unique_ptr<Controller> controller_;
    std::unique_ptr<unitree::robot::go2::RobotStateClient> stateClient_;
    
    std::atomic<bool> running_;
    std::atomic<bool> initialized_;
};

} // namespace custom