#include "analog_read.h"

#include <Arduino.h>
#include <FreeRTOS_SAMD21.h>
#include <wiring_analog_SAMD_TT.h>
#include <wiring_private.h>

#include "mapping.h"
#include "priority.h"
#include "util.h"

void analog_read_task(void *pvParameters);

TaskHandle_t analog_read_task_handle;
SemaphoreHandle_t analog_read_mutex;
AnalogReadData analog_read_data;

void setup_analog_read() {
  analogReadResolution_SAMD_TT(12);
  analogReference_SAMD_TT(AR_DEFAULT);

  xTaskCreate(analog_read_task, "analog_read_task", 256, NULL, ANALOG_READ_PRIORITY, &analog_read_task_handle);

  analog_read_mutex = xSemaphoreCreateMutex();
}

void analog_read_task(void *pvParameters) {
  for(;;) {
    xSemaphoreTake(analog_read_mutex, portMAX_DELAY);
    analog_read_data.preamp += 0.1 * ((float) analogRead_SAMD_TT(PREAMP_PIN) - analog_read_data.preamp);
    analog_read_data.buzz += 0.1 * ((float) analogRead_SAMD_TT(BUZZ_PIN) - analog_read_data.buzz);
    analog_read_data.punch += 0.1 * ((float) analogRead_SAMD_TT(PUNCH_PIN) - analog_read_data.punch);
    analog_read_data.crunch += 0.1 * ((float) analogRead_SAMD_TT(CRUNCH_PIN) - analog_read_data.crunch);
    analog_read_data.drive += 0.1 * ((float) analogRead_SAMD_TT(DRIVE_PIN) - analog_read_data.drive);
    analog_read_data.level += 0.1 * ((float) analogRead_SAMD_TT(LEVEL_PIN) - analog_read_data.level);
    analog_read_data.low += 0.1 * ((float) analogRead_SAMD_TT(LOW_PIN) - analog_read_data.low);
    analog_read_data.high += 0.1 * ((float) analogRead_SAMD_TT(HIGH_PIN) - analog_read_data.high);
    xSemaphoreGive(analog_read_mutex);
    delay_ms(30);
  }
}

AnalogReadData get_analog_read_data() {
  xSemaphoreTake(analog_read_mutex, portMAX_DELAY);
  AnalogReadData copy = analog_read_data;
  xSemaphoreGive(analog_read_mutex);
  return copy;
}