#include <esp_system.h>
#include <esp_log.h>

static const uint16_t timer_divider = 80;
static const uint64_t timer_max_count = 1000000;
static const TickType_t task_delay = 1000 / portTICK_PERIOD_MS;
static hw_timer_t *timer = NULL;

EventGroupHandle_t evt;
#define EV_1SEG (1<<0)

QueueHandle_t queue;
SemaphoreHandle_t sequenceSemaphore;

int q_sensor = 10;
int pin[2] = {34, 35};
int button = 32;
static volatile uint16_t sensor1;
static volatile uint16_t sensor2;

struct sensor {
  int deviceId;
  int measurementType;
  float value;
};

void IRAM_ATTR onTimer() {
  BaseType_t task_woken = pdFALSE;

  Serial.println("----------------");
  Serial.println("Sending data...");
  sensor1 = analogRead(pin[0]);
  Serial.println(sensor1); 
  sensor2 = analogRead(pin[1]);
  Serial.println(sensor2); 
  Serial.println("----------------");

  xSemaphoreGiveFromISR(sequenceSemaphore, &task_woken);

  if (task_woken) {
    portYIELD_FROM_ISR();
  }
}

void buttonPressed (void *args) {
  while (1) {
    if(xSemaphoreTake(sequenceSemaphore, portMAX_DELAY) != pdTRUE) {
      Serial.println("Erro taking the semaphore in buttonPressed");
    }

    if (digitalRead(button) == 0) {
      Serial.println("Button pressed \n Sending data...");
    }

    //delay(100);
  }
}

void readSensor (void *args) {
  EventBits_t eventBits;

  if(queue == NULL || sequenceSemaphore == NULL){
    Serial.println("Queue or semaphore were not created");
    vTaskDelete(NULL);
  }

  int i = 0;
  while(1) {
    eventBits = xEventGroupWaitBits(evt, EV_1SEG, true, true, pdMS_TO_TICKS(1000));

    if (eventBits & EV_1SEG) {
      struct sensor mySensor;
      mySensor.deviceId = i%2;
      mySensor.measurementType = pin[i%2];
      mySensor.value = 20;
  
      if (xQueueSend(queue, &mySensor, portMAX_DELAY) != pdTRUE) {
        Serial.println("Error seding the queue");
      }

      i++;
      xSemaphoreGive(sequenceSemaphore);

      ESP_LOGI("ReadSensor", "OK");
      Serial.println("ReadSensor OK");

      vTaskDelay(pdMS_TO_TICKS(400));
    } else {
      ESP_LOGE("ReadSensor", "Event group TIMEOUT");
      Serial.println("ReadSensor Timeout");
    }
  }
}

void printSensor (void *args) {
  struct sensor element;
 
  while(1) {
    if (xSemaphoreTake(sequenceSemaphore, portMAX_DELAY) != pdTRUE) {
      Serial.println("Error taking the semaphore");
    }

    if(queue == NULL) {
      Serial.println("Error creating the mutex");
      vTaskDelete(NULL);
    }

    xQueueReceive(queue, &element, portMAX_DELAY);
 
    Serial.print("Device ID: ");
    Serial.println(element.deviceId);
 
    Serial.print("Measurement type: ");
    Serial.println(element.measurementType);
 
    Serial.print("Value: ");
    Serial.println(element.value);
 
    Serial.println("----------------");
  }
  delay(50);
  Serial.println();
}
 
void setup() {
  Serial.begin(115200);

  vTaskDelay(1000 / portTICK_PERIOD_MS);
  Serial.println();
  
  pinMode(button, INPUT_PULLUP);
 
  queue = xQueueCreate(q_sensor, sizeof(struct sensor));
  sequenceSemaphore = xSemaphoreCreateBinary();
 
  if(queue == NULL){
    Serial.println("Error creating the queue");
  }

  evt = xEventGroupCreate();

  xTaskCreatePinnedToCore(readSensor, "readSensor", 1024, NULL, 1, NULL, 0);
  while(1){
    for (uint8_t i = 0; i < 2; i++) {
        vTaskDelay(pdMS_TO_TICKS(333));
        xEventGroupSetBits(evt, EV_1SEG);
    }

    xTaskCreatePinnedToCore(printSensor, "printSensor", 1024, NULL, 1, NULL, 0);
    xTaskCreatePinnedToCore(buttonPressed, "buttonPressed", 1024, NULL, 1, NULL, 0);

    timer = timerBegin(0, timer_divider, true);
    timerAttachInterrupt(timer, &onTimer, true);
    timerAlarmWrite(timer, timer_max_count, true);
    timerAlarmEnable(timer);

    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}
 
void loop() {
 
}