#include "custom_robot.hpp"
#include <thread>
#include <chrono>
#include <functional>
#include <ctime>
#include <random>
#include <sstream>
#include <iomanip>
#include <nlohmann/json.hpp>
#include <unitree/robot/channel/channel_factory.hpp>

namespace custom {

CustomRobot::CustomRobot()
    : initialized_(false), running_(false) {
    config_.loadDefaults();
    
    try {
        unitree::robot::ChannelFactory::Instance()->Init(0, config_.network_interface);
        LOG_INFO("ChannelFactory initialized with interface: " + config_.network_interface);
    } catch (const std::exception& e) {
        LOG_ERROR("Failed to initialize ChannelFactory: " + std::string(e.what()));
    }
}

CustomRobot::~CustomRobot() {
    LOG_INFO("Shutting down CustomRobot");
    
    if (mqttClient_) {
        mqttClient_->stopMessageProcessor();
        mqttClient_->disconnect();
        mqttClient_.reset();
        LOG_INFO("MQTT client disconnected and cleaned up");
    }
    
    if (controller_) {
        controller_->shutdown();
        controller_.reset();
    }
    
    if (rsc_) {
        rsc_.reset();
    }
    
    try {
        unitree::robot::ChannelFactory::Instance()->Release();
        LOG_INFO("ChannelFactory released");
    } catch (const std::exception& e) {
        LOG_ERROR("Failed to release ChannelFactory: " + std::string(e.what()));
    }
    
    initialized_ = false;
}

bool CustomRobot::initialize() {
    try {
        controller_ = std::make_unique<Controller>();
        controller_->initialize();

        rsc_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
        rsc_->SetTimeout(3.0f);
        rsc_->Init();
        
        if (!initializeMqtt()) {
            LOG_ERROR("Failed to initialize MQTT client");
            return false;
        }
    } catch (const std::exception& e) {
        LOG_ERROR("Failed to initialize : " + std::string(e.what()));
        return false;
    }
    
    initialized_ = true;
    LOG_INFO("CustomRobot initialized successfully");
    return true;
}


bool CustomRobot::start() {
    if (!initialized_) {
        LOG_ERROR("Cannot start: not initialized");
        return false;
    }
    
    if (running_) {
        LOG_WARN("CustomRobot already running");
        return true;
    }
    
    std::vector<unitree::robot::go2::ServiceState> serviceList;
    if (!GetServiceList(serviceList)) {
        LOG_ERROR("Failed to get service list");
        return false;
    }
    
    if (!SwitchService("sport_mode", false)) {
        LOG_ERROR("Failed to stop sport_mode service");
        return false;
    }
    
    std::this_thread::sleep_for(std::chrono::seconds(1));
    
    if (!controller_->start()) {
        LOG_ERROR("Failed to start robot controller");
        return false;
    }
    running_ = true;
    LOG_INFO("CustomRobot started successfully");
    return true;
}

bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
    if (!initialized_ || !rsc_) {
        LOG_ERROR("Robot not initialized or stateClient is null");
        return false;
    }
    
    try {
        int32_t ret = rsc_->ServiceList(serviceList);
        if (ret != 0) {
            LOG_ERROR("Failed to get service list, error code: " + std::to_string(ret));
            return false;
        }
        LOG_INFO("Successfully retrieved service list with " + std::to_string(serviceList.size()) + " services");
        return true;
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in getServiceList: " + std::string(e.what()));
        return false;
    }
}

bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) {
    if (!initialized_ || !rsc_) {
        LOG_ERROR("Robot not initialized or stateClient is null");
        return false;
    }
    
    try {
        int32_t status;
        int32_t ret = rsc_->ServiceSwitch(serviceName, enable ? 1 : 0, status);
        if (ret != 0) {
            LOG_ERROR("Failed to switch service " + serviceName + ", error code: " + std::to_string(ret));
            return false;
        }
        LOG_INFO("Successfully switched service " + serviceName + " to " + (enable ? "enabled" : "disabled"));
        return true;
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in switchService: " + std::string(e.what()));
        return false;
    }
}

bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
    if (!initialized_ || !rsc_) {
        LOG_ERROR("Robot not initialized or stateClient is null");
        return false;
    }
    
    try {
        int32_t ret = rsc_->SetReportFreq(interval, duration);
        if (ret != 0) {
            LOG_ERROR("Failed to set report frequency, error code: " + std::to_string(ret));
            return false;
        }
        LOG_INFO("Successfully set report frequency: interval=" + std::to_string(interval) + ", duration=" + std::to_string(duration));
        return true;
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in setReportFreq: " + std::string(e.what()));
        return false;
    }
}

