#include "hal/adc_types.h"
#include <esp_adc/adc_oneshot.h>
#include <esp_adc/adc_cali.h>
#include <esp_adc/adc_cali_scheme.h>

#include "pins.h"
#include "FreeRTOS.h"

static float g_battery_voltage = 0;
static int g_battery_percent = 0;

adc_oneshot_unit_handle_t adc_handle;
adc_cali_handle_t adc_cali_handle;

float getBatteryVoltage() {
    return g_battery_voltage;
}

int getBatteryPercentage() {
    return g_battery_percent;
}

void battery_adc_init() {
    // ADC init
    adc_oneshot_unit_init_cfg_t init_config = {
        .unit_id = ADC_UNIT_1,
        .clk_src = ADC_RTC_CLK_SRC_DEFAULT,
    };

    ESP_ERROR_CHECK(adc_oneshot_new_unit(&init_config, &adc_handle));

    // Channel config
    adc_oneshot_chan_cfg_t config = {
        .bitwidth = ADC_BITWIDTH_DEFAULT,
        .atten = ADC_ATTEN_DB_12,
    };

    ESP_ERROR_CHECK(
        adc_oneshot_config_channel(adc_handle, ADC_CHANNEL_9, &config)
    );

    // Calibration
    adc_cali_curve_fitting_config_t cali_config = {
        .unit_id = ADC_UNIT_1,
        .chan = ADC_CHANNEL_9,
        .atten = ADC_ATTEN_DB_12,
        .bitwidth = ADC_BITWIDTH_DEFAULT,
    };

    ESP_ERROR_CHECK(
        adc_cali_create_scheme_curve_fitting(
            &cali_config,
            &adc_cali_handle
        )
    );
}

typedef struct {
    float voltage;
    int percentage;
} BatteryPoint;

static const BatteryPoint battery_curve[] = {
    {4.20, 100},
    {4.10, 90},
    {4.00, 80},
    {3.90, 70},
    {3.80, 60},
    {3.70, 45},
    {3.60, 30},
    {3.50, 15},
    {3.40, 8},
    {3.30, 5},
    {3.20, 0},
};

int batteryPercentage(float voltage) {
    int size = sizeof(battery_curve) / sizeof(BatteryPoint);

    // Above max
    if (voltage >= battery_curve[0].voltage)
        return 100;

    // Below min
    if (voltage <= battery_curve[size - 1].voltage)
        return 0;

    // Find range
    for (int i = 0; i < size - 1; i++) {
        if (voltage <= battery_curve[i].voltage &&
            voltage >= battery_curve[i + 1].voltage) {

            float v1 = battery_curve[i].voltage;
            float v2 = battery_curve[i + 1].voltage;

            int p1 = battery_curve[i].percentage;
            int p2 = battery_curve[i + 1].percentage;

            // Linear interpolation
            float t = (voltage - v2) / (v1 - v2);

            return p2 + (int)((p1 - p2) * t);
        }
    }

    return 0;
}

static float readBatteryVoltsSamples(int samples) {
    uint32_t raw_sum = 0;
    int raw;
    int voltage_mv;

    for (int i = 0; i < samples; i++) {
        ESP_ERROR_CHECK(
            adc_oneshot_read(adc_handle, ADC_CHANNEL_9, &raw)
        );
        raw_sum += raw;

        // only delay for "slow mode"
        if (samples > 16) {
            vTaskDelay(pdMS_TO_TICKS(10));
        }
    }

    int raw_avg = raw_sum / samples;

    ESP_ERROR_CHECK(
        adc_cali_raw_to_voltage(adc_cali_handle, raw_avg, &voltage_mv)
    );

    return (voltage_mv / 1000.0f) * 2.0f;
}

static void battery_task(void *arg) {
    battery_adc_init();
    g_battery_voltage = readBatteryVoltsSamples(1);
    while (1) {        
        g_battery_voltage = readBatteryVoltsSamples(256);
        g_battery_percent = batteryPercentage(g_battery_voltage);
    }
}

void battery_start_task() {
    xTaskCreate(
        battery_task,
        "battery_task",
        4096,
        NULL,
        5,
        NULL
    );
}