/** 
 * @file    battery.h
 * @brief   Battery module
 * @author  Kenny Tran
 * @date    Dec 02 2024
 * @version 0.0.1
 *
 * @section LICENSES
 * Copyright (C) Morgan Advanced Programmable Systems, Inc.
 */



/*--------------------------------------------------------------------------------------------------
 * INCLUDE FILES
 *------------------------------------------------------------------------------------------------*/
#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#include "../lib/errno.h"
#include "../lib/print.h"

#include "../wrappers/adc.h"
#include "../wrappers/gpio.h"

#include "../app/app.h"

#include "battery.h"


/*--------------------------------------------------------------------------------------------------
 * MACROS & CONSTANTS
 *------------------------------------------------------------------------------------------------*/
#define _BATTERY_CAPACITY_STEPS     (11)
#define _BATTERY_N_CURVES           (5)


/*--------------------------------------------------------------------------------------------------
 * TYPE DEFINITIONS & ENUMERATIONS
 *------------------------------------------------------------------------------------------------*/
typedef struct
{
    float voltage;
    float capacity;   /* NOTE: 1.0 = 100 % */
} _battery_point_s;

typedef struct
{
    int8_t           temp_c;
    _battery_point_s curve_readings[_BATTERY_CAPACITY_STEPS];
} _battery_table_s;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE CONSTANTS & VARIABLES
 *------------------------------------------------------------------------------------------------*/
static bool             _battery_init = false;
static _battery_table_s _battery_tables[_BATTERY_N_CURVES] =
{
    {
        .temp_c = -20,
        { (_battery_point_s) { .voltage = 4.20, .capacity = 1.00 },
          (_battery_point_s) { .voltage = 3.36, .capacity = 0.90 },
          (_battery_point_s) { .voltage = 3.34, .capacity = 0.80 },
          (_battery_point_s) { .voltage = 3.32, .capacity = 0.70 },
          (_battery_point_s) { .voltage = 3.29, .capacity = 0.60 },
          (_battery_point_s) { .voltage = 3.24, .capacity = 0.50 },
          (_battery_point_s) { .voltage = 3.19, .capacity = 0.40 },
          (_battery_point_s) { .voltage = 3.08, .capacity = 0.30 },
          (_battery_point_s) { .voltage = 2.84, .capacity = 0.20 },
          (_battery_point_s) { .voltage = 2.50, .capacity = 0.10 },
          (_battery_point_s) { .voltage = 2.00, .capacity = 0.00 } }
    },
    {
        .temp_c = -10,
        { (_battery_point_s) { .voltage = 4.20, .capacity = 1.00 },
          (_battery_point_s) { .voltage = 3.58, .capacity = 0.90 },
          (_battery_point_s) { .voltage = 3.55, .capacity = 0.80 },
          (_battery_point_s) { .voltage = 3.52, .capacity = 0.70 },
          (_battery_point_s) { .voltage = 3.48, .capacity = 0.60 },
          (_battery_point_s) { .voltage = 3.43, .capacity = 0.50 },
          (_battery_point_s) { .voltage = 3.34, .capacity = 0.40 },
          (_battery_point_s) { .voltage = 3.27, .capacity = 0.30 },
          (_battery_point_s) { .voltage = 3.15, .capacity = 0.20 },
          (_battery_point_s) { .voltage = 2.90, .capacity = 0.10 },
          (_battery_point_s) { .voltage = 2.50, .capacity = 0.00 } }
    },
    {
        .temp_c = 0,
        { (_battery_point_s) { .voltage = 4.20, .capacity = 1.00 },
          (_battery_point_s) { .voltage = 3.87, .capacity = 0.90 },
          (_battery_point_s) { .voltage = 3.78, .capacity = 0.80 },
          (_battery_point_s) { .voltage = 3.73, .capacity = 0.70 },
          (_battery_point_s) { .voltage = 3.70, .capacity = 0.60 },
          (_battery_point_s) { .voltage = 3.68, .capacity = 0.50 },
          (_battery_point_s) { .voltage = 3.65, .capacity = 0.40 },
          (_battery_point_s) { .voltage = 3.60, .capacity = 0.30 },
          (_battery_point_s) { .voltage = 3.54, .capacity = 0.20 },
          (_battery_point_s) { .voltage = 3.37, .capacity = 0.10 },
          (_battery_point_s) { .voltage = 2.58, .capacity = 0.00 } }
    },
    {
        .temp_c = 25,
        { (_battery_point_s) { .voltage = 4.20, .capacity = 1.00 },
          (_battery_point_s) { .voltage = 4.03, .capacity = 0.90 },
          (_battery_point_s) { .voltage = 3.96, .capacity = 0.80 },
          (_battery_point_s) { .voltage = 3.91, .capacity = 0.70 },
          (_battery_point_s) { .voltage = 3.89, .capacity = 0.60 },
          (_battery_point_s) { .voltage = 3.87, .capacity = 0.50 },
          (_battery_point_s) { .voltage = 3.85, .capacity = 0.40 },
          (_battery_point_s) { .voltage = 3.81, .capacity = 0.30 },
          (_battery_point_s) { .voltage = 3.75, .capacity = 0.20 },
          (_battery_point_s) { .voltage = 3.62, .capacity = 0.10 },
          (_battery_point_s) { .voltage = 3.10, .capacity = 0.00 } }
    },
    {
        .temp_c = 60,
        { (_battery_point_s) { .voltage = 4.20, .capacity = 1.00 },
          (_battery_point_s) { .voltage = 4.07, .capacity = 0.90 },
          (_battery_point_s) { .voltage = 4.02, .capacity = 0.80 },
          (_battery_point_s) { .voltage = 3.99, .capacity = 0.70 },
          (_battery_point_s) { .voltage = 3.97, .capacity = 0.60 },
          (_battery_point_s) { .voltage = 3.95, .capacity = 0.50 },
          (_battery_point_s) { .voltage = 3.91, .capacity = 0.40 },
          (_battery_point_s) { .voltage = 3.89, .capacity = 0.30 },
          (_battery_point_s) { .voltage = 3.83, .capacity = 0.20 },
          (_battery_point_s) { .voltage = 3.75, .capacity = 0.10 },
          (_battery_point_s) { .voltage = 3.37, .capacity = 0.00 } }
    }
};


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION PROTOTYPES
 *------------------------------------------------------------------------------------------------*/
