/*
 * battery_adc.c — DMA-based battery voltage/current ADC driver (Issue #533)
 *
 * Voltage divider calibration, DMA-based continuous sampling, IIR low-pass
 * filter, and USART telemetry publish to Jetson via jlink (JLINK_TLM_BATTERY).
 * Integrates with power_mgmt for low-battery sleep (Issue #467).
 *
 * DMA mapping (STM32F7 RM Table 27):
 *   DMA2_Stream0 Channel2 → ADC3  (no conflict: Stream2/Ch4 = USART1_RX)
 *
 * ADC3 scan sequence:
 *   Rank 1: IN11 (PC1) — Vbat
 *   Rank 2: IN13 (PC3) — Ibat
 *   Continuous mode, 480-cycle sampling → ~14 kSPS per channel @ 13.5 MHz ADC
 *
 * DMA circular buffer layout (8 × uint16_t, pairs repeat continuously):
 *   [vbat0, ibat0, vbat1, ibat1, vbat2, ibat2, vbat3, ibat3]
 *
 * On each battery_adc_tick() call the 4 Vbat and 4 Ibat samples are averaged
 * (4× hardware oversampling) then fed into an IIR low-pass filter.
 */

#include "battery_adc.h"
#include "jlink.h"
#include "power_mgmt.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>

/* ---- DMA buffer ---- */
#define ADC_DMA_BUF_LEN     8u      /* 4 Vbat/Ibat pairs */
#define ADC_OVERSAMPLE      4u      /* pairs per tick average */

static volatile uint16_t s_dma_buf[ADC_DMA_BUF_LEN];

/* ---- HAL handles ---- */
static ADC_HandleTypeDef s_hadc;
static DMA_HandleTypeDef s_hdma;

/* ---- Filtered state (Q0 integer, units: raw ADC counts × 8 for sub-LSB) ---- */
/*
 * IIR accumulator: s_lpf_vbat = filtered Vbat in (raw_counts × 8).
 * Multiply by 8 gives 3 bits of sub-LSB precision with integer arithmetic.
 * On each tick: s_lpf_vbat += (raw_avg - (s_lpf_vbat >> 3)) >> LPF_SHIFT
 * Simplified: s_lpf_vbat8 = s_lpf_vbat8 + (raw_avg_x8 - s_lpf_vbat8) >> SHIFT
 */
static uint32_t s_lpf_vbat8 = 0;   /* Vbat accumulator × 8 */
static int32_t  s_lpf_ibat8 = 0;   /* Ibat accumulator × 8 (signed) */

/* ---- Calibrated outputs (updated each tick) ---- */
static uint32_t s_vbat_mv      = 0;   /* calibrated LPF Vbat (mV) */
static int32_t  s_ibat_ma      = 0;   /* calibrated LPF Ibat (mA) */
static uint32_t s_vbat_raw_mv  = 0;   /* unfiltered last-tick average (mV) */

/* ---- Calibration ---- */
static battery_adc_cal_t s_cal = {
    .vbat_offset_mv = 0,
    .ibat_offset_ma = 0,
    .vbat_scale_num = 0,   /* 0 = use VBAT_SCALE_NUM from config.h */
    .vbat_scale_den = 0,   /* 0 = use 1 */
};

/* ---- Low-voltage tracking ---- */
static uint32_t s_low_since_ms   = 0;    /* tick when Vbat first went low     */
static bool     s_low_active     = false; /* currently below LOW_MV threshold  */
static bool     s_critical_notified = false; /* pm notified for this event     */
static uint32_t s_low_mv         = BATTERY_ADC_LOW_MV;
static uint32_t s_critical_mv    = BATTERY_ADC_CRITICAL_MV;

/* ---- Ready flag ---- */
static bool s_ready = false;

/* ---- Telemetry rate limiting ---- */
static uint32_t s_last_publish_ms  = 0;
static bool     s_published_once   = false;  /* force send on first call */

