#include <ch.h>
#include <hal.h>
#include "stm32l_adc.h"

static const ADCConversionGroup g_adcconvgrp = {
    0,                                      // Circular
    2,                                      // Number of channels
    NULL,                                   // Callback
    NULL,                                   // Error callback
    0,                                      // CR1
    ADC_CR2_SWSTART | ADC_CR2_DELS_0,                        // CR2
    0,                                      // SMPR1
    ADC_SMPR2_SMP_SENSOR(ADC_SAMPLE_96) |
    ADC_SMPR2_SMP_VREF(ADC_SAMPLE_96),      // SMPR2
    0,                                      // SMPR3
    ADC_SQR1_NUM_CH(2),                     // SQR1
    0,                                      // SQR2
    0,                                      // SQR3
    0,                                      // SQR4
    ADC_SQR5_SQ1_N(ADC_CHANNEL_SENSOR) |
    ADC_SQR5_SQ2_N(ADC_CHANNEL_VREFINT)     // SQR5
};

void adc_on()
{
    RCC->CR |= RCC_CR_HSION;
    while ((RCC->CR & RCC_CR_HSIRDY) == 0)
        chThdSleepMilliseconds(1);
    
    adcStart(&ADCD1, NULL);
    ADC->CCR = ADC_CCR_TSVREFE | ADC_CCR_ADCPRE_1;
    chThdSleepMilliseconds(5);
}

void adc_off()
{
    ADC->CCR &= ~ADC_CCR_TSVREFE;
    adcStop(&ADCD1);
    RCC->CR &= ~RCC_CR_HSION;
}

int adc_vbat()
{
    adcsample_t buf[2] = {};
    adcConvert(&ADCD1, &g_adcconvgrp, buf, 1);
    
    // buf[1] = 4096 * 1224mV / Vbat
    // => Vbat = 1224mV * 4096 / buf[1]
    return 1224 * 4096 / buf[1];
}

int adc_temperature()
{
    int sum_vref = 0;
    int sum_temp = 0;
    int count = 0;
    
    for (int i = 0; i < 32; i++)
    {
        adcsample_t buf[32] = {};
        adcConvert(&ADCD1, &g_adcconvgrp, buf, 16);
        
        for (int j = 0; j < 16; j++)
        {
            sum_temp += buf[j * 2];
            sum_vref += buf[j * 2 + 1];
            count++;
        }
    }
    
    // Convert measurements to microvolts based on calibrated Vref value.
    int64_t vref_uV = (int64_t)3000000 * (*(uint16_t*)0x1FF80078) / 4096;
    int64_t temp_uV = vref_uV * sum_temp / sum_vref;
    
    // Get the temperature calibration values
    int64_t temp_110c_uV = (int64_t)3000000 * (*(uint16_t*)0x1FF8007E) / 4096;
    int64_t temp_25c_uV  = (int64_t)3000000 * (*(uint16_t*)0x1FF8007A) / 4096;
    
    // Convert to millicelcius
    int64_t uV_per_C = (temp_110c_uV - temp_25c_uV) / (110 - 25);
    int millicelcius = 25000 + 1000 * (temp_uV - temp_25c_uV) / uV_per_C;
    
    return millicelcius;
}