static float _battery_interpolate_capacity_at_voltage (_battery_point_s *p_point, float voltage);
static float _battery_interpolate_capacity_across_temps (float T1, float T2, float T, float C1, float C2);


/*--------------------------------------------------------------------------------------------------
 * PUBLIC FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
ERRNO_e BATTERY_init (void)
{
    if (_battery_init)
    {
        return ERRNO_ALREADY_INITIALIZED;
    }

    if (GPIO_init () < ERRNO_SUCCESS)
    {
        return ERRNO_GPIO_FAILURE;
    }

    if (GPIO_cfg (GPIO_PIN_BATTERY_CHECK_ENABLE, GPIO_DIRECTION_OUT, GPIO_PULLUP, GPIO_INIT_HIGH, GPIO_INT_STATE_CHANGE_LL_HIGH, NULL))
    {
        return ERRNO_GPIO_FAILURE;
    }

    if (ADC_init () < ERRNO_SUCCESS)
    {
        return ERRNO_ADC_FAILURE;
    }

    if (ADC_cfg (ADC_CH_0, ADC_GAIN_DIV6, ADC_REF_INT, ADC_RESOLUTION_14_BITS) != ERRNO_SUCCESS)
    {
        return ERRNO_ADC_FAILURE;
    }

    /* Enable buffer to read battery voltage. Active-low. */
    if (GPIO_level_set (GPIO_PIN_BATTERY_CHECK_ENABLE, false))
    {
        return ERRNO_GPIO_FAILURE;
    }

    /* Configure GPIO to detect when charger / external battery is connected. */
    if (GPIO_cfg (GPIO_PIN_CHARGER_STATUS, GPIO_DIRECTION_IN, GPIO_PULLNONE, GPIO_INIT_HIGH, GPIO_INT_FALLING_EDGE, NULL))
    {
        return ERRNO_GPIO_FAILURE;
    }

    _battery_init = true;

    return ERRNO_SUCCESS;
}

