#pragma once

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

#include <memory>
#include <thread>
#include <atomic>
#include <chrono>
#include <nlohmann/json.hpp>

namespace custom {

class CustomRobot {
public:
    explicit CustomRobot(const std::string& configFile = "");
    ~CustomRobot();

    bool initialize();
    void shutdown();
    
    bool start();
    bool stop();
    bool isRunning() const { return running_; }
    
    // Main run loop
    void run();
    void runAsync();
    void waitForShutdown();

private:
    // MQTT message handlers
    void handleMqttMessage(const std::string& topic, const std::string& payload);
    void handleCommandMessage(const nlohmann::json& command);
    void handleConfigMessage(const nlohmann::json& config);
    void handleControlMessage(const nlohmann::json& control);
    
    // Robot state handlers
    void handleRobotState(const RobotState& state);
    void handleRobotError(const std::string& error);
    
    // Publishing functions
    void publishState();
    void publishHeartbeat();
    void publishError(const std::string& error);
    void publishResponse(const std::string& requestId, bool success, const std::string& message = "");
    
    // Command processing
    bool processMotionCommand(const nlohmann::json& cmd);
    bool processSpecialAction(const nlohmann::json& cmd);
    bool processSystemCommand(const nlohmann::json& cmd);
    bool processConfigCommand(const nlohmann::json& cmd);
    
    // Utility functions
    MotionCommand jsonToMotionCommand(const nlohmann::json& json);
    nlohmann::json robotStateToJson(const RobotState& state);
    nlohmann::json createStatusMessage();
    
    void startPeriodicTasks();
    void stopPeriodicTasks();
    
    // Connection management
    void handleMqttConnection(bool connected);
    void reconnectMqtt();
    
    // Safety monitoring
    void safetyMonitorLoop();
    void checkEmergencyConditions();
    
    // Components
    std::unique_ptr<Controller> robotController_;
    std::unique_ptr<MqttClient> mqttClient_;
    
    // Configuration
    std::string configFile_;
    
    // State
    std::atomic<bool> running_;
    std::atomic<bool> initialized_;
    std::atomic<bool> mqttConnected_;
    
    // Threads
    std::thread mainThread_;
    std::thread safetyThread_;
    std::vector<std::thread> periodicThreads_;
    
    // Timing
    std::chrono::steady_clock::time_point lastHeartbeat_;
    std::chrono::steady_clock::time_point lastStatePublish_;
    std::chrono::steady_clock::time_point lastCommandReceived_;
    
    // Statistics
    struct Statistics {
        uint64_t commandsReceived = 0;
        uint64_t commandsExecuted = 0;
        uint64_t commandsFailed = 0;
        uint64_t statesPublished = 0;
        uint64_t errorsOccurred = 0;
        std::chrono::steady_clock::time_point startTime;
    } stats_;
    
    // Safety parameters
    double maxIdleTime_ = 30.0; // seconds
    double heartbeatInterval_ = 5.0; // seconds
    double statePublishInterval_ = 0.02; // 50Hz
    
    // Current robot state cache
    mutable std::mutex stateCacheMutex_;
    RobotState lastKnownState_;
    
    // Request tracking
    std::mutex requestMutex_;
    std::map<std::string, std::chrono::steady_clock::time_point> pendingRequests_;
    
    // Error handling
    std::mutex errorMutex_;
    std::queue<std::string> errorQueue_;
    void processErrorQueue();
};

} // namespace custom