// example.cpp — quick try-out of the OLEI LiDAR driver
#include "olei_lidar.hpp"
#include <cstdio>

int main() {
    // ── pick a model ────────────────────────────────────────────────────────
    // olei::Driver drv(olei::MODEL_VF);        // 2D 360°
    // olei::Driver drv(olei::MODEL_LR1F);      // 2D 360°, 50m
    olei::Driver drv(olei::MODEL_VB);           // 2D 270°

    if (!drv.open()) {
        fprintf(stderr, "Không mở được socket\n");
        return 1;
    }

    // ── option 1: blocking recv ─────────────────────────────────────────────
    for (int i = 0; i < 10; ++i) {
        olei::Scan scan;
        if (!drv.recv_scan(scan, 2000)) {
            fprintf(stderr, "Timeout hoặc lỗi nhận packet\n");
            break;
        }
        printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X\n",
               i, scan.points.size(), scan.timestamp_ms, scan.error_status);

        // Print the first few points
        for (size_t j = 0; j < 5 && j < scan.points.size(); ++j) {
            const auto& p = scan.points[j];
            printf("  [%zu] angle=%.2f° dist=%.3fm intensity=%u\n",
                   j, p.angle_deg, p.distance_m, p.intensity);
        }
    }

    // ── option 2: callback (your own loop) ──────────────────────────────────
    // drv.set_scan_callback([](const olei::Scan& scan) {
    //     printf("Got scan: %zu pts\n", scan.points.size());
    // });
    // while (true) drv.spin_once();

    drv.close();
    return 0;
}

// Build:
//   g++ -std=c++17 -O2 -o example example.cpp olei_lidar.cpp