#include <stdio.h>
#include <inttypes.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"

#define LED_BARRERA_ABAJO GPIO_NUM_4
#define LED_BARRERA_EN_MEDIO GPIO_NUM_13
#define LED_BARRERA_ARRIBA GPIO_NUM_14
#define BOTON_CARRO_ESPERANDO GPIO_NUM_16
#define BOTON_CARRO_PASO GPIO_NUM_17

typedef enum {
    eStart = 0,
    eDown,
    eMiddle,
    eUp
} PosSatates;

void init_gpio(void);
uint8_t checkBtn(uint8_t);

void app_main() {
  PosSatates currentPos = eStart;
  uint8_t carro_esperando = 0;
  while (true) {
    switch (currentPos) {
      case eStart:
        init_gpio(); // Configurar los puertos de entrada y salida.
        // Apagar todos los LEDs menos el de la barrera abajo.
        gpio_set_level(LED_BARRERA_ABAJO, 1);
        gpio_set_level(LED_BARRERA_EN_MEDIO, 0);
        gpio_set_level(LED_BARRERA_ARRIBA, 0);
        currentPos = eDown;
        printf("Go!\n");
        break;
      case eDown:
        if (checkBtn(BOTON_CARRO_ESPERANDO)) {
          printf("Hay un carro en espera!\n");
          carro_esperando = 1;
          gpio_set_level(LED_BARRERA_ABAJO, 0);
          gpio_set_level(LED_BARRERA_EN_MEDIO, 1);
          currentPos = eMiddle;
        }
        break;
      case eMiddle:
        vTaskDelay(3000 / portTICK_PERIOD_MS);
        gpio_set_level(LED_BARRERA_EN_MEDIO, 0);
        if (carro_esperando) {
          gpio_set_level(LED_BARRERA_ARRIBA, 1);
          currentPos = eUp;
        }   
        else {
          gpio_set_level(LED_BARRERA_ABAJO, 1);
          currentPos = eDown;
        }
        break;
      case eUp:
        if (checkBtn(BOTON_CARRO_PASO)) {
          printf("El carro ha cruzado!\n");
          carro_esperando = 0;
          gpio_set_level(LED_BARRERA_ARRIBA, 0);
          gpio_set_level(LED_BARRERA_EN_MEDIO, 1);
          currentPos = eMiddle;
        }
        break;
      default:
        break; // Manejo de estado no esperado
    }
  }
}

void init_gpio(void) {
  // Configurar los pines de los leds como salida.
  gpio_reset_pin(LED_BARRERA_ABAJO);
  gpio_set_direction(LED_BARRERA_ABAJO, GPIO_MODE_OUTPUT);
  gpio_set_pull_mode(LED_BARRERA_ABAJO, GPIO_FLOATING);

  gpio_reset_pin(LED_BARRERA_EN_MEDIO);
  gpio_set_direction(LED_BARRERA_EN_MEDIO, GPIO_MODE_OUTPUT);
  gpio_set_pull_mode(LED_BARRERA_EN_MEDIO, GPIO_FLOATING);

  gpio_reset_pin(LED_BARRERA_ARRIBA);
  gpio_set_direction(LED_BARRERA_ARRIBA, GPIO_MODE_OUTPUT);
  gpio_set_pull_mode(LED_BARRERA_ARRIBA, GPIO_FLOATING);

  // Configurar los pines de los botones como entrada y activar pull-down.
  gpio_reset_pin(BOTON_CARRO_ESPERANDO);
  gpio_set_direction(BOTON_CARRO_ESPERANDO, GPIO_MODE_INPUT);
  gpio_set_pull_mode(BOTON_CARRO_ESPERANDO, GPIO_PULLDOWN_ONLY);

  gpio_reset_pin(BOTON_CARRO_PASO);
  gpio_set_direction(BOTON_CARRO_PASO, GPIO_MODE_INPUT);
  gpio_set_pull_mode(BOTON_CARRO_PASO, GPIO_PULLDOWN_ONLY);
}

uint8_t checkBtn(uint8_t pinNumber) {
  vTaskDelay(50 / portTICK_PERIOD_MS); // Debounce
  if (gpio_get_level(pinNumber)) {
    while(gpio_get_level(pinNumber));
    return 1;
  }
  return  0;
}