/*
 * mag.c — Auto-detect magnetometer on I2C1
 *
 * Probes for QMC5883L (0x0D), HMC5883L (0x1E), IST8310 (0x0E) in that order.
 * Returns detected type; mag_read_heading() returns heading in degrees×10.
 *
 * Requires i2c1_init() called before mag_init().
 */
#include "mag.h"
#include "i2c1.h"
#include <math.h>

/* ------------------------------------------------------------------ */
/* I2C helpers (7-bit addr left-shifted for HAL)                       */
/* ------------------------------------------------------------------ */
static uint16_t s_addr;
static mag_type_t s_type = MAG_NONE;

static uint8_t mag_rreg(uint8_t reg) {
    uint8_t v = 0;
    HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &v, 1, 50);
    return v;
}
static void mag_wreg(uint8_t reg, uint8_t val) {
    HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 50);
}
static void mag_rregs(uint8_t reg, uint8_t *buf, uint8_t len) {
    HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 50);
}

/* ------------------------------------------------------------------ */
/* QMC5883L — QUST QMC5883L (0x0D)                                    */
/*   chip_id reg 0x0D = 0xFF                                           */
/*   Data regs 0x00-0x05: XL XH YL YH ZL ZH                          */
/* ------------------------------------------------------------------ */
static int qmc_init(void) {
    s_addr = 0x0D << 1;
    uint8_t id = mag_rreg(0x0D);
    if (id != 0xFF) return 0;

    /* Reset */
    mag_wreg(0x0B, 0x01);   /* SET/RESET period = 0x01 */
    HAL_Delay(10);

    /* Control 1: continuous mode (0x01), 200 Hz ODR (0x0C), 512 OSR (0x00), 2G (0x00) */
    mag_wreg(0x09, 0x0D);   /* MODE=cont, ODR=200Hz, RNG=2G, OSR=512 */
    HAL_Delay(10);
    return 1;
}

static int16_t qmc_heading(void) {
    /* Status reg 0x06, bit0=DRDY */
    if (!(mag_rreg(0x06) & 0x01)) return -1;

    uint8_t buf[6];
    mag_rregs(0x00, buf, 6);
    int16_t x = (int16_t)((buf[1] << 8) | buf[0]);
    int16_t y = (int16_t)((buf[3] << 8) | buf[2]);

    float heading_deg = atan2f((float)y, (float)x) * (180.0f / 3.14159265f);
    if (heading_deg < 0.0f) heading_deg += 360.0f;
    return (int16_t)(heading_deg * 10.0f);
}

/* ------------------------------------------------------------------ */
/* HMC5883L — Honeywell HMC5883L (0x1E)                               */
/*   ID regs 0x0A='H', 0x0B='4', 0x0C='3'                            */
/*   Data regs 0x03-0x08: XH XL ZH ZL YH YL (note XZY order!)       */
/* ------------------------------------------------------------------ */
static int hmc_init(void) {
    s_addr = 0x1E << 1;
    uint8_t a = mag_rreg(0x0A);
    uint8_t b = mag_rreg(0x0B);
    uint8_t c = mag_rreg(0x0C);
    if (a != 'H' || b != '4' || c != '3') return 0;

    mag_wreg(0x00, 0x70);   /* Config A: 8 samples/output, 15 Hz, normal */
    mag_wreg(0x01, 0x20);   /* Config B: gain=1090 LSB/Ga */
    mag_wreg(0x02, 0x00);   /* Mode: continuous measurement */
    HAL_Delay(10);
    return 1;
}

static int16_t hmc_heading(void) {
    /* Status reg 0x09, bit0=RDY */
    if (!(mag_rreg(0x09) & 0x01)) return -1;

    uint8_t buf[6];
    mag_rregs(0x03, buf, 6);
    int16_t x = (int16_t)((buf[0] << 8) | buf[1]);
    int16_t y = (int16_t)((buf[4] << 8) | buf[5]);  /* HMC order: XZY */

    float heading_deg = atan2f((float)y, (float)x) * (180.0f / 3.14159265f);
    if (heading_deg < 0.0f) heading_deg += 360.0f;
    return (int16_t)(heading_deg * 10.0f);
}

/* ------------------------------------------------------------------ */
/* IST8310 — iSentek IST8310 (0x0E)                                   */
/*   WHO_AM_I reg 0x00 = 0x10                                          */
/*   Data regs 0x03-0x08: XL XH YL YH ZL ZH                          */
/* ------------------------------------------------------------------ */
static int ist_init(void) {
    s_addr = 0x0E << 1;
    uint8_t id = mag_rreg(0x00);
    if (id != 0x10) return 0;

    /* Soft reset via CNTL2 reg 0x0B bit[0] */
    mag_wreg(0x0B, 0x01);
    HAL_Delay(20);

    /* CNTL1 reg 0x0A: single measurement mode, trigger on write */
    /* Use continuous mode bit[2]=1 if supported — trigger manually instead */
    mag_wreg(0x0A, 0x01);   /* single measurement */
    HAL_Delay(10);
    return 1;
}

static int16_t ist_heading(void) {
    /* Trigger a measurement */
    mag_wreg(0x0A, 0x01);
    HAL_Delay(10);  /* IST8310 needs ~6.6ms for single measurement */

    /* STAT1 reg 0x02 bit0=DRDY */
    if (!(mag_rreg(0x02) & 0x01)) return -1;

    uint8_t buf[6];
    mag_rregs(0x03, buf, 6);
    int16_t x = (int16_t)((buf[1] << 8) | buf[0]);
    int16_t y = (int16_t)((buf[3] << 8) | buf[2]);

    float heading_deg = atan2f((float)y, (float)x) * (180.0f / 3.14159265f);
    if (heading_deg < 0.0f) heading_deg += 360.0f;
    return (int16_t)(heading_deg * 10.0f);
}

/* ------------------------------------------------------------------ */
/* Public API                                                          */
/* ------------------------------------------------------------------ */
mag_type_t mag_init(void) {
    if (qmc_init()) { s_type = MAG_QMC5883L; return s_type; }
    if (hmc_init()) { s_type = MAG_HMC5883L; return s_type; }
    if (ist_init()) { s_type = MAG_IST8310;  return s_type; }
    s_type = MAG_NONE;
    return MAG_NONE;
}

int16_t mag_read_heading(void) {
    switch (s_type) {
        case MAG_QMC5883L: return qmc_heading();
        case MAG_HMC5883L: return hmc_heading();
        case MAG_IST8310:  return ist_heading();
        default:           return -1;
    }
}