main.cpp

#include <iostream>
#include <thread>
#include <atomic>
#include <chrono>
#include <iomanip>
#include <csignal>
#include <sstream>

#include "BNO080.hpp"
#include "RosBagWriter.hpp"

// Global signal flag
std::atomic<bool> running(true);

void signal_handler(int signum) {
    running = false;
}

int main() {
    // Setup signal handler
    std::signal(SIGINT, signal_handler);

    // Initialise RosBag Writer
    RosBagWriter bag;
    
    // Create filename with timestamp
    auto now = std::chrono::system_clock::now();
    std::time_t now_c = std::chrono::system_clock::to_time_t(now);
    std::stringstream ss;
    ss << "imu_data_" << std::put_time(std::localtime(&now_c), "%Y%m%d_%H%M%S") << ".bag";
    std::string filename = ss.str();
    
    if (!bag.open(filename)) {
        std::cerr << "Failed to open " << filename << " for writing." << std::endl;
        return 1;
    }
    
    std::cout << "Writing to ROS bag: " << filename << std::endl;

    // Initialize I2C
    BNO080 imu;
    if (!imu.begin(1, 0x4A)) { // bus 1, address 0x4A
        std::cerr << "Failed to initialize BNO080. Check wiring and permissions." << std::endl;
        bag.close();
        return 1;
    }

    // Enable sensors
    // Report intervals: 400Hz accel/gyro ideal for Kalibr (2500us)
    // Mag at 100Hz (10000us)
    imu.enableReport(SENSOR_REPORTID_ACCELEROMETER, 2500); 
    imu.enableReport(SENSOR_REPORTID_GYROSCOPE, 2500);
    imu.enableReport(SENSOR_REPORTID_MAGNETIC_FIELD, 10000);

    std::cout << "Sensors enabled. Starting capture loop... (Press Ctrl+C to stop)" << std::endl;

    IMUData data;
    long sample_count = 0;
    auto start_time = std::chrono::steady_clock::now();

    while (running) {
        // Read available packets
        // update() calls readPacket and parseInputReport internally
        if (imu.update()) {
            imu.getLatestData(data);
            
            // Check if we have fresh data to write
            // The current BNO080 class is simple state holding.
            
            if (data.has_accel && data.has_gyro) {
                // Use system time for message timestamp or use sensor timestamp if we had it.
                // Using system clock is standard for drivers without hw sync PPS.
                auto timestamp = std::chrono::high_resolution_clock::now();
                uint64_t timestamp_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(timestamp.time_since_epoch()).count();
                
                bag.writeImu(timestamp_ns, 
                             data.accel[0], data.accel[1], data.accel[2],
                             data.gyro[0], data.gyro[1], data.gyro[2],
                             0, 0, 0, 1.0); // Orientation identity/unknown for now
                
                // Clear flags to wait for next update (simple sync latch)
                imu.clearFlags(); 
                
                sample_count++;
                
                // Stats every 1s
                auto now_s = std::chrono::steady_clock::now();
                if (std::chrono::duration_cast<std::chrono::seconds>(now_s - start_time).count() >= 1) {
                    std::cout << "Rate: " << sample_count << " Hz\r" << std::flush;
                    sample_count = 0;
                    start_time = now_s;
                }
            }
        }
    }

    std::cout << "\nStopping..." << std::endl;
    bag.close();
    
    return 0;
}