/* ---- Helper: convert raw ADC count to mV ---- */
/*
 * Apply voltage divider and calibration:
 *   vbat_mv = (raw × Vref_mV × scale_num) / (4096 × scale_den)
 *           + cal.vbat_offset_mv
 * Default: Vref=3300 mV, scale_num=VBAT_SCALE_NUM=11, scale_den=1.
 */
static uint32_t raw_to_vbat_mv(uint32_t raw_counts)
{
    uint16_t snum = (s_cal.vbat_scale_num != 0u) ? s_cal.vbat_scale_num
                                                  : (uint16_t)VBAT_SCALE_NUM;
    uint16_t sden = (s_cal.vbat_scale_den != 0u) ? s_cal.vbat_scale_den : 1u;

    /* raw_counts × (3300 × 11) / 4096 — keep 32-bit intermediate */
    uint32_t mv = (raw_counts * (uint32_t)VBAT_AREF_MV * snum) /
                  ((1u << VBAT_ADC_BITS) * sden);

    int32_t cal_mv = (int32_t)mv + (int32_t)s_cal.vbat_offset_mv;
    if (cal_mv < 0) cal_mv = 0;
    return (uint32_t)cal_mv;
}

/* ---- Helper: convert raw ADC count to mA ---- */
/*
 * ADC_IBAT_SCALE = 115 (betaflight ibata_scale units: mA per count × 10).
 * Formula: ibat_ma = raw_counts × ADC_IBAT_SCALE / 10 + cal.ibat_offset_ma
 * Betaflight ibata_scale is defined as: current = raw * scale / (4096 * 10)
 * in units of 100mA per count → ibat_ma = raw × ibata_scale * 100 / 4096
 *
 * With ibata_scale=115: ibat_ma = raw × 115 × 100 / 4096
 *                               ≈ raw × 2.808 mA/count
 * For raw=4095 (full-scale): ~11,500 mA (11.5 A) — appropriate for 3S robot.
 */
static int32_t raw_to_ibat_ma(uint32_t raw_counts)
{
    int32_t ma = (int32_t)((raw_counts * (uint32_t)ADC_IBAT_SCALE * 100u) /
                           (1u << VBAT_ADC_BITS));
    ma += (int32_t)s_cal.ibat_offset_ma;
    return ma;
}

