#pragma once

#include <string>
#include <nlohmann/json.hpp>

namespace custom {

// Compile-time configuration flag
constexpr bool USE_COMPILE_TIME_CONFIG = true;

// Network settings
constexpr const char* NETWORK_INTERFACE = "eth0";

// MQTT settings
constexpr const char* MQTT_BROKER = "localhost";
constexpr int MQTT_PORT = 1883;
constexpr const char* MQTT_USERNAME = "";
constexpr const char* MQTT_PASSWORD = "";
constexpr const char* MQTT_CLIENT_ID = "unitree_go2_client";

// Topic settings
constexpr const char* TOPIC_PREFIX = "unitree/go2";
constexpr const char* TOPIC_CMD = "cmd";
constexpr const char* TOPIC_STATE = "state";
constexpr const char* TOPIC_VIDEO = "video";
constexpr const char* TOPIC_AUDIO = "audio";

// Robot control settings
constexpr double CONTROL_FREQUENCY = 200.0; // Hz
constexpr double STATE_PUBLISH_FREQUENCY = 50.0; // Hz
constexpr bool ENABLE_VIDEO = true;
constexpr bool ENABLE_AUDIO = true;

// Safety settings
constexpr double MAX_LINEAR_VELOCITY = 1.5; // m/s
constexpr double MAX_ANGULAR_VELOCITY = 2.0; // rad/s
constexpr double EMERGENCY_STOP_TIMEOUT = 5.0; // seconds

// Motion settings
constexpr double STAND_HEIGHT = 0.0; // relative height
constexpr int GAIT = 0; // 0:idle, 1:trot, 2:trot_running

// Video settings
constexpr int VIDEO_WIDTH = 1280;
constexpr int VIDEO_HEIGHT = 720;
constexpr int VIDEO_FPS = 30;
constexpr const char* VIDEO_FORMAT = "h264";

// Audio settings
constexpr int AUDIO_SAMPLE_RATE = 16000;
constexpr int AUDIO_CHANNELS = 1;
constexpr const char* AUDIO_FORMAT = "pcm";

// Configuration presets
struct HighPerformancePreset {
    static constexpr double CONTROL_FREQUENCY = 500.0;
    static constexpr double STATE_PUBLISH_FREQUENCY = 100.0;
    static constexpr double MAX_LINEAR_VELOCITY = 2.5;
    static constexpr double MAX_ANGULAR_VELOCITY = 3.0;
    static constexpr bool ENABLE_VIDEO = false; // Disable for performance
    static constexpr bool ENABLE_AUDIO = false; // Disable for performance
};

struct DevelopmentPreset {
    static constexpr double CONTROL_FREQUENCY = 100.0;
    static constexpr double STATE_PUBLISH_FREQUENCY = 20.0;
    static constexpr double MAX_LINEAR_VELOCITY = 0.8;
    static constexpr double MAX_ANGULAR_VELOCITY = 1.0;
    static constexpr bool ENABLE_VIDEO = true;
    static constexpr bool ENABLE_AUDIO = true;
};

struct SafetyPreset {
    static constexpr double CONTROL_FREQUENCY = 50.0;
    static constexpr double STATE_PUBLISH_FREQUENCY = 10.0;
    static constexpr double MAX_LINEAR_VELOCITY = 0.5;
    static constexpr double MAX_ANGULAR_VELOCITY = 0.5;
    static constexpr double EMERGENCY_STOP_TIMEOUT = 2.0;
    static constexpr bool ENABLE_VIDEO = true;
    static constexpr bool ENABLE_AUDIO = true;
};

// Runtime configuration structure - uses compile-time defaults
struct RobotConfig {
    // Network settings
    std::string network_interface = NETWORK_INTERFACE;
    
    // MQTT settings
    std::string mqtt_broker = MQTT_BROKER;
    int mqtt_port = MQTT_PORT;
    std::string mqtt_username = MQTT_USERNAME;
    std::string mqtt_password = MQTT_PASSWORD;
    std::string mqtt_client_id = MQTT_CLIENT_ID;
    
    // Topics
    std::string topic_prefix = TOPIC_PREFIX;
    std::string cmd_topic = TOPIC_CMD;
    std::string state_topic = TOPIC_STATE;
    std::string video_topic = TOPIC_VIDEO;
    std::string audio_topic = TOPIC_AUDIO;
    
    // Robot settings
    double control_frequency = CONTROL_FREQUENCY;
    double state_publish_frequency = STATE_PUBLISH_FREQUENCY;
    bool enable_video = ENABLE_VIDEO;
    bool enable_audio = ENABLE_AUDIO;
    
    // Safety settings
    double max_linear_velocity = MAX_LINEAR_VELOCITY;
    double max_angular_velocity = MAX_ANGULAR_VELOCITY;
    double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
    
    // Motion settings
    double stand_height = STAND_HEIGHT;
    double default_gait = GAIT;
    
    // Video settings
    int video_width = VIDEO_WIDTH;
    int video_height = VIDEO_HEIGHT;
    int video_fps = VIDEO_FPS;
    std::string video_format = VIDEO_FORMAT;
    
    // Audio settings
    int audio_sample_rate = AUDIO_SAMPLE_RATE;
    int audio_channels = AUDIO_CHANNELS;
    std::string audio_format = AUDIO_FORMAT;
};

class Config {
public:
    static Config& getInstance();
    
    /**
     * Load configuration from JSON file
     */
    bool loadConfig(const std::string& configPath);
    
    /**
     * Save current configuration to JSON file
     */
    bool saveConfig(const std::string& configPath);
    
    /**
     * Load configuration from compile-time defaults
     */
    void loadDefaults();
    
    /**
     * Load high-performance preset
     */
    void loadHighPerformancePreset();
    
    /**
     * Load development preset
     */
    void loadDevelopmentPreset();
    
    /**
     * Load safety preset
     */
    void loadSafetyPreset();
    
    /**
     * Validate configuration values
     */
    bool validateConfig();
    
    /**
     * Get current configuration
     */
    const RobotConfig& getConfig() const { return config_; }
    
    /**
     * Get mutable reference to configuration
     */
    RobotConfig& getConfig() { return config_; }
    
    /**
     * Convenience getters
     */
    std::string getFullTopic(const std::string& topic) const;

private:
    Config();
    ~Config() = default;
    Config(const Config&) = delete;
    Config& operator=(const Config&) = delete;
    
    RobotConfig config_;
};

} // namespace custom