// Open one lidar through the manager and read 10 scans.
//   ./example <driver_id> [plugins_dir]
//   ./example olei_lidar_driver
//   ./example rplidar_c1_driver
#include "lidar_manager.hpp"
#include <cstdio>

int main(int argc, char** argv) {
    if (argc < 2) {
        fprintf(stderr, "usage: %s <driver_id> [plugins_dir]\n", argv[0]);
        return 1;
    }
    const std::string plugins_dir = (argc > 2) ? argv[2] : "plugins";

    xlidar::LidarManager manager(plugins_dir);
    manager.load_all_plugins();

    xlidar::DeviceConfig cfg;
    cfg.name      = "demo";
    cfg.driver_id = argv[1];
    // Network drivers use cfg.ip / cfg.port (0 = driver default port);
    // the rplidar driver uses cfg.serial_port / cfg.baudrate instead.

    auto lidar = manager.create_lidar_device(cfg);
    if (!lidar) return 1;

    xlidar::DriverInfo info = lidar->get_driver_info();
    printf("driver: %s (%s %s)\n", info.driver_id.c_str(), info.vendor.c_str(),
           info.model.c_str());

    xlidar::ErrorCode err = lidar->open();
    if (err != xlidar::ErrorCode::Ok) {
        fprintf(stderr, "open failed: %s\n", xlidar::to_string(err));
        return 1;
    }

    // Wait until the sensor is usable: at least one complete scan decoded
    // and the device reports no fault (motor/voltage/pollution/...).
    if (!lidar->wait_ready(5000)) {
        fprintf(stderr, "sensor not ready: %s (diag: %s)\n",
                xlidar::to_string(lidar->last_error()),
                xlidar::to_string(lidar->get_diagnostics()).c_str());
        lidar->close();
        return 1;
    }

    for (int i = 0; i < 10; ++i) {
        xlidar::ScanResult result;
        if (!lidar->recv_scan(result, 2000)) {
            fprintf(stderr, "recv_scan failed: %s\n", xlidar::to_string(lidar->last_error()));
            break;
        }
        const xlidar::LaserScan& scan = result.scan;
        printf("scan #%d: %zu points, ts=%u ms, model=%s\n",
               i, scan.ranges.size(), scan.timestamp_ms,
               result.info.detected_model.c_str());

        // Device self-diagnostics from the newest scan.
        xlidar::Diagnostics diag = lidar->get_diagnostics();
        printf("  diag: %s\n", xlidar::to_string(diag).c_str());

        for (size_t j = 0; j < 5 && 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]);
        }
    }

    lidar->close();
    return 0;
}