/* ---- battery_adc_init() ---- */
void battery_adc_init(void)
{
    /* ---- GPIO: PC1 (Vbat) and PC3 (Ibat) as analog input ---- */
    __HAL_RCC_GPIOC_CLK_ENABLE();
    GPIO_InitTypeDef gpio = {0};
    gpio.Pin  = GPIO_PIN_1 | GPIO_PIN_3;
    gpio.Mode = GPIO_MODE_ANALOG;
    gpio.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOC, &gpio);

    /* ---- DMA2_Stream0 Channel2 → ADC3 (circular, 16-bit, 8 words) ---- */
    __HAL_RCC_DMA2_CLK_ENABLE();
    s_hdma.Instance                 = DMA2_Stream0;
    s_hdma.Init.Channel             = DMA_CHANNEL_2;
    s_hdma.Init.Direction           = DMA_PERIPH_TO_MEMORY;
    s_hdma.Init.PeriphInc           = DMA_PINC_DISABLE;
    s_hdma.Init.MemInc              = DMA_MINC_ENABLE;
    s_hdma.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
    s_hdma.Init.MemDataAlignment    = DMA_MDATAALIGN_HALFWORD;
    s_hdma.Init.Mode                = DMA_CIRCULAR;
    s_hdma.Init.Priority            = DMA_PRIORITY_LOW;
    s_hdma.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
    if (HAL_DMA_Init(&s_hdma) != HAL_OK) return;

    /* ---- ADC3: continuous scan, DMA, 12-bit, 2 channels ---- */
    __HAL_RCC_ADC3_CLK_ENABLE();
    s_hadc.Instance                   = ADC3;
    s_hadc.Init.ClockPrescaler        = ADC_CLOCK_SYNC_PCLK_DIV8;  /* 108/8 ≈ 13.5 MHz */
    s_hadc.Init.Resolution            = ADC_RESOLUTION_12B;
    s_hadc.Init.ScanConvMode          = ENABLE;          /* scan both channels */
    s_hadc.Init.ContinuousConvMode    = ENABLE;          /* free-running       */
    s_hadc.Init.DiscontinuousConvMode = DISABLE;
    s_hadc.Init.ExternalTrigConvEdge  = ADC_EXTERNALTRIGCONVEDGE_NONE;
    s_hadc.Init.ExternalTrigConv      = ADC_SOFTWARE_START;
    s_hadc.Init.DataAlign             = ADC_DATAALIGN_RIGHT;
    s_hadc.Init.NbrOfConversion       = 2;
    s_hadc.Init.DMAContinuousRequests = ENABLE;          /* DMA circular       */
    s_hadc.Init.EOCSelection          = ADC_EOC_SEQ_CONV;
    __HAL_LINKDMA(&s_hadc, DMA_Handle, s_hdma);
    if (HAL_ADC_Init(&s_hadc) != HAL_OK) return;

    /* ---- Rank 1: IN11 (PC1) — Vbat ---- */
    ADC_ChannelConfTypeDef ch = {0};
    ch.Channel      = ADC_CHANNEL_11;
    ch.Rank         = 1;
    ch.SamplingTime = ADC_SAMPLETIME_480CYCLES;
    if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return;

    /* ---- Rank 2: IN13 (PC3) — Ibat ---- */
    ch.Channel      = ADC_CHANNEL_13;
    ch.Rank         = 2;
    ch.SamplingTime = ADC_SAMPLETIME_480CYCLES;
    if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return;

    /* ---- Detect battery cell count at init for correct thresholds ---- */
    /*
     * Note: at init Vbat may not be settled; thresholds are updated after
     * first tick once LPF has stabilised.  Set 4S thresholds if voltage
     * detected ≥ 13 V in battery_adc_tick() on first valid reading.
     */

    /* ---- Start continuous DMA conversion ---- */
    if (HAL_ADC_Start_DMA(&s_hadc, (uint32_t *)s_dma_buf, ADC_DMA_BUF_LEN)
        != HAL_OK) return;

    s_ready = true;
}

