// =============================================================================
// SaltyBot — Battery Coulomb Counter Implementation (Issue #325)
// Agent: sl-mechanical  |  2026-03-03
//
// ADC-based coulomb integrator for LiPo state-of-charge tracking.
// Interfaces with INA219 current sensor via I2C, maintains persistent
// calibration and cycle count in flash.
//
// =============================================================================

#include "battery_coulomb.h"
#include <string.h>
#include <math.h>

// =============================================================================
// STATIC STATE
// =============================================================================

static coulomb_state_t g_coulomb_state = {
    .current_a = 0.0f,
    .coulombs_total = 0.0f,
    .coulombs_discharged = 0.0f,
    .soc_percent = 100.0f,
    .tick_count = 0,
    .charging = false,
    .cycle_count = 0,
};

static coulomb_calibration_t g_calibration = {
    .magic = 0xCAFEBABE,
    .current_offset_a = 0.0f,
    .current_scale = 1.0f,
    .total_cycles = 0,
    .timestamp = 0,
};

// Cycle tracking (ring buffer)
#define CYCLE_HISTORY_SIZE 16
static coulomb_cycle_t g_cycle_history[CYCLE_HISTORY_SIZE];
static uint8_t g_cycle_idx = 0;

// Current cycle accumulator
static float g_cycle_coulombs = 0.0f;
static float g_cycle_start_current = 0.0f;
static uint32_t g_cycle_start_tick = 0;

// =============================================================================
// INA219 I2C REGISTER DEFINITIONS
// =============================================================================

#define INA219_REG_CONFIG       0x00
#define INA219_REG_SHUNT_VOLT   0x01
#define INA219_REG_BUS_VOLT     0x02
#define INA219_REG_POWER        0x03
#define INA219_REG_CURRENT      0x04
#define INA219_REG_CALIBRATION  0x05

// Config bits
#define INA219_CONFIG_MODE_CONT_BOTH  0x7  // Continuous V & I
#define INA219_CONFIG_PGA_40MV        0x0  // ±40mV range
#define INA219_CONFIG_SADC_12BIT      0x0  // 12-bit ADC

// =============================================================================
// I2C HELPER FUNCTIONS (stubs — implement per STM32 variant)
// =============================================================================

// Forward declarations
static int ina219_read_current_raw(int16_t* out_raw);
static int ina219_write_config(void);
static int i2c_read_reg(uint8_t addr, uint8_t reg, uint16_t* data);
static int i2c_write_reg(uint8_t addr, uint8_t reg, uint16_t data);

// =============================================================================
// FLASH OPERATIONS (STM32 sector operations)
// =============================================================================

/**
 * @brief Write calibration struct to flash
 * @param addr Flash address (must be sector-aligned)
 * @param data Pointer to coulomb_calibration_t
 * @return 0 on success
 */
static int flash_write_calibration(uint32_t addr, const coulomb_calibration_t* data)
{
    // Stub: Implement STM32 flash unlock, erase sector, write data
    // For now, assume this is handled by HAL: HAL_FLASH_Program()
    // In real implementation: call HAL_FLASH_Unlock(), HAL_FLASHEx_Erase(), 
    // then loop through data bytes calling HAL_FLASH_Program()
    
    // Copy to flash (simplified)
    uint32_t* src = (uint32_t*)data;
    uint32_t* dst = (uint32_t*)addr;
    
    for (int i = 0; i < sizeof(coulomb_calibration_t) / 4; i++) {
        dst[i] = src[i];  // Assuming flash write is handled externally
    }
    
    return 0;
}

/**
 * @brief Read calibration struct from flash
 * @param addr Flash address
 * @param data Pointer to output coulomb_calibration_t
 * @return 0 on success, -1 if invalid magic
 */
static int flash_read_calibration(uint32_t addr, coulomb_calibration_t* data)
{
    uint32_t* src = (uint32_t*)addr;
    uint32_t* dst = (uint32_t*)data;
    
    for (int i = 0; i < sizeof(coulomb_calibration_t) / 4; i++) {
        dst[i] = src[i];
    }
    
    if (data->magic != 0xCAFEBABE) {
        return -1;  // Invalid calibration
    }
    
    return 0;
}

// =============================================================================
// INA219 INTERFACE STUBS
// =============================================================================

/**
 * @brief Read raw current from INA219
 * @param out_raw 16-bit signed raw value
 * @return 0 on success
 */
