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


#define WAITING_CAR GPIO_NUM_18 
#define CAR_LEFT GPIO_NUM_19

#define SPIKE_DOWN GPIO_NUM_21
#define SPIKE_MIDDLE GPIO_NUM_22
#define SPIKE_UP GPIO_NUM_23

#define LED_CHANGE_DELAY_MS 1000
#define DEBOUNCER_DELAY_uS  4000

#define MIDDLE_TIME_LED     2000000

QueueHandle_t handlerQueue;

static int64_t past_time = 0;
static int64_t current_time = 0;
static int64_t middle_led_time = 0;

static uint8_t bounce_rectifier = 0xFF;

enum spike_states {
  down = 0,
  middle, 
  up
}; 

void gpio_init () {

  gpio_reset_pin(WAITING_CAR);
  gpio_reset_pin(CAR_LEFT);

  gpio_reset_pin(SPIKE_DOWN);
  gpio_reset_pin(SPIKE_MIDDLE);
  gpio_reset_pin(SPIKE_UP);

  gpio_set_direction(WAITING_CAR, GPIO_MODE_INPUT);
  gpio_set_direction(CAR_LEFT, GPIO_MODE_INPUT);

  gpio_set_direction(SPIKE_DOWN, GPIO_MODE_OUTPUT);
  gpio_set_direction(SPIKE_MIDDLE, GPIO_MODE_OUTPUT);
  gpio_set_direction(SPIKE_UP, GPIO_MODE_OUTPUT);

}

static void IRAM_ATTR gpio_interrupt_handler(void *args) {
  int pinNumber = (int)args;

  if ( (current_time - past_time) >= DEBOUNCER_DELAY_uS ) {
    past_time = current_time;

    bounce_rectifier++;

    if (bounce_rectifier & 1) {
      xQueueSendFromISR(handlerQueue, &pinNumber, NULL);
    }
  }
} 
/*
void spike_activation (uint8_t state, uint8_t level) {
  gpio_set_level(state, level);
}
*/
static void  spike_states_task (void * params) {
  int pinNumber = 0;
  enum spike_states states;
  states = down;

  while (1) {
    
    current_time = esp_timer_get_time();
    
    xQueueReceive(handlerQueue , &pinNumber, 0);

      switch ( states ) {
        case down: 
          if (pinNumber == WAITING_CAR) {
            gpio_set_level(SPIKE_DOWN , 0);
            middle_led_time = current_time;
            states ++;
          }
          else {
            gpio_set_level(SPIKE_DOWN , 1);
          }
          break;
        case middle: 

          gpio_set_level(SPIKE_MIDDLE , 1);

          if ( (pinNumber == WAITING_CAR) && ((current_time - middle_led_time)>MIDDLE_TIME_LED) ) {
            gpio_set_level(SPIKE_MIDDLE , 0);
            states ++;
          }
          else if ( (pinNumber == CAR_LEFT) && ((current_time - middle_led_time)>MIDDLE_TIME_LED) ){
            gpio_set_level(SPIKE_MIDDLE , 0);
            states --;
          }
          break;
        case up: 
          if ( pinNumber == CAR_LEFT ){
            gpio_set_level(SPIKE_UP , 0);
            middle_led_time = current_time;
            states --;
          }
          else {
            gpio_set_level(SPIKE_UP , 1);
          }
          break; 
      }
  }
}


void app_main() {
  
  gpio_init();

  gpio_set_intr_type(WAITING_CAR, GPIO_INTR_NEGEDGE);
  gpio_set_intr_type(CAR_LEFT, GPIO_INTR_NEGEDGE);

  handlerQueue = xQueueCreate(10, 1);

  xTaskCreate(spike_states_task , "spike_states_task" , 2048 , NULL , 1 , NULL);

  gpio_install_isr_service(0);

  gpio_isr_handler_add(WAITING_CAR, gpio_interrupt_handler, (void *) WAITING_CAR);
  gpio_isr_handler_add(CAR_LEFT, gpio_interrupt_handler, (void *) CAR_LEFT);

}