#include "esp_adc/adc_cali.h"
#include "esp_adc/adc_continuous.h"
#include "esp_adc/adc_cali_scheme.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/portmacro.h"
#include "freertos/task.h"
#include "math.h"

#include "hal/adc_types.h"
#include "measure.h"
#include "soc/soc_caps.h"


adc_continuous_handle_t init_measurement_inputs(measurement_input inputs[], int inputs_num){
  adc_continuous_handle_cfg_t adc_h_conf = {
    .conv_frame_size = SOC_ADC_DIGI_DATA_BYTES_PER_CONV, // One sample per interrupt for now
    .max_store_buf_size = SOC_ADC_DIGI_DATA_BYTES_PER_CONV * inputs_num * ADC_STORE_BUFFER_SIZE,
  };

  adc_continuous_handle_t handle;
  ESP_ERROR_CHECK(adc_continuous_new_handle(&adc_h_conf, &handle));

  adc_digi_pattern_config_t adc_patt[inputs_num];

  for(int i = 0; i < inputs_num; i++){
    adc_digi_pattern_config_t patt = {
      .atten = ADC_ATTEN_DB_0,
      .bit_width = ADC_BITWIDTH_12,
      .channel = inputs[i].channel,
      .unit = ADC_UNIT_1,
    };

    adc_patt[i] = patt;
  }
  
  adc_continuous_config_t adc_conf = {
    .adc_pattern = adc_patt,
    .pattern_num = inputs_num,
    .conv_mode = ADC_CONV_SINGLE_UNIT_1,
    .format = ADC_DIGI_OUTPUT_FORMAT_TYPE2,
    .sample_freq_hz = SOC_ADC_SAMPLE_FREQ_THRES_HIGH,
  };

  ESP_ERROR_CHECK(adc_continuous_config(handle, &adc_conf));

  return handle;
}

void init_conv_driver(measurement_input inputs[], int inputs_num, adc_cali_handle_t* out){
  for(int i = 0; i < inputs_num; i++){
    adc_cali_curve_fitting_config_t cali_config = {
      .unit_id = ADC_UNIT_1,
      .atten = ADC_ATTEN_DB_0,
      .bitwidth = ADC_BITWIDTH_12,
    };
    ESP_ERROR_CHECK(adc_cali_create_scheme_curve_fitting(&cali_config, &out[i]));
  }
}

void cali_get_one_chunk_avg(adc_continuous_handle_t handle, unsigned nb_samples, measurement_input inputs[], int nb_inputs, uint32_t meas[], uint32_t meas_nb[]){

  uint32_t samples_size = nb_samples * SOC_ADC_DIGI_DATA_BYTES_PER_CONV;
  
  uint8_t res_r[samples_size];
  uint32_t ret_num;

  ESP_ERROR_CHECK(adc_continuous_read(handle, res_r, samples_size, &ret_num, 100));
  if(ret_num != samples_size)
    ESP_LOGW("ZERO calibration", "Zero calibration : number of samples received not equal to requested %lu / %lu", ret_num, samples_size);

  uint32_t ret_nb = ret_num / SOC_ADC_DIGI_DATA_BYTES_PER_CONV;

  for(int i = 0; i < ret_nb; i++){
    adc_digi_output_data_t* res = (adc_digi_output_data_t*)(&res_r[i*sizeof(adc_digi_output_data_t)]);
    int index = -1;

    index = get_input_index_from_channel(res->type2.channel);

    if(index == -1)
      continue;
    
    meas[index] += res->type2.data;
    meas_nb[index]++;
  }
}

void start_zero_cali(measurements_ctxt* ctxt, unsigned int nb_samples, uint32_t offsets[]){
  uint32_t meas[ctxt->nb_inputs];
  uint32_t meas_nb[ctxt->nb_inputs];
  for(int i = 0; i < ctxt->nb_inputs; i++){
    meas[i] = 0;
    meas_nb[i] = 0;
  }

  for(int i = 0; i < nb_samples / ADC_STORE_BUFFER_SIZE; i++){
    unsigned int nsamp = fmin(nb_samples - (i * ADC_STORE_BUFFER_SIZE), ADC_STORE_BUFFER_SIZE);
    cali_get_one_chunk_avg(ctxt->handle, nsamp, ctxt->inputs, ctxt->nb_inputs, meas, meas_nb);
    vTaskDelay(10 / portTICK_PERIOD_MS);
  }

  ESP_LOGI("ZERO cali", "%lu %lu %lu", meas[0], meas[1], meas[2]);
  ESP_LOGI("ZERO cali", "%lu %lu %lu", meas_nb[0], meas_nb[1], meas_nb[2]);
  ESP_LOGI("ZERO cali", "%lu %lu %lu", meas[0]/meas_nb[0], meas[1]/meas_nb[1], meas[2]/meas_nb[2]);

  for(int i = 0; i < ctxt->nb_inputs; i++){
    offsets[i] = meas[i] / meas_nb[i];
  }
}

int get_input_index_from_channel(adc_channel_t channel){
  switch(channel){
    case ADC_CHANNEL_3:
      return 0;
    case ADC_CHANNEL_0:
      return 1;
    case ADC_CHANNEL_1:
      return 2;
    default:
      return -1;
  }
}