/* ---- battery_adc_tick() ---- */
void battery_adc_tick(uint32_t now_ms)
{
    if (!s_ready) return;
    (void)now_ms;   /* captured for caller context; may be used for future rate ctrl */

    /* ---- Snapshot DMA buffer with IRQ guard to prevent torn reads ---- */
    uint16_t snap[ADC_DMA_BUF_LEN];
    uint32_t primask = __get_PRIMASK();
    __disable_irq();
    memcpy(snap, (void *)s_dma_buf, sizeof(snap));
    __set_PRIMASK(primask);

    /* ---- Average 4 Vbat and 4 Ibat samples (interleaved pairs) ---- */
    uint32_t vbat_sum = 0u;
    uint32_t ibat_sum = 0u;
    for (uint32_t i = 0u; i < ADC_DMA_BUF_LEN; i += 2u) {
        vbat_sum += snap[i];        /* even indices: Vbat (rank 1) */
        ibat_sum += snap[i + 1u];   /* odd  indices: Ibat (rank 2) */
    }
    uint32_t vbat_avg = vbat_sum / ADC_OVERSAMPLE;   /* 0–4095 */
    uint32_t ibat_avg = ibat_sum / ADC_OVERSAMPLE;

    /* ---- Raw (unfiltered) calibrated voltage — for calibration query ---- */
    s_vbat_raw_mv = raw_to_vbat_mv(vbat_avg);

    /* ---- IIR low-pass filter ---- */
    /*
     * Accumulator stores value × 8 for sub-LSB precision.
     * Update: acc8 += (raw × 8 - acc8) >> LPF_SHIFT
     * Result: filtered_raw = acc8 >> 3
     *
     * With LPF_SHIFT=3: α = 1/(1+8) ≈ 0.111 → cutoff ≈ 4 Hz at 100 Hz tick.
     */
    s_lpf_vbat8 += ((vbat_avg << 3u) - s_lpf_vbat8) >> BATTERY_ADC_LPF_SHIFT;
    s_lpf_ibat8 += (((int32_t)ibat_avg << 3u) - s_lpf_ibat8)
                   >> BATTERY_ADC_LPF_SHIFT;

    uint32_t vbat_filtered = s_lpf_vbat8 >> 3u;   /* back to ADC counts */
    uint32_t ibat_filtered = (uint32_t)((s_lpf_ibat8 < 0 ? 0 : s_lpf_ibat8) >> 3u);

    /* ---- Apply calibration and convert to engineering units ---- */
    s_vbat_mv = raw_to_vbat_mv(vbat_filtered);
    s_ibat_ma = raw_to_ibat_ma(ibat_filtered);

    /* ---- Auto-set thresholds based on detected cell count ---- */
    /*
     * Detect 4S (Vbat ≥ 13 V) vs 3S (<13 V).  Only update thresholds once
     * after initial settling (first non-zero filtered reading).
     */
    static bool s_thresholds_set = false;
    if (!s_thresholds_set && s_vbat_mv > 500u) {
        if (s_vbat_mv >= 13000u) {
            s_low_mv      = BATTERY_ADC_LOW_MV_4S;
            s_critical_mv = BATTERY_ADC_CRITICAL_MV_4S;
        }
        /* else keep 3S defaults set at file scope */
        s_thresholds_set = true;
    }
}

/* ---- battery_adc_get_voltage_mv() ---- */
uint32_t battery_adc_get_voltage_mv(void)
{
    return s_vbat_mv;
}

/* ---- battery_adc_get_current_ma() ---- */
int32_t battery_adc_get_current_ma(void)
{
    return s_ibat_ma;
}

/* ---- battery_adc_get_raw_voltage_mv() ---- */
uint32_t battery_adc_get_raw_voltage_mv(void)
{
    return s_vbat_raw_mv;
}

/* ---- battery_adc_calibrate() ---- */
void battery_adc_calibrate(const battery_adc_cal_t *cal)
{
    if (cal == NULL) {
        /* Reset to defaults */
        s_cal.vbat_offset_mv = 0;
        s_cal.ibat_offset_ma = 0;
        s_cal.vbat_scale_num = 0;
        s_cal.vbat_scale_den = 0;
        return;
    }
    /* Clamp offsets to prevent runaway calibration values */
    int16_t voff = cal->vbat_offset_mv;
    if (voff >  500) voff =  500;
    if (voff < -500) voff = -500;
    int16_t ioff = cal->ibat_offset_ma;
    if (ioff >  200) ioff =  200;
    if (ioff < -200) ioff = -200;
    s_cal.vbat_offset_mv = voff;
    s_cal.ibat_offset_ma = ioff;
    s_cal.vbat_scale_num = cal->vbat_scale_num;
    s_cal.vbat_scale_den = cal->vbat_scale_den;
}

/* ---- battery_adc_get_calibration() ---- */
void battery_adc_get_calibration(battery_adc_cal_t *out_cal)
{
    if (out_cal != NULL) {
        *out_cal = s_cal;
    }
}

/* ---- battery_adc_publish() ---- */
/*
 * Sends JLINK_TLM_BATTERY (0x82) frame to Jetson at BATTERY_ADC_PUBLISH_HZ.
 *
 * Payload (jlink_tlm_battery_t, 10 bytes packed):
 *   vbat_mv     uint16  — calibrated LPF voltage (mV)
 *   ibat_ma     int16   — calibrated LPF current (mA)
 *   vbat_raw_mv uint16  — unfiltered last-tick voltage (mV)
 *   flags       uint8   — bit0=low, bit1=critical, bit2=4S detected, bit3=ready
 *   cal_offset  int8    — vbat_offset_mv / 4 (compressed, ±127 → ±508 mV)
 *   lpf_shift   uint8   — BATTERY_ADC_LPF_SHIFT value (for Jetson info)
 *   soc_pct     uint8   — voltage-based SoC estimate (0-100, 255=unknown)
 */
