/**
 * @file battery_monitor.c
 * @brief Source file for the battery monitoring module.
 *
 * This module handles ADC measurements, voltage calculation,
 * battery percentage conversion, and hysteresis application for
 * a Li-Po battery connected via a voltage divider, with configuration
 * pulled from Device Tree.
 */

#include "battery_monitor.h"
#include "sys_event.h"

#include <zephyr/kernel.h>
#include <zephyr/drivers/adc.h>
#include <zephyr/devicetree.h>
#include <zephyr/logging/log.h>
#include <math.h> // For round()
#include <zephyr/devicetree/io-channels.h>
#include <zephyr/drivers/gpio.h>


LOG_MODULE_REGISTER(battery_monitor);

static struct k_timer test_timer;

/***************************************************************************/
/* --- Defines and Constants --- */
/** Charging detect node */
#define CHARGING_NODE DT_NODELABEL(charging_status)

/** ADC channel node for battery voltage measurement */
#define ADC_CHANNEL_NODE DT_NODELABEL(adc_ch4)

/** Node label for the battery voltage measurement ADC channel. */
#define VBATT DT_PATH(vbatt)

/** ADC channel ID derived from the 'vbatt' node in devicetree. */
#define ADC_CHANNEL_FROM_DTS (DT_REG_ADDR(DT_NODELABEL(adc_ch4)))

/** Resistor R2 (low side) value in ohms, from 'vbatt' node in devicetree. */
#define R2_DIVIDER_OHM DT_PROP(VBATT, output_ohms)

/** Total resistance (R1 + R2) in ohms, from 'vbatt' node in devicetree. */
#define R_TOTAL_DIVIDER_OHM DT_PROP(VBATT, full_ohms)

/** Resistor R1 (high side) value in ohms, calculated from DTS values. */
#define R1_DIVIDER_OHM (R_TOTAL_DIVIDER_OHM - R2_DIVIDER_OHM)

/** ADC resolution in bits. SAADC supports up to 14 bits. */
#define ADC_RESOLUTION 12

/** ADC gain setting. ADC_GAIN_1_2 allows input up to 1.8V with internal reference. */
#define ADC_GAIN adc_channel_cfg.gain

/** ADC oversampling setting. 4x oversampling for burst mode. */
#define ADC_OVERSAMPLING_4X 4

/** Number of samples to average for each battery measurement. */
#define NUM_SAMPLES 5

/** Interval between individual ADC measurements in milliseconds. */
#define MEASUREMENT_INTERVAL_MS 400

/** Voltage (in mV) considered 100% battery. */
#define BATTERY_FULL_MV 4000

/** Voltage (in mV) considered 0% battery. */
#define BATTERY_EMPTY_MV 3700

/** Internal ADC reference voltage in millivolts (0.6V). */
#define VREF_MV 900

/***************************************************************************/
/* --- Static Variables --- */

/** GPIO device structure for the battery charge status pin. */
static const struct gpio_dt_spec charge_detect = GPIO_DT_SPEC_GET(CHARGING_NODE, gpios);

/** Work structure for ADC processing. */
static struct k_work adc_work;

/** ADC device pointer. */
static const struct device *adc_dev;

/** ADC channel configuration structure, derived from devicetree. */
static const struct adc_channel_cfg adc_channel_cfg = ADC_CHANNEL_CFG_DT(ADC_CHANNEL_NODE);

/** ADC sequence structure for a single sample. */
static struct adc_sequence adc_sequence = {
    .channels    = BIT(ADC_CHANNEL_FROM_DTS),
    .resolution  = ADC_RESOLUTION,
    .oversampling = ADC_OVERSAMPLING_4X, /* Enabled burst mode (4x oversampling) */
    .calibrate   = false,
};

/** Buffer to store a single ADC sample. */
static int16_t adc_buffer;

/** Sum of collected ADC samples for averaging. */
static uint32_t sample_sum = 0;

/** Counter for collected ADC samples. */
static uint8_t sample_count = 0;

/** Stores the last calculated battery percentage. */
static uint8_t current_battery_percentage = 0;

/** Timer count for debugging purposes. */
static uint16_t timer_count = 0;

/***************************************************************************/
/* --- Static Function Prototypes --- */

/**
 * @brief Timer handler for periodic battery measurements.
 *
 * This function is called by the Zephyr kernel timer every
 * MEASUREMENT_INTERVAL_MS. It triggers an ADC conversion, processes
 * the sample, and if enough samples are collected, calculates the
 * battery percentage and invokes the callback.
 *
 * @param timer_id Pointer to the k_timer instance that expired.
 */
