// Test 2 Olei lidars (front + rear) concurrently.
#include "lidarlib/lidar.hpp"
#include <cstdio>
#include <thread>

static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg,
                       const std::string& local_ip, uint16_t port, bool inverted, int n_scans) {
    lidarlib::Driver drv(cfg, local_ip, port, inverted);
    lidarlib::ErrorCode err = drv.open();
    if (err != lidarlib::ErrorCode::Ok) {
        fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (%s)\n",
                tag, local_ip.c_str(), port, lidarlib::to_string(err));
        return;
    }
    printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port);

    for (int i = 0; i < n_scans; ++i) {
        lidarlib::ScanResult result;
        if (!drv.recv_scan(result, 2000)) {
            fprintf(stderr, "[%s] Timeout/loi nhan packet (scan #%d)\n", tag, i);
            continue;
        }
        const lidarlib::LaserScan& scan = result.scan;
        const lidarlib::ExtraInfo& info = result.info;
        printf("[%s] Scan #%d: %zu diem, ts=%u ms, err=0x%02X, model=%s\n",
               tag, i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
               info.detected_model.c_str());
        for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
            float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
            printf("       [%zu] angle=%.2f dist=%.3fm intensity=%.0f\n",
                   j, angle_deg, scan.ranges[j], scan.intensities[j]);
        }
    }
    drv.close();
}

int main() {
    // Real headers (UDP sniff): front = "OLELR-1BS2", rear = "OLELR-1BS5".
    std::thread t_front(run_lidar, "front/scan_1", lidarlib::MODEL_AUTO,
                         "192.168.100.100", 2368, false, 5);
    std::thread t_rear(run_lidar, "rear/scan_2", lidarlib::MODEL_AUTO,
                        "192.168.100.100", 2369, true, 5);

    t_front.join();
    t_rear.join();
    return 0;
}