#include "sync_board.hpp"
#include <iostream>
#include <thread>

int main() {
    SyncBoard board;
    if (!board.connect("/dev/ttyACM0", 921600)) {
        std::cerr << "Failed to connect" << std::endl;
        return -1;
    }

    board.read_highres_imu();     // 注册IMU回调
    board.read_system_time();     // 注册系统时间回调
    board.read_camera_trigger();  // 注册相机触发回调
    board.start_recv_message();   // 启动消息接收线程

    while (1)
    {
        ImuData imu;
        // if (board.getLatestImu(imu)) {
        //     std::cout << "[IMU] Timestamp: " << imu.timestamp_us << " us, "
        //               << "Acc: (" << imu.ax << ", " << imu.ay << ", " << imu.az << ") "
        //               << "Gyro: (" << imu.gx << ", " << imu.gy << ", " << imu.gz << ")" << std::endl;
        // }
        uint64_t cam_ts;
        if (board.pop_timestamp(cam_ts)) {
            std::cout << "[pop Trigger] Timestamp: " << cam_ts << " us" << std::endl;
        }
        // std::cout << "[camera trigger] Timestamp: " << board.cam_trigger_time_us << " us" << std::endl;
        
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    
    board.stop_recv_message();
    board.disconnect();
    return 0;
}
