// OLEI driver example.
#include "lidarlib/lidar.hpp"
#include <cstdio>

int main() {
    lidarlib::Driver drv(lidarlib::MODEL_VB);

    lidarlib::ErrorCode err = drv.open();
    if (err != lidarlib::ErrorCode::Ok) {
        fprintf(stderr, "Không mở được socket: %s\n", lidarlib::to_string(err));
        return 1;
    }

    // Chờ cảm biến sẵn sàng: đã nhận được ít nhất một scan hoàn chỉnh
    // và thiết bị không báo lỗi (motor/điện áp/nhiệt độ...)
    if (!drv.wait_ready(5000)) {
        fprintf(stderr, "Cảm biến chưa sẵn sàng: %s (diag: %s)\n",
                lidarlib::to_string(drv.last_error()),
                lidarlib::to_string(drv.get_diagnostics()).c_str());
        drv.close();
        return 1;
    }

    for (int i = 0; i < 10; ++i) {
        lidarlib::ScanResult result;
        if (!drv.recv_scan(result, 2000)) {
            fprintf(stderr, "Timeout hoặc lỗi nhận packet\n");
            break;
        }
        const lidarlib::LaserScan& scan = result.scan;
        const lidarlib::ExtraInfo& info = result.info;
        printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X, model=%s\n",
               i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
               info.detected_model.c_str());

        // Chẩn đoán thiết bị từ scan mới nhất
        lidarlib::Diagnostics diag = drv.get_diagnostics();
        printf("  diag: %s\n", lidarlib::to_string(diag).c_str());
        if (diag.has_fault()) {
            if (diag.monitor_fault())     printf("    !! lỗi monitor/motor\n");
            if (diag.voltage_fault())     printf("    !! điện áp bất thường\n");
            if (diag.temperature_fault()) printf("    !! nhiệt độ bất thường\n");
        }

        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();
    return 0;
}

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