#include <Arduino.h>
#include <ultrasonic.h">

#define LEDVR 15
#define LEDAM 2
#define LEDVD 0
#define VR 0
#define VD 1
#define AM 2

int Trig_PIN = 14;
int Echo_PIN = 12;

TaskHandle_t sensorTask;
#define GPIO_OUTPUT_PIN_SEL ((1ULL << LEDVR) | (1ULL << LEDVD) | (1ULL << LEDAM))
static TaskHandle_t xtask_handle_leds = NULL;
int state = VR;

void init_leds() {
  // Configura os três LEDs
  gpio_config_t led_conf = {};
  led_conf.intr_type = GPIO_INTR_DISABLE;
  led_conf.mode = GPIO_MODE_OUTPUT;
  led_conf.pin_bit_mask = GPIO_OUTPUT_PIN_SEL;
  led_conf.pull_down_en = GPIO_PULLDOWN_ENABLE;
  led_conf.pull_up_en = GPIO_PULLDOWN_DISABLE;
  gpio_config(&led_conf);
}

int controla(int *state) {
  if (*state == VR) {
    digitalWrite(LEDVR, HIGH);
    digitalWrite(LEDVD, LOW);
    digitalWrite(LEDAM, LOW);
    *state = VD;
  } else if (*state == AM) {
    digitalWrite(LEDVR, LOW);
    digitalWrite(LEDVD, LOW);
    digitalWrite(LEDAM, HIGH);
    *state = VR;
    return 1;
  } else {
    digitalWrite(LEDVR, LOW);
    digitalWrite(LEDVD, HIGH);
    digitalWrite(LEDAM, LOW);
    *state = AM;
    return 10;
  }
}

void semaforo(void *pvParameters) {
  while (1) {
    int time = controla(&state);
    vTaskDelay((1000 * time) / portTICK_RATE_MS);
  }
}

void sensorultra(void *pvParameters) {
  float distancia;
  ultrasonic_sensor_t sensor = {
      .trigger_pin = Trig_PIN,
      .echo_pin = Echo_PIN,
  };

  ultrasonic_init(&sensor);
  int pause = 0;

  while (1) {
    esp_err_t res = ultrasonic_measure(&sensor, 400, &distancia);
    if (res == ESP_OK) {
      Serial.print("Distance: ");
      Serial.println(distancia);
      if (distancia < 1 && pause == 0) {
        vTaskSuspend(xtask_handle_leds);
        digitalWrite(LEDVR, HIGH);
        digitalWrite(LEDVD, LOW);
        digitalWrite(LEDAM, LOW);
        state = VR;
        pause = 1;
      } else if (distancia >= 1 && pause == 1) {
        vTaskResume(xtask_handle_leds);
        pause = 0;
      }
    } else {
      Serial.print("Error ");
      Serial.println(res);
    }
    vTaskDelay(pdMS_TO_TICKS(500));  // Espera 200 milissegundos
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(LEDVR, OUTPUT);
  pinMode(LEDAM, OUTPUT);
  pinMode(LEDVD, OUTPUT);
  pinMode(Trig_PIN, OUTPUT);
  pinMode(Echo_PIN, INPUT);

  xTaskCreatePinnedToCore(sensorultra, "sensorTask", configMINIMAL_STACK_SIZE * 3, NULL, 5, NULL);
  xTaskCreatePinnedToCore(semaforo, "semaforoTask", 2048, NULL, 5, &xtask_handle_leds);
}

void loop() {}