#include <cstdio>
#include <csignal>
#include <atomic>
#include <unistd.h>
#include "vl53l0x.h"

static std::atomic<bool> running(true);
static void on_signal(int) { running.store(false); }

int main()
{
    setvbuf(stdout, NULL, _IONBF, 0);
    std::signal(SIGINT, on_signal);
    std::signal(SIGTERM, on_signal);

    VL53L0X sensor;
    if (!sensor.init()) {
        fprintf(stderr, "VL53L0X init failed\n");
        return 1;
    }

    sensor.startMeasure();

    printf("lidar_test running, Ctrl+C to stop\n");
    printf("%-8s %-8s %-10s\n", "cnt", "range_mm", "status");

    int count = 0;
    VL53L0X_RangingMeasurementData_t data;

    while (running.load()) {
        if (!sensor.readResult(data)) {
            printf("%-8d %-8s %-10s\n", ++count, "ERR", "timeout");
            sensor.startMeasure();
            usleep(50000);
            continue;
        }

        int range = data.RangeMilliMeter;
        const char* st = (data.RangeStatus == 0) ? "OK" : "SIGMA";
        printf("%-8d %-8d %-10s\n", ++count, range, st);

        sensor.startMeasure();
        usleep(30000);
    }

    sensor.stop();
    printf("stopped\n");
    return 0;
}