void battery_adc_publish(uint32_t now_ms)
{
    if (!s_ready) return;

    uint32_t period_ms = 1000u / BATTERY_ADC_PUBLISH_HZ;
    if (s_published_once && (now_ms - s_last_publish_ms) < period_ms) return;
    s_last_publish_ms = now_ms;
    s_published_once  = true;

    /* Build SoC estimate (voltage-based) */
    uint8_t soc = 255u;   /* unknown */
    if (s_vbat_mv > 500u) {
        uint32_t v_min, v_max;
        if (s_vbat_mv >= 13000u) {
            v_min = 13200u; v_max = 16800u;   /* 4S */
        } else {
            v_min =  9900u; v_max = 12600u;   /* 3S */
        }
        if (s_vbat_mv <= v_min) {
            soc = 0u;
        } else if (s_vbat_mv >= v_max) {
            soc = 100u;
        } else {
            soc = (uint8_t)(((s_vbat_mv - v_min) * 100u) / (v_max - v_min));
        }
    }

    /* Flags byte */
    uint8_t flags = 0u;
    if (s_vbat_mv > 0u && s_vbat_mv < s_low_mv)       flags |= 0x01u;
    if (s_vbat_mv > 0u && s_vbat_mv < s_critical_mv)  flags |= 0x02u;
    if (s_low_mv == BATTERY_ADC_LOW_MV_4S)             flags |= 0x04u;
    if (s_ready)                                        flags |= 0x08u;

    /* Build JLINK_TLM_BATTERY frame via jlink_send_battery_telemetry() */
    jlink_tlm_battery_t tlm = {
        .vbat_mv     = (uint16_t)(s_vbat_mv     & 0xFFFFu),
        .ibat_ma     = (int16_t) (s_ibat_ma      & 0xFFFF),
        .vbat_raw_mv = (uint16_t)(s_vbat_raw_mv  & 0xFFFFu),
        .flags       = flags,
        .cal_offset  = (int8_t)  (s_cal.vbat_offset_mv / 4),
        .lpf_shift   = (uint8_t) BATTERY_ADC_LPF_SHIFT,
        .soc_pct     = soc,
    };
    jlink_send_battery_telemetry(&tlm);
}

/* ---- battery_adc_check_pm() ---- */
void battery_adc_check_pm(uint32_t now_ms)
{
    if (!s_ready || s_vbat_mv == 0u) return;

    bool below_low = (s_vbat_mv < s_low_mv);
    bool below_critical = (s_vbat_mv < s_critical_mv);

    /* Track first entry below low threshold */
    if (below_low && !s_low_active) {
        s_low_active   = true;
        s_low_since_ms = now_ms;
        s_critical_notified = false;
    } else if (!below_low) {
        s_low_active   = false;
        s_critical_notified = false;
    }

    /* Notify power management on sustained critical voltage */
    if (below_critical && !s_critical_notified &&
        (now_ms - s_low_since_ms) >= BATTERY_ADC_LOW_HOLD_MS) {
        s_critical_notified = true;
        power_mgmt_notify_battery(s_vbat_mv);
    }
}

/* ---- battery_adc_is_low() ---- */
bool battery_adc_is_low(void)
{
    return (s_ready && s_vbat_mv > 0u && s_vbat_mv < s_low_mv);
}

/* ---- battery_adc_is_critical() ---- */
bool battery_adc_is_critical(void)
{
    return (s_ready && s_vbat_mv > 0u && s_vbat_mv < s_critical_mv);
}
