#pragma once

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

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

namespace custom {

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

    bool initialize();
    bool start();
    
    Controller* getController() const { return controller_.get(); }
    
    // 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> rsc_;
    
    CustomConfig config_;  
    std::atomic<bool> running_;
    std::atomic<bool> initialized_;
};

} // namespace custom