ERRNO_e BATTERY_wake_on_charge (bool enable)
{
    if (!_battery_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    if (enable)
    {
        /* Reconfigure GPIO interrupt for charge detection to be used to wake from sleep.
         * Note: Only logic level type interrupts can be used for GPIO to wake MCU from sleep.  This is because
         *       putting MCU to sleep uses a slower clock and GPIO sense, so rising and falling edge interrupts don't work. */
        if (GPIO_cfg (GPIO_PIN_CHARGER_STATUS, GPIO_DIRECTION_IN, GPIO_PULLNONE, GPIO_INIT_HIGH, GPIO_INT_LL_LOW, NULL))
        {
            return ERRNO_GPIO_FAILURE;
        }
    }
    else
    {
        if (GPIO_cfg (GPIO_PIN_CHARGER_STATUS, GPIO_DIRECTION_IN, GPIO_PULLNONE, GPIO_INIT_HIGH, GPIO_INT_FALLING_EDGE, NULL))
        {
            return ERRNO_GPIO_FAILURE;
        }
    }

    return ERRNO_SUCCESS;
}

bool BATTERY_is_charging (void)
{
    /* Active-low */
    return !GPIO_level_get (GPIO_PIN_CHARGER_STATUS);
}

ERRNO_e BATTERY_voltage_get (float *p_voltage)
{
    const int32_t adc_fullscale = (1 << ADC_RESOLUTION_14_BITS) - 1;
    const float   v_scaler      = BATTERY_VOLTAGE_DIVIDER_R2_OHMS / (BATTERY_VOLTAGE_DIVIDER_R1_OHMS + BATTERY_VOLTAGE_DIVIDER_R2_OHMS);
          int32_t adc_raw;

    if (!_battery_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    /* Enable buffer to read battery voltage. Active-low. */
    // if (GPIO_level_set (GPIO_PIN_BATTERY_CHECK_ENABLE, false))
    // {
    //     return ERRNO_GPIO_FAILURE;
    // }

    if (ADC_read (ADC_CH_0, &adc_raw) != ERRNO_SUCCESS)
    {
        return ERRNO_ADC_FAILURE;
    }

    /* NOTE: We are using ADC_REF_INT for reference (0.6V), with prescaler of 1/6.  Therefore, the maximum ADC value will be 16383 (14-bits)
             with an input voltage of (0.6V * 6) = 3.6V. */
    *p_voltage = ((float) adc_raw / (float) adc_fullscale) * 3.6f;

    /* NOTE: With voltage divider of R2 = 510k and R1 = 1M.  A full charge of 4.2V will be seen as ((4.2V * 510k) / (510k + 1M)) ~= 1.419V.
       Divide by scaler to get actual voltage. */
    *p_voltage /= v_scaler;

    /* Disable buffer to read battery voltage. Active-low. */
    // if (GPIO_level_set (GPIO_PIN_BATTERY_CHECK_ENABLE, true))
    // {
    //     return ERRNO_GPIO_FAILURE;
    // }

    return ERRNO_SUCCESS;
}

float BATTERY_level_calculate (float voltage)
{
    float   temp = APP_temp_c_get ();
    float   capacity;
    uint8_t curve;

    if (temp <= -20.0f)
    {
        /* For temperatures at or below -20C, use the -20C curve with interpolation. */
        capacity = _battery_interpolate_capacity_at_voltage (_battery_tables[0].curve_readings, voltage);
        
        /* Validate interpolation result. */
        if (capacity < 0.0f)
        {
            /* Below minimum voltage, battery is empty. */
            return 0.0f;
        }
        
        return capacity;
    }
    else if (temp >= 60.0f)
    {
        /* For temperatures at or above 60C, use the 60C curve with interpolation. */
        capacity = _battery_interpolate_capacity_at_voltage (_battery_tables[4].curve_readings, voltage);
        
        /* Validate interpolation result. */
        if (capacity < 0.0f)
        {
            /* Below minimum voltage, battery is empty. */
            return 0.0f;
        }
        
        return capacity;
    }
    else
    {
        /* Loop through the temperature curves. */
        for (curve = 1; curve < _BATTERY_N_CURVES; curve++)
        {
            /* Find the two curves the temp reading is between. */
            if (temp <= (float) _battery_tables[curve].temp_c)
            {
                float cap_t1 = _battery_interpolate_capacity_at_voltage (_battery_tables[curve-1].curve_readings, voltage);
                float cap_t2 = _battery_interpolate_capacity_at_voltage (_battery_tables[curve].curve_readings, voltage);

                /* Check if either interpolation failed (voltage out of range). */
                if (cap_t1 < 0.0f || cap_t2 < 0.0f)
                {
                    /* Below minimum voltage, battery is empty. */
                    return 0.0f;
                }

                return _battery_interpolate_capacity_across_temps (_battery_tables[curve-1].temp_c,
                                                                    _battery_tables[curve].temp_c,
                                                                    temp,
                                                                    cap_t1,
                                                                    cap_t2);
            }
        }
    }

    /* Should never reach here, but return 0 as safe fallback. */
    return 0.0f;
}

void BATTERY_cli_cmd (int32_t argc, char **argv)
{
    ERRNO_e err;

    if (argc == 0)
    {
        float voltage = 0.0f;

        PRINT (INFO, "Battery\r\n");
        PRINT (INFO, "-------------------------------------------\r\n");
        (void) BATTERY_voltage_get (&voltage);
        PRINT (INFO, "Voltage  : %.3f V\r\n", (double) voltage);
        PRINT (INFO, "Level    : %.2f %%\r\n", (double) (BATTERY_level_calculate (voltage) * 100.0f));
        PRINT (INFO, "Status   : %s...\r\n", BATTERY_is_charging () ? "Charging": "Not Charging");
    }
    else if (argc == 2)
    {
        if (!strcmp (argv[0], "--init") || !strcmp (argv[0], "-i"))
        {
            err = BATTERY_init ();
            PRINT (INFO, "Ret code : %d\r\n", err);
        }
        else
        {
            goto USAGE;
        }
    }
    else if (argc == 1)
    {
        if (!strcmp (argv[0], "--help") || !strcmp (argv[0], "?"))
        {
            goto USAGE;
        }
        else if (!strcmp (argv[0], "--init") || !strcmp (argv[0], "-i"))
        {
            err = BATTERY_init ();
            PRINT (INFO, "Ret code : %d\r\n", err);
        }
        else
        {
            goto USAGE;
        }
    }
    else
    {
USAGE:
        PRINT (INFO, "Usage: battery [OPTION...]\r\n");
        PRINT (INFO, "  -i | --init          Initializes peripherals necessary to read battery levels\r\n");
    }
}


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
static float _battery_interpolate_capacity_at_voltage (_battery_point_s *p_point, float voltage)
{
          uint8_t i;
    const uint8_t bounds = _BATTERY_CAPACITY_STEPS - 1;

    if (voltage > 4.2f)
    {
        return 1.0f;
    }
    
    /* Find the two points where voltage lies between. */
    for (i = 0; i < bounds; i++)
    {
        if (p_point[i].voltage >= voltage && voltage >= p_point[i + 1].voltage)
        {
            float V1 = p_point[i].voltage;
            float V2 = p_point[i + 1].voltage;                        
            float C1 = p_point[i].capacity;
            float C2 = p_point[i + 1].capacity;

            /* Linear interpolation: C(V) = C1 + [(C2 - C1) / (V2 - V1)] * (V - V1) */
            return C1 + (C2 - C1) / (V2 - V1) * (voltage - V1);
        }
    }

    /* Out of range, return -1 */
    return -1.0f;
}

static float _battery_interpolate_capacity_across_temps (float T1, float T2, float T, float C1, float C2)
{
    /* Linear interpolation: C(T) = C(T1) + [(C(T2) - C(T1)) / (T2 - T1)] * (T - T1) */
    float slope = (C2 - C1) / (T2 - T1);

    return ((slope * (T - T1)) + C1);
}
