// imu_test.cpp — ICM-20948 IMU test
// Usage: sudo ./imu_test [-v] [-r] [-n COUNT]
//   -v        verbose init debug
//   -r        print raw 16-bit values instead of scaled
//   -n COUNT  exit after COUNT samples (default: run forever)
//
// If chip not found:
//   sudo i2cdetect -y 1   ← check 0x68 or 0x69 appears
//   If nothing shows: check wiring, I2C enabled in raspi-config
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <thread>
#include <chrono>
#include "icm20948.hpp"

int main(int argc, char **argv)
{
    icm20948::Config cfg;
    cfg.verbose  = false;
    bool raw_mode = false;
    int  count    = -1;

    for (int i = 1; i < argc; ++i) {
        if (std::strcmp(argv[i], "-v") == 0) {
            cfg.verbose = true;
        } else if (std::strcmp(argv[i], "-r") == 0) {
            raw_mode = true;
        } else if (std::strcmp(argv[i], "-n") == 0 && i + 1 < argc) {
            count = std::atoi(argv[++i]);
        }
    }

    icm20948::Imu imu;
    if (!imu.begin(cfg)) {
        std::fprintf(stderr, "ERROR: IMU init failed\n"
                             "  Check: I2C enabled? (sudo raspi-config)\n"
                             "  Check: sudo i2cdetect -y 1  (expect 68 or 69)\n"
                             "  Check: wiring SDA=GPIO2 SCL=GPIO3, 3.3V, GND\n"
                             "  Check: AD0 pin → 0x68 (GND) or 0x69 (VCC)\n"
                             "  WHO_AM_I=0x00 usually means NACK (no chip) — run with -v\n");
        return 1;
    }

    std::printf("IMU OK at 0x%02X — reading at 10 Hz%s\n\n",
                imu.address(), raw_mode ? " (raw mode)" : "");

    for (int n = 0; count < 0 || n < count; ++n) {
        if (raw_mode) {
            icm20948::RawSample raw{};
            if (!imu.read(raw)) { std::fprintf(stderr, "read error\n"); break; }
            std::printf("ax=%6d ay=%6d az=%6d | gx=%6d gy=%6d gz=%6d | t_raw=%6d\n",
                        raw.ax, raw.ay, raw.az, raw.gx, raw.gy, raw.gz, raw.temp_raw);
        } else {
            icm20948::Sample s{};
            if (!imu.read(s)) { std::fprintf(stderr, "read error\n"); break; }
            std::printf("a[g]  %+7.3f %+7.3f %+7.3f | "
                        "g[°/s] %+8.2f %+8.2f %+8.2f | "
                        "T %.1f°C\n",
                        s.ax, s.ay, s.az,
                        s.gx, s.gy, s.gz,
                        s.temp_c);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    imu.end();
    return 0;
}