// icm20948.hpp — InvenSense ICM-20948 IMU driver
// Linux /dev/i2c-1 via I2C_RDWR ioctl — no external libs, no libgpiod.
//
// Hardware (THE TRUTH):
//   SDA = GPIO2 (pin 3), SCL = GPIO3 (pin 5)
//   VDD = 3.3V, GND = GND
//   AD0 pin determines I2C address: GND → 0x68, VCC → 0x69
//
// If i2cdetect -y 1 shows nothing:
//   1. Enable I2C: sudo raspi-config → Interfaces → I2C → Yes → reboot
//   2. Check wiring — SDA/SCL must have pull-ups (Pi has built-in 1.8kΩ)
//   3. Verify AD0 pin state to confirm address (0x68 vs 0x69)
//   4. Try slower I2C: add i2c_arm_baudrate=50000 in /boot/config.txt
#pragma once

#include <chrono>
#include <cstdio>
#include <cstring>
#include <initializer_list>
#include <thread>

#include <fcntl.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <sys/ioctl.h>
#include <unistd.h>

namespace icm20948 {

// ---- Register map (Bank 0) -------------------------------------------------
constexpr uint8_t REG_WHO_AM_I    = 0x00;  // should read 0xEA
constexpr uint8_t REG_USER_CTRL   = 0x03;
constexpr uint8_t REG_PWR_MGMT_1  = 0x06;  // bit7=DEVICE_RESET, bits2:0=CLKSEL
constexpr uint8_t REG_PWR_MGMT_2  = 0x07;  // bit5:3=disable_accel, bit2:0=disable_gyro
constexpr uint8_t REG_ACCEL_XOUT_H = 0x2D;
constexpr uint8_t REG_GYRO_XOUT_H  = 0x33;
constexpr uint8_t REG_TEMP_OUT_H   = 0x39;
constexpr uint8_t REG_INT_STATUS   = 0x19;
constexpr uint8_t REG_BANK_SEL    = 0x7F;

// ---- Register map (Bank 2) -------------------------------------------------
constexpr uint8_t B2_GYRO_SMPLRT_DIV  = 0x00;
constexpr uint8_t B2_GYRO_CONFIG_1    = 0x01; // bits3:2=FS_SEL bits1:0=DLPF
constexpr uint8_t B2_ACCEL_SMPLRT_DIV_1 = 0x10;
constexpr uint8_t B2_ACCEL_SMPLRT_DIV_2 = 0x11;
constexpr uint8_t B2_ACCEL_CONFIG      = 0x14; // bits3:2=FS_SEL bits1:0=DLPF

constexpr uint8_t WHO_AM_I_VAL = 0xEA;

// ---- Scale configuration ---------------------------------------------------
// Accel full-scale: 0=±2g  1=±4g  2=±8g  3=±16g
// Gyro  full-scale: 0=±250 1=±500 2=±1000 3=±2000 dps
constexpr float ACCEL_SCALE[4] = { 2.0f/32768, 4.0f/32768, 8.0f/32768, 16.0f/32768 };
constexpr float GYRO_SCALE[4]  = { 250.0f/32768, 500.0f/32768, 1000.0f/32768, 2000.0f/32768 };

// Raw 16-bit sensor sample
struct RawSample {
    int16_t ax, ay, az;  // accelerometer
    int16_t gx, gy, gz;  // gyroscope
    int16_t temp_raw;     // temperature raw
};

// Scaled sample with physical units
struct Sample {
    float ax, ay, az;   // g
    float gx, gy, gz;   // deg/s
    float temp_c;        // Celsius
};

struct Config {
    const char *i2c_path   = "/dev/i2c-1";
    uint8_t     accel_fs   = 0;  // 0=±2g, 1=±4g, 2=±8g, 3=±16g
    uint8_t     gyro_fs    = 0;  // 0=±250dps, 1=±500, 2=±1000, 3=±2000
    bool        verbose    = false;
};

class Imu {
public:
    bool begin(const Config &cfg);
    void end();

    // Returns true when new data is read.
    bool read(RawSample &raw);
    bool read(Sample &s);

    // Returns the detected I2C address (0x68 or 0x69), or 0 if not found.
    uint8_t address() const { return addr_; }

