#include <iostream>
#include <memory>
#include <signal.h>
#include <thread>
#include <chrono>
#include "custom_robot.hpp"
#include "logger.hpp"
#include "config.hpp"

using namespace custom;

std::unique_ptr<CustomRobot> g_robot;

void signalHandler(int signal) {
    LOG_INFO("Received signal " + std::to_string(signal) + ", shutting down...");
    exit(0);
}

int main(int argc, char** argv) {
    Logger::getInstance().setLevel(LogLevel::INFO);
    
    LOG_INFO("Starting Unitree GO2 System v0.0.2");
    
    signal(SIGINT, signalHandler);
    signal(SIGTERM, signalHandler);
    
    try {
        g_robot = std::make_unique<CustomRobot>();
        
        if (!g_robot->initialize()) {
            LOG_ERROR("Failed to initialize robot");
            return 1;
        }
        
        LOG_INFO("Robot initialized successfully");
        g_robot->start();
        
        LOG_INFO("System is running. Press Ctrl+C to stop.");
        
        while (true) {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        
    } catch (const std::exception& e) {
        LOG_ERROR("Exception: " + std::string(e.what()));
        return 1;
    } catch (...) {
        LOG_ERROR("Unknown exception occurred");
        return 1;
    }
    
    return 0;
}