static void battery_measurement_timer_handler(struct k_timer *timer_id);

/**
 * @brief Timer handler for testing purposes.
 * 
 * @param timer_id 
 */
static void test_timer_handler(struct k_timer *timer_id);

/**
 * @brief Calculates the battery percentage from raw voltage.
 *
 * This function converts the measured battery voltage (in mV) into
 * a percentage based on defined full and empty voltage levels.
 *
 * @param voltage_mv The measured battery voltage in millivolts.
 * @return The calculated battery percentage (0-100).
 */
static uint8_t calculate_battery_percentage(uint32_t voltage_mv);

/**
 * @brief Applies hysteresis to the calculated battery percentage.
 *
 * This function adjusts the battery percentage to provide a more stable
 * reading, preventing rapid fluctuations around specific thresholds.
 *
 * @param percentage The raw calculated battery percentage.
 * @return The battery percentage after applying hysteresis.
 */
static uint8_t apply_hysteresis(uint8_t percentage);

/***************************************************************************/
/* --- Timer Definition --- */

/** Zephyr kernel timer for periodic battery measurements. */
K_TIMER_DEFINE(battery_measurement_timer, battery_measurement_timer_handler, NULL);

/***************************************************************************/
/* --- Static Function Implementations --- */

static void battery_measurement_timer_handler(struct k_timer *timer_id)
{
    timer_count++;
    LOG_INF("Bat mes timer triggered, count: %u", timer_count);
    int rc = k_work_submit(&adc_work);
    LOG_INF("timer=%u work_submit rc=%d", timer_count, rc);
}

/* Test timer handler */
static void test_timer_handler(struct k_timer *timer_id)
{
    LOG_INF("Test timer triggered");
}

/***************************************************************************/
static void adc_work_handler(struct k_work *work)
{
    int ret;
    int32_t adc_vref_mv = VREF_MV; // Internal reference voltage
    int32_t adc_gain_factor;
    int32_t measured_adc_mv;
    uint32_t battery_voltage_mv;
    uint8_t calculated_percentage;

    /* Set buffer for ADC sequence */
    adc_sequence.buffer = &adc_buffer;
    adc_sequence.buffer_size = sizeof(adc_buffer);

    ret = adc_read(adc_dev, &adc_sequence);
    if (ret < 0) {
        LOG_INF("ADC read failed with error %d", ret);
        return;
    }

    /*
     * Convert raw ADC value to millivolts.
     * The formula for millivolts:
     * millivolts = raw_value * Vref_mv / (2^resolution - 1) * gain_factor
     * The gain_factor depends on the ADC_GAIN setting.
     */
    switch (ADC_GAIN)
    {
        case ADC_GAIN_1_6:
            adc_gain_factor = 6;
            break;
        case ADC_GAIN_1_5:
            adc_gain_factor = 5;
            break;
        case ADC_GAIN_1_4:
            adc_gain_factor = 4;
            break;
        case ADC_GAIN_1_3:
            adc_gain_factor = 3;
            break;
        case ADC_GAIN_1_2:
            adc_gain_factor = 2;
            break;
        case ADC_GAIN_1:
            adc_gain_factor = 1;
            break;
        case ADC_GAIN_2:
            adc_gain_factor = 1; // For gain 2, input is divided by 2, so factor is 1 for compensation
            break;
        case ADC_GAIN_4:
            adc_gain_factor = 1; // For gain 4, input is divided by 4, so factor is 1 for compensation
            break;
        default:
            LOG_INF("Unsupported ADC gain configuration.");
            return;
    }

    // Calculate measured voltage at the ADC input pin
    measured_adc_mv = (int32_t)adc_buffer * adc_vref_mv * adc_gain_factor / ((1 << ADC_RESOLUTION) - 1);

    // Account for voltage divider: V_battery = V_measured * (R1 + R2) / R2
    // R1_DIVIDER_OHM and R2_DIVIDER_OHM are now pulled from DTS.
    battery_voltage_mv = (uint32_t)round((double)measured_adc_mv * R_TOTAL_DIVIDER_OHM / R2_DIVIDER_OHM);

    LOG_INF("Measured ADC raw: %d, Measured ADC mV: %d, Battery mV: %d",
            adc_buffer, measured_adc_mv, battery_voltage_mv);

    sample_sum += battery_voltage_mv;
    sample_count++;

    if (sample_count >= NUM_SAMPLES) {
        /* Stop the timer until explicitly restarted */
        k_timer_stop(&battery_measurement_timer);

        uint32_t average_voltage_mv = sample_sum / NUM_SAMPLES;
        LOG_INF("Average Battery Voltage: %u mV", average_voltage_mv);

        calculated_percentage = calculate_battery_percentage(average_voltage_mv);
        current_battery_percentage = apply_hysteresis(calculated_percentage);

        LOG_INF("Battery Percentage (raw): %u%%, (hysteresis): %u%%",
                calculated_percentage, current_battery_percentage);

        sys_event_t evt = {
            .type = EVENT_BATTERY_LEVEL_UPDATE,
            .data.battery.percentage = current_battery_percentage
        };
        sys_event_post(&evt);

        /* Reset for the next measurement cycle */
        sample_sum = 0;
        sample_count = 0;
    }
}

