#pragma once

#include "controller.hpp"
#include "config.hpp"
#include "logger.hpp"
#include "mqtt.hpp"
#include "navigation.hpp"
#include "recharge.hpp"
// #include "low_controller.hpp"
#include "monitor.hpp"

#include <memory>
#include <atomic>
#include <string>
#include <vector>
#include <nlohmann/json.hpp>
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
#include <unitree/common/thread/recurrent_thread.hpp>

namespace custom {

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

    bool initialize();
    bool start();
    
    Controller* getController() const { return controller_.get(); }
    Navigation* getNavigation() const { return navigation_.get(); }
    
    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);
    
    bool initializeMqtt();
    void onMqttMessage(const std::string& topic, const std::string& payload);
    void onMqttConnection(bool connected);
    
    void processCmd(const nlohmann::json& message);
    bool processOacCmd(const std::string& cmd, const nlohmann::json& message);
    bool processSportCmd(const std::string& cmd, const nlohmann::json& message);
    bool processRscCmd(const std::string& cmd, const nlohmann::json& message);
    bool processNavCmd(const std::string& cmd, const nlohmann::json& message);
    bool processMscCmd(const std::string& cmd, const nlohmann::json& message);
    bool processLowCmd(const std::string& cmd, const nlohmann::json& message);
    bool processRechargeCmd(const std::string& cmd, const nlohmann::json& message);
    void printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus = -1);
    
    // Continuous movement control
    bool startContinuousMove(float vx, float vy, float vyaw);
    bool stopContinuousMove();
    void continuousMoveLoop();

private:
    std::string generateRandomClientId() const;
    std::unique_ptr<Controller> controller_;
    // std::unique_ptr<LowController> low_controller_;
    std::unique_ptr<Navigation> navigation_;
    std::unique_ptr<unitree::robot::go2::RobotStateClient> rsc_;
    std::unique_ptr<MqttClient> mqttClient_;
    std::unique_ptr<Recharge> recharge_;
    std::unique_ptr<Monitor> monitor_;
    
    CustomConfig config_;  
    std::atomic<bool> running_;
    std::atomic<bool> initialized_;
    unitree::common::ThreadPtr traj_th_;
    
    // Continuous movement control variables
    std::atomic<bool> continuous_move_enabled_;
    std::atomic<float> continuous_vx_;
    std::atomic<float> continuous_vy_;
    std::atomic<float> continuous_vyaw_;
};

} // namespace custom