#include <string.h>
#include <unistd.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "driver/adc.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "esp32-gp2y.h"

int32_t gp2y_read(gp2y_cfg_t *cfg, uint8_t samples) {
  int32_t accum = 0;

  for(uint8_t i = 0; i < samples; i++) {
    portMUX_TYPE myMutex = portMUX_INITIALIZER_UNLOCKED;
    taskENTER_CRITICAL(&myMutex);

    gpio_set_level(cfg->pin_led, 0);
    usleep(280);

    accum += adc1_get_raw(cfg->adc_channel);

    gpio_set_level(cfg->pin_led, 1);

    taskEXIT_CRITICAL(&myMutex);

    usleep(9620);
  }

  float avg_adc = (accum / samples);
  float avg_volt = avg_adc / 4095 * 3.3;
  float dV = avg_volt - cfg->offset_voltage;

  // recalibrate offset_volt to lowest value seen
  if(dV < 0) {
    dV = 0;
    cfg->offset_voltage = avg_volt;
  }

  return dV / cfg->K * 100.0;
}