bool CustomRobot::initializeMqtt() {
    try {
        std::string broker = config_.mqtt_broker;
        int port = config_.mqtt_port;
        std::string clientId = generateRandomClientId();  
        
        mqttClient_ = std::make_unique<MqttClient>(broker, port, clientId);
        using namespace std::placeholders;
        mqttClient_->setMessageCb(std::bind(&CustomRobot::onMqttMessage, this, _1, _2));
        mqttClient_->setConnectionCb(std::bind(&CustomRobot::onMqttConnection, this, _1));
        
        mqttClient_->startMessageProcessor();
        
        if (!mqttClient_->connect(config_.mqtt_username, config_.mqtt_password)) {
            LOG_ERROR("Failed to connect to MQTT broker");
            return false;
        }
        
        std::string controlTopic = config_.topic_prefix + "/" + config_.topic_cmd;
        
        mqttClient_->subscribe(controlTopic, 1);
        
        LOG_INFO("MQTT client initialized successfully");
        return true;
        
    } catch (const std::exception& e) {
        LOG_ERROR("Exception during MQTT initialization: " + std::string(e.what()));
        return false;
    }
}

void CustomRobot::onMqttMessage(const std::string& topic, const std::string& payload) {
    try {
        LOG_INFO("Received MQTT message on topic: " + topic + ", payload: " + payload);

        // nlohmann::json message = nlohmann::json::parse(payload);
        
        // std::string controlTopic = config_.topic_prefix + "/" + config_.topic_cmd;
        
        // if (topic == controlTopic) {
        //     if (message.contains("command")) {
        //         std::string command = message["command"];
                
        //         if (command == "stand" && controller_) {
        //             controller_->standUp();
                    
        //             // Publish status response
        //             nlohmann::json response;
        //             response["status"] = "executed";
        //             response["command"] = command;
        //             response["timestamp"] = std::time(nullptr);
        //             std::string statusTopic = config_.topic_prefix + "/" + config_.topic_state;
        //             mqttClient_->publishJson(statusTopic, response);
                    
        //             // Publish updated status
        //             publishStatus();
                    
        //         } else if (command == "sit" && controller_) {
        //             controller_->sitDown();
                    
        //             nlohmann::json response;
        //             response["status"] = "executed";
        //             response["command"] = command;
        //             response["timestamp"] = std::time(nullptr);
        //             std::string statusTopic = config_.topic_prefix + "/" + config_.topic_state;
        //             mqttClient_->publishJson(statusTopic, response);
                    
        //             // Publish updated status
        //             publishStatus();
        //         }
        //     } else {
        //         // Unknown command, send error response
        //         nlohmann::json response;
        //         response["status"] = "error";
        //         response["message"] = "Unknown command: " + command;
        //         response["timestamp"] = std::time(nullptr);
        //         std::string statusTopic = config_.topic_prefix + "/" + config_.topic_state;
        //         mqttClient_->publishJson(statusTopic, response);
                
        //         // Publish updated status
        //         publishStatus();
        //     }
        // }
        
    } catch (const std::exception& e) {
        LOG_ERROR("Error processing MQTT message: " + std::string(e.what()));
    }
}

void CustomRobot::onMqttConnection(bool connected) {
    if (connected) {
        LOG_INFO("MQTT client connected to broker");
        
        nlohmann::json status;
        status["status"] = "online";
        status["timestamp"] = std::time(nullptr);
        std::string statusTopic = config_.topic_prefix + "/" + config_.topic_state;
        mqttClient_->publishJson(statusTopic, status, 1, true);
        
        publishStatus();
        
    } else {
        LOG_WARN("MQTT client disconnected from broker");
    }
}

void CustomRobot::publishStatus() {
    if (!mqttClient_ || !mqttClient_->isConnected()) {
        return;
    }
    
    try {
        nlohmann::json status;
        status["initialized"] = initialized_.load();
        status["running"] = running_.load();
        status["mqtt_connected"] = mqttClient_->isConnected();
        status["timestamp"] = std::time(nullptr);
        
        std::string statusTopic = config_.topic_prefix + "/" + config_.topic_state;
        mqttClient_->publishJson(statusTopic, status, 1, true);
        
        LOG_DEBUG("Published status to topic: " + statusTopic);
        
    } catch (const std::exception& e) {
        LOG_ERROR("Failed to publish status: " + std::string(e.what()));
    }
}

std::string CustomRobot::generateRandomClientId() const {
    std::string baseId = config_.mqtt_client_id;
    
    // 确保base ID只包含ASCII字符
    std::string safeBaseId;
    for (char c : baseId) {
        if (std::isalnum(c) || c == '_' || c == '-') {
            safeBaseId += c;
        }
    }
    if (safeBaseId.empty()) {
        safeBaseId = "unitree_go2_client";
    }
    
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<> dis(1000, 9999);
    
    std::stringstream ss;
    ss << safeBaseId << "_" << std::time(nullptr) << "_" << dis(gen);
    
    return ss.str();
}

} // namespace custom