#include <stdint.h>
#include <stdatomic.h>
#include "analog.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "driver/adc.h"
#include "esp_adc_cal.h"

static esp_adc_cal_characteristics_t *adc_chars;

// 32-39
static const adc1_channel_t channel = ADC1_CHANNEL_4;     //GPIO34 if ADC1, GPIO14 if ADC2

static float measurement_celsius;
static const adc_atten_t atten = ADC_ATTEN_DB_0;
static const adc_unit_t unit = ADC_UNIT_1;

float reg_meas_history[REG_HISTORY_LEN] = {};
float reg_tset_history[REG_HISTORY_LEN] = {};
uint32_t history_counter = 0;

// TODO move to regulator module (make extern)
float reg_setpoint = 125;

static void analog_service(void *arg);

static TaskHandle_t hAnalog;

#define DEFAULT_VREF 1100
#define NO_OF_SAMPLES 64

void analog_init() {
    printf("Analog init\n");

    adc1_config_width(ADC_WIDTH_BIT_12);
    adc1_config_channel_atten(channel, atten);
    adc_chars = calloc(1, sizeof(esp_adc_cal_characteristics_t));
    esp_adc_cal_characterize(unit, atten, ADC_WIDTH_BIT_12, DEFAULT_VREF, adc_chars);

    int rv = xTaskCreate(analog_service, "analog", 4096, NULL, 6, &hAnalog);
    assert (rv == pdPASS);
}


static void __attribute__((noreturn)) analog_service(void *arg) {
    while (1) {
        uint32_t adc_reading = 0;
        //Multisampling
        for (int i = 0; i < NO_OF_SAMPLES; i++) {
            adc_reading += adc1_get_raw(channel);
        }
        adc_reading /= NO_OF_SAMPLES;

        //Convert adc_reading to voltage in mV
        uint32_t voltage = esp_adc_cal_raw_to_voltage(adc_reading, adc_chars);

#define CORRECT -10;
        voltage += CORRECT;

//        printf("Raw: %d ... Voltage: %dmV ...", adc_reading, voltage);

        float volts = voltage * 0.001f;

#define R1 4750
#define V1 3.3f
        float r_pt100 = (volts * R1)/(V1 - volts);
        float celsius = (r_pt100/100.0f - 1.0f) / 3.9083E-3f;
//        printf("Rpt %.3f, Celsius: %.1f\n", r_pt100, celsius);

        measurement_celsius = celsius;

        for (int i = 0; i < REG_HISTORY_LEN-1; i++) {
            reg_meas_history[i] = reg_meas_history[i+1];
            reg_tset_history[i] = reg_tset_history[i+1];
        }
        reg_meas_history[REG_HISTORY_LEN-1] = celsius;
        reg_tset_history[REG_HISTORY_LEN-1] = reg_setpoint;
        history_counter = (history_counter + 1) % 20;

        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

float analog_read() {
    return measurement_celsius;
}