#include <iostream>
#include <memory>
#include <signal.h>
#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...");
    if (g_robot) {
        g_robot->stop();
    }
    exit(0);
}

int main(int argc, char** argv) {
    // Initialize logger with default settings
    Logger::getInstance().setLevel(LogLevel::INFO);
    
    LOG_INFO("Starting Unitree GO2 System v0.0.1");
    
    // Setup signal handlers
    signal(SIGINT, signalHandler);
    signal(SIGTERM, signalHandler);
    
    try {
        // Create robot with compile-time defaults (no config file needed)
        g_robot = std::make_unique<CustomRobot>("");
        
        if (!g_robot->initialize()) {
            LOG_ERROR("Failed to initialize robot");
            return 1;
        }
        
        LOG_INFO("Robot initialized successfully");
        LOG_INFO("Press Ctrl+C to stop the robot");
        
        // Run the robot
        g_robot->run();
        
    } catch (const std::exception& e) {
        LOG_ERROR("Exception: " + std::string(e.what()));
        return 1;
    } catch (...) {
        LOG_ERROR("Unknown exception occurred");
        return 1;
    }
    
    return 0;
}