    // Direct register access (for debugging / bank switching experiments).
    bool    writeReg(uint8_t reg, uint8_t val);
    uint8_t readReg(uint8_t reg);
    bool    readRegs(uint8_t reg, uint8_t *buf, uint8_t n);
    bool    selectBank(uint8_t bank);  // 0–3

private:
    Config  cfg_{};
    int     fd_   = -1;
    uint8_t addr_ = 0;
    uint8_t accel_fs_ = 0;
    uint8_t gyro_fs_  = 0;
};

// ---- Implementation ---------------------------------------------------------

inline bool Imu::writeReg(uint8_t reg, uint8_t val)
{
    uint8_t buf[2] = { reg, val };
    i2c_msg msg{};
    msg.addr  = addr_;
    msg.flags = 0;
    msg.len   = 2;
    msg.buf   = buf;
    i2c_rdwr_ioctl_data io{};
    io.msgs  = &msg;
    io.nmsgs = 1;
    return ioctl(fd_, I2C_RDWR, &io) == 1;
}

inline bool Imu::readRegs(uint8_t reg, uint8_t *buf, uint8_t n)
{
    // Generates proper repeated-start: [START|ADDR+W|REG|RSTART|ADDR+R|DATA|STOP]
    i2c_msg msgs[2]{};
    msgs[0].addr  = addr_;
    msgs[0].flags = 0;
    msgs[0].len   = 1;
    msgs[0].buf   = &reg;
    msgs[1].addr  = addr_;
    msgs[1].flags = I2C_M_RD;
    msgs[1].len   = n;
    msgs[1].buf   = buf;
    i2c_rdwr_ioctl_data io{};
    io.msgs  = msgs;
    io.nmsgs = 2;
    return ioctl(fd_, I2C_RDWR, &io) == 2;
}

inline uint8_t Imu::readReg(uint8_t reg)
{
    uint8_t v = 0;
    readRegs(reg, &v, 1);
    return v;
}

inline bool Imu::selectBank(uint8_t bank)
{
    return writeReg(REG_BANK_SEL, (uint8_t)(bank << 4));
}

inline bool Imu::begin(const Config &c)
{
    cfg_ = c;

    fd_ = ::open(c.i2c_path, O_RDWR);
    if (fd_ < 0) {
        if (c.verbose)
            std::fprintf(stderr, "[icm20948] cannot open %s: %m\n", c.i2c_path);
        return false;
    }

    // Auto-detect I2C address (AD0 low → 0x68, AD0 high → 0x69)
    bool found = false;
    for (uint8_t a : {(uint8_t)0x68, (uint8_t)0x69}) {
        addr_ = a;
        // Ensure bank 0 before reading WHO_AM_I
        writeReg(REG_BANK_SEL, 0x00);
        uint8_t id = readReg(REG_WHO_AM_I);
        if (c.verbose)
            std::fprintf(stderr, "[icm20948] probe 0x%02X → WHO_AM_I = 0x%02X\n", a, id);
        if (id == WHO_AM_I_VAL) { found = true; break; }
    }
    if (!found) {
        if (c.verbose)
            std::fprintf(stderr, "[icm20948] chip not found on %s at 0x68 or 0x69\n"
                                 "           check wiring, I2C enabled, pull-ups\n",
                         c.i2c_path);
        ::close(fd_); fd_ = -1;
        return false;
    }
    if (c.verbose)
        std::fprintf(stderr, "[icm20948] found at 0x%02X\n", addr_);

    // Device reset — sets all registers to default values.
    selectBank(0);
    writeReg(REG_PWR_MGMT_1, 0x80);  // DEVICE_RESET bit
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    // Wait until reset is complete (DEVICE_RESET bit self-clears).
    for (int i = 0; i < 50; ++i) {
        if (!(readReg(REG_PWR_MGMT_1) & 0x80)) break;
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    // Verify still alive after reset.
    if (readReg(REG_WHO_AM_I) != WHO_AM_I_VAL) {
        if (c.verbose)
            std::fprintf(stderr, "[icm20948] WHO_AM_I mismatch after reset\n");
        ::close(fd_); fd_ = -1;
        return false;
    }

    // Wake up: auto-select clock source (best practice per datasheet).
    writeReg(REG_PWR_MGMT_1, 0x01);
    std::this_thread::sleep_for(std::chrono::milliseconds(30));

    // Enable all accel + gyro axes.
    writeReg(REG_PWR_MGMT_2, 0x00);

    // Configure full-scale in Bank 2.
    accel_fs_ = c.accel_fs & 0x03;
    gyro_fs_  = c.gyro_fs  & 0x03;
    selectBank(2);
    writeReg(B2_ACCEL_CONFIG, (uint8_t)(accel_fs_ << 2));
    writeReg(B2_GYRO_CONFIG_1, (uint8_t)(gyro_fs_ << 2));

    selectBank(0);  // leave in bank 0 for normal operation

    if (c.verbose) {
        static const unsigned gyro_dps[] = { 250u, 500u, 1000u, 2000u };
        std::fprintf(stderr, "[icm20948] init OK: accel ±%ug gyro ±%udps\n",
                     (unsigned)(2u << accel_fs_), gyro_dps[gyro_fs_]);
    }
    return true;
}

inline void Imu::end()
{
    if (fd_ >= 0) { ::close(fd_); fd_ = -1; }
}

inline bool Imu::read(RawSample &raw)
{
    selectBank(0);
    uint8_t buf[14]{};
    if (!readRegs(REG_ACCEL_XOUT_H, buf, 14)) return false;

    auto s16 = [](uint8_t hi, uint8_t lo) -> int16_t {
        return (int16_t)((uint16_t)hi << 8 | lo);
    };
    raw.ax       = s16(buf[0],  buf[1]);
    raw.ay       = s16(buf[2],  buf[3]);
    raw.az       = s16(buf[4],  buf[5]);
    raw.temp_raw = s16(buf[6],  buf[7]);
    raw.gx       = s16(buf[8],  buf[9]);
    raw.gy       = s16(buf[10], buf[11]);
    raw.gz       = s16(buf[12], buf[13]);
    return true;
}

inline bool Imu::read(Sample &s)
{
    RawSample raw{};
    if (!read(raw)) return false;
    float as = ACCEL_SCALE[accel_fs_];
    float gs = GYRO_SCALE[gyro_fs_];
    s.ax = raw.ax * as;
    s.ay = raw.ay * as;
    s.az = raw.az * as;
    s.gx = raw.gx * gs;
    s.gy = raw.gy * gs;
    s.gz = raw.gz * gs;
    // Temp formula from ICM-20948 datasheet: T°C = (raw - 0) / 333.87 + 21.0
    s.temp_c = (float)raw.temp_raw / 333.87f + 21.0f;
    return true;
}

} // namespace icm20948