static int ina219_read_current_raw(int16_t* out_raw)
{
    uint16_t raw;
    int ret = i2c_read_reg(INA219_I2C_ADDR, INA219_REG_CURRENT, &raw);
    if (ret != 0) return -1;
    
    *out_raw = (int16_t)raw;
    return 0;
}

/**
 * @brief Configure INA219 for continuous measurement
 * @return 0 on success
 */
static int ina219_write_config(void)
{
    // Config: Continuous I & V, ±40mV shunt range, 12-bit ADC
    uint16_t config = (INA219_CONFIG_MODE_CONT_BOTH << 0) |
                      (INA219_CONFIG_PGA_40MV << 11) |
                      (INA219_CONFIG_SADC_12BIT << 3);
    
    return i2c_write_reg(INA219_I2C_ADDR, INA219_REG_CONFIG, config);
}

// I2C read/write stubs (implement per STM32 HAL)
static int i2c_read_reg(uint8_t addr, uint8_t reg, uint16_t* data)
{
    // Stub: Replace with HAL_I2C_Mem_Read()
    // HAL_I2C_Mem_Read(&hi2c, addr << 1, reg, 1, (uint8_t*)data, 2, 100);
    return 0;
}

static int i2c_write_reg(uint8_t addr, uint8_t reg, uint16_t data)
{
    // Stub: Replace with HAL_I2C_Mem_Write()
    // HAL_I2C_Mem_Write(&hi2c, addr << 1, reg, 1, (uint8_t*)&data, 2, 100);
    return 0;
}

// =============================================================================
// PUBLIC API IMPLEMENTATION
// =============================================================================

int coulomb_init(uint16_t capacity_mah)
{
    // Initialize INA219
    if (ina219_write_config() != 0) {
        return -1;  // I2C init failed
    }
    
    // Load calibration from flash
    coulomb_load_calibration();
    
    // Initialize state
    g_coulomb_state.soc_percent = 100.0f;
    g_coulomb_state.coulombs_discharged = 0.0f;
    g_coulomb_state.coulombs_total = 0.0f;
    g_coulomb_state.tick_count = 0;
    g_coulomb_state.cycle_count = g_calibration.total_cycles;
    
    return 0;
}

void coulomb_tick(void)
{
    int16_t raw_current;
    float dt = 1.0f / COULOMB_SAMPLE_RATE_HZ;  // 1 ms per tick
    
    // Read current from INA219
    if (ina219_read_current_raw(&raw_current) != 0) {
        return;  // I2C read failed, skip this tick
    }
    
    // Convert raw to amps
    // LSB = 1 mA (calibration register sets this)
    // INA219: raw [−32768, +32767] → [-3.2A, +3.2A] = 0.1 mA/LSB
    float current_a = (raw_current * 0.0001f) + g_calibration.current_offset_a;
    current_a *= g_calibration.current_scale;
    
    // Update state
    g_coulomb_state.current_a = current_a;
    g_coulomb_state.charging = (current_a < 0.0f);
    g_coulomb_state.tick_count++;
    
    // Integrate coulombs: ΔQ = I * Δt
    float coulombs_delta = current_a * dt;
    g_coulomb_state.coulombs_total += coulombs_delta;
    
    // Update discharged counter
    if (current_a > 0.05f) {  // Discharging (5 mA hysteresis)
        g_coulomb_state.coulombs_discharged += coulombs_delta;
        g_cycle_coulombs += coulombs_delta;
    } else if (current_a < -0.05f) {  // Charging
        // Don't add negative coulombs to discharged counter
    }
    
    // Calculate SOC
    float coulombs_capacity = BATTERY_CAPACITY_Q;
    if (coulombs_capacity > 0) {
        g_coulomb_state.soc_percent = 100.0f * 
            (1.0f - (g_coulomb_state.coulombs_discharged / coulombs_capacity));
        
        // Clamp to 0–100%
        if (g_coulomb_state.soc_percent < 0.0f) {
            g_coulomb_state.soc_percent = 0.0f;
        } else if (g_coulomb_state.soc_percent > 100.0f) {
            g_coulomb_state.soc_percent = 100.0f;
        }
    }
}

const coulomb_state_t* coulomb_get_state(void)
{
    return &g_coulomb_state;
}

float coulomb_get_soc(void)
{
    return g_coulomb_state.soc_percent;
}

float coulomb_get_current_a(void)
{
    return g_coulomb_state.current_a;
}