/***************************************************************************/
static uint8_t calculate_battery_percentage(uint32_t voltage_mv)
{
    if (voltage_mv >= BATTERY_FULL_MV) {
        return 100;
    } else if (voltage_mv <= BATTERY_EMPTY_MV) {
        return 0;
    } else {
        /* Linear interpolation */
        uint32_t range_mv = BATTERY_FULL_MV - BATTERY_EMPTY_MV;
        uint32_t current_mv_offset = voltage_mv - BATTERY_EMPTY_MV;
        uint8_t percentage = (uint8_t)round((double)current_mv_offset * 100.0 / range_mv);
        return percentage;
    }
}

/***************************************************************************/
static uint8_t apply_hysteresis(uint8_t percentage)
{
    if (percentage <= 5) {
        return 0;
    } else if (percentage <= 15) {
        return 10;
    } else if (percentage <= 25) {
        return 20;
    } else if (percentage <= 35) {
        return 30;
    } else if (percentage <= 45) {
        return 40;
    } else if (percentage <= 55) {
        return 50;
    } else if (percentage <= 65) {
        return 60;
    } else if (percentage <= 75) {
        return 70;
    } else if (percentage <= 85) {
        return 80;
    } else if (percentage <= 95) {
        return 90;
    } else { /* percentage > 95 */
        return 100;
    }
}

/***************************************************************************/
/* --- Public Function Implementations --- */

int battery_monitor_start_periodic_measurement(void)
{
    /* Ensure previous measurement cycle is reset before starting a new one */
    sample_sum = 0;
    sample_count = 0;
    timer_count = 0;

    k_timer_stop(&battery_measurement_timer);
    k_timer_start(&battery_measurement_timer, K_NO_WAIT, K_MSEC(MEASUREMENT_INTERVAL_MS));
    LOG_INF("Periodic battery measurement started (every %d ms)", MEASUREMENT_INTERVAL_MS);
    return 0;
}

/***************************************************************************/
uint8_t battery_monitor_get_percentage(void)
{
    return current_battery_percentage;
}

/***************************************************************************/
bool batt_get_charge_status(void)
{
    return (gpio_pin_get_dt(&charge_detect) == 0); // 0 = charging (active low)
}

/***************************************************************************/
int battery_monitor_init(void)
{
    int ret;

    adc_dev = DEVICE_DT_GET(DT_IO_CHANNELS_CTLR(VBATT));
    if (!device_is_ready(adc_dev)) {
        LOG_ERR("ADC device not ready");
        return -ENODEV;
    }

    ret = adc_channel_setup(adc_dev, &adc_channel_cfg);
    if (ret < 0) {
        LOG_ERR("ADC channel setup failed with error %d", ret);
        return ret;
    }

    LOG_INF("Battery monitor initialized on AIN%d using DTS config",
            ADC_CHANNEL_FROM_DTS);

    k_work_init(&adc_work, adc_work_handler);

    if (!device_is_ready(charge_detect.port)) {
        LOG_ERR("Battery charge GPIO port not ready");
        return -ENODEV;
    }

    ret = gpio_pin_configure_dt(&charge_detect, GPIO_INPUT);
    if (ret < 0) {
        LOG_ERR("Failed to configure batt_charge_gpio");
        return ret;
    }

    LOG_INF("Battery is %s", batt_get_charge_status() ? "charging" : "not charging");

    k_timer_init(&test_timer, test_timer_handler, NULL);
    k_timer_start(&test_timer, K_NO_WAIT, K_MSEC(1000));

    return 0;
}