#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 {

/**
 * @brief 轨迹路径样式枚举
 */
enum class TrajectoryPattern : int {
    SINE_WAVE = 0,      // 正弦波（原有的波浪形）
    CIRCLE = 1,         // 圆形
    SQUARE = 2,         // 正方形
    DIAMOND = 3,        // 菱形
    FIGURE_EIGHT = 4    // 8字形
};

/**
 * @brief 轨迹参数结构
 */
struct TrajectoryParams {
    TrajectoryPattern pattern = TrajectoryPattern::SINE_WAVE;  // 路径样式
    float scale = 1.0f;         // 缩放因子
    float speed = 0.3f;         // 移动速度 (m/s)
    float frequency = 1.0f;     // 频率参数（用于周期性路径）
    float amplitude = 0.6f;     // 振幅参数
    int points_count = 30;      // 路径点数量
    float time_step = 0.06f;    // 时间步长
    
    // 构造函数
    TrajectoryParams() = default;
    TrajectoryParams(TrajectoryPattern p, float s = 1.0f, float sp = 0.3f) 
        : pattern(p), scale(s), speed(sp) {}
};

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);

    bool startTraj(const TrajectoryParams& params = TrajectoryParams());
    void stopTraj();

private:
    void trajControl();
    std::vector<unitree::robot::go2::PathPoint> generateTrajectoryPath(const TrajectoryParams& params);
    std::vector<unitree::robot::go2::PathPoint> generateSineWavePath(const TrajectoryParams& params);
    std::vector<unitree::robot::go2::PathPoint> generateCirclePath(const TrajectoryParams& params);
    std::vector<unitree::robot::go2::PathPoint> generateSquarePath(const TrajectoryParams& params);
    std::vector<unitree::robot::go2::PathPoint> generateDiamondPath(const TrajectoryParams& params);
    std::vector<unitree::robot::go2::PathPoint> generateFigureEightPath(const TrajectoryParams& params);
    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_;
    std::atomic<bool> traj_running_;
    float traj_dt_;
    float traj_count_;
    TrajectoryParams current_traj_params_;  // 当前轨迹参数
};

} // namespace custom