float coulomb_get_coulombs_total(void)
{
    return g_coulomb_state.coulombs_total;
}

float coulomb_get_coulombs_discharged(void)
{
    return g_coulomb_state.coulombs_discharged;
}

uint16_t coulomb_get_cycle_count(void)
{
    return g_coulomb_state.cycle_count;
}

void coulomb_set_soc(float soc_percent)
{
    // Clamp to 0–100%
    if (soc_percent < 0.0f) soc_percent = 0.0f;
    if (soc_percent > 100.0f) soc_percent = 100.0f;
    
    // Recalculate coulombs_discharged
    float coulombs_capacity = BATTERY_CAPACITY_Q;
    g_coulomb_state.coulombs_discharged = coulombs_capacity * (1.0f - (soc_percent / 100.0f));
    g_coulomb_state.soc_percent = soc_percent;
}

void coulomb_mark_full_charge(void)
{
    g_coulomb_state.coulombs_discharged = 0.0f;
    g_coulomb_state.soc_percent = 100.0f;
    g_cycle_coulombs = 0.0f;
    g_cycle_start_tick = g_coulomb_state.tick_count;
}

void coulomb_mark_fully_discharged(void)
{
    // Log cycle
    coulomb_cycle_t* cycle = &g_cycle_history[g_cycle_idx];
    cycle->coulombs_capacity = g_cycle_coulombs;
    cycle->depth_of_discharge = g_cycle_coulombs / BATTERY_CAPACITY_Q;
    cycle->duration_s = (g_coulomb_state.tick_count - g_cycle_start_tick) / 
                        COULOMB_SAMPLE_RATE_HZ;
    cycle->avg_current_a = (cycle->duration_s > 0) ? 
        (g_cycle_coulombs / cycle->duration_s) : 0.0f;
    
    // Update cycle count
    g_coulomb_state.cycle_count++;
    g_calibration.total_cycles++;
    
    // Move to next cycle slot
    g_cycle_idx = (g_cycle_idx + 1) % CYCLE_HISTORY_SIZE;
    g_cycle_coulombs = 0.0f;
    
    // Mark as empty (SOC = 0%)
    g_coulomb_state.soc_percent = 0.0f;
}

void coulomb_calibrate_offset(void)
{
    // Sample current ~20 times and average (assumes zero current during cal)
    int16_t raw;
    int32_t sum = 0;
    int samples = 20;
    
    for (int i = 0; i < samples; i++) {
        if (ina219_read_current_raw(&raw) == 0) {
            sum += raw;
        }
    }
    
    // Calculate offset and store
    float avg_raw = (float)sum / samples;
    g_calibration.current_offset_a = -(avg_raw * 0.0001f);  // Negate to zero it
}

int coulomb_save_calibration(void)
{
    g_calibration.timestamp = 0;  // Would be unix time from RTC
    return flash_write_calibration(COULOMB_FLASH_ADDR, &g_calibration);
}

int coulomb_load_calibration(void)
{
    coulomb_calibration_t temp;
    int ret = flash_read_calibration(COULOMB_FLASH_ADDR, &temp);
    
    if (ret == 0) {
        memcpy(&g_calibration, &temp, sizeof(coulomb_calibration_t));
    } else {
        // No valid calibration in flash, use defaults
        g_calibration.current_offset_a = 0.0f;
        g_calibration.current_scale = 1.0f;
        g_calibration.total_cycles = 0;
    }
    
    return ret;
}

int coulomb_factory_reset(void)
{
    // Clear flash
    coulomb_calibration_t blank = {0};
    flash_write_calibration(COULOMB_FLASH_ADDR, &blank);
    
    // Reset runtime state
    memset(&g_coulomb_state, 0, sizeof(coulomb_state_t));
    g_coulomb_state.soc_percent = 100.0f;
    g_coulomb_state.cycle_count = 0;
    
    memset(&g_calibration, 0, sizeof(coulomb_calibration_t));
    
    return 0;
}

int coulomb_get_cycle_info(uint8_t cycle_idx, coulomb_cycle_t* out_cycle)
{
    if (cycle_idx >= CYCLE_HISTORY_SIZE) {
        return -1;
    }
    
    memcpy(out_cycle, &g_cycle_history[cycle_idx], sizeof(coulomb_cycle_t));
    return 0;
}

// =============================================================================
// END IMPLEMENTATION
// =============================================================================