#include "U8glib.h"
#include <Arduino_FreeRTOS.h>
#include <semphr.h>

#define PIN_TRIG 3
#define PIN_ECHO 2
#define PIN_BUTTON 5
#define PIN_BUZZER 8

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); // Fast I2C / TWI

volatile bool systemActive = true;
volatile int distancia = 0; 
SemaphoreHandle_t xMutex;
QueueHandle_t xQueue;

void setup() {
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  pinMode(PIN_BUTTON, INPUT_PULLUP);
  pinMode(PIN_BUZZER, OUTPUT);
  
  u8g.setFont(u8g_font_tpssb);
  u8g.setColorIndex(1); 
Serial.println("Iniciando");

  xMutex = xSemaphoreCreateMutex();
  xQueue = xQueueCreate(5, sizeof(int));

  //xTaskCreate(taskMonitorButton, "Monitor Button", 128, NULL, 1, NULL);
  xTaskCreate(taskMeasureDistance, "Measure Distance", 128, NULL, 1, NULL);
  xTaskCreate(taskDisplayDistance, "Display Distance", 128, NULL, 1, NULL);
  xTaskCreate(taskControlBuzzer, "Control Buzzer", 128, NULL, 1, NULL);

  vTaskStartScheduler();
}

void loop() {

}

/*void taskMonitorButton(void *pvParameters) {
  static bool lastButtonState = HIGH;
  bool buttonState;
  Serial.println("Probando");
  
  for (;;) {
    buttonState = digitalRead(PIN_BUTTON);

    if (buttonState == LOW && lastButtonState == HIGH) {
      systemActive = !systemActive;

      if (!systemActive) {
        xSemaphoreTake(xMutex, portMAX_DELAY);
        u8g.firstPage();
        do {
        } while (u8g.nextPage());
        xSemaphoreGive(xMutex);
        noTone(PIN_BUZZER);
      }

      vTaskDelay(50 / portTICK_PERIOD_MS);
    }
    lastButtonState = buttonState;
    vTaskDelay(10 / portTICK_PERIOD_MS);
  }
}*/

void taskMeasureDistance(void *pvParameters) {
  Serial.println("Probando medir distancia");
  for (;;) {
    if (systemActive) {
      digitalWrite(PIN_TRIG, HIGH);
      delayMicroseconds(10);
      digitalWrite(PIN_TRIG, LOW);

      long duration = pulseIn(PIN_ECHO, HIGH);
      int newDistance = duration / 58.2;

      xQueueSendToBack(xQueue, &newDistance, portMAX_DELAY);
    }
    vTaskDelay(100 / portTICK_PERIOD_MS);
  }
}

void taskDisplayDistance(void *pvParameters) {
  int localDistance;
  
  Serial.println("Probando display distancia");
  for (;;) {
    if (systemActive) {
      if (xQueueReceive(xQueue, &localDistance, portMAX_DELAY) == pdPASS) {
        xSemaphoreTake(xMutex, portMAX_DELAY);

        char buffer[10];
        sprintf(buffer, "%d cm", localDistance);

        u8g.firstPage();
        do {
          u8g.drawStr(25, 20, "Distancia:");
          u8g.drawStr(25, 40, buffer);
        } while (u8g.nextPage());

        xSemaphoreGive(xMutex);
      }
    } else {
      vTaskDelay(100 / portTICK_PERIOD_MS);
    }
  }
}

void taskControlBuzzer(void *pvParameters) {
  int localDistance;
  Serial.println("Probando control buzzer");

  for (;;) {
    if (systemActive) {
      if (xQueueReceive(xQueue, &localDistance, portMAX_DELAY) == pdPASS) {
        tone(PIN_BUZZER, 500, 5);

        if (localDistance > 49) {
          vTaskDelay(500 / portTICK_PERIOD_MS);
        } else {
          vTaskDelay(localDistance / portTICK_PERIOD_MS);
        }
      }
    } else {
      vTaskDelay(100 / portTICK_PERIOD_MS);
    }
  }
}