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


typedef enum {
  Floor1DoorOpen,
  Floor1DoorClosed,
  Floor2DoorOpen,
  Floor2DoorClosed
} State;

typedef enum {
  DoorOpen,
  DoorClosed,
  Up,
  Down
} Event;

typedef struct {
  State old_state;
  Event trigger;
  State new_state;
} Transition;

Transition all_transitions[] = {
  {Floor1DoorOpen, DoorOpen, Floor1DoorOpen},
  {Floor1DoorOpen, DoorClosed, Floor1DoorClosed},
  {Floor1DoorClosed, DoorOpen, Floor1DoorOpen},
  {Floor1DoorClosed, DoorClosed, Floor1DoorClosed},
  {Floor1DoorClosed, Up, Floor2DoorClosed},
  {Floor2DoorClosed, DoorOpen, Floor2DoorOpen},
  {Floor2DoorClosed, Down, Floor1DoorClosed},
  {Floor2DoorOpen, DoorOpen, Floor2DoorOpen},
  {Floor2DoorOpen, DoorClosed, Floor2DoorClosed}
};

gpio_num_t buttons[]{
  GPIO_NUM_15, //Open Door Button
  GPIO_NUM_2, //Close Door Button
  GPIO_NUM_12, //Up Button
  GPIO_NUM_13, //Down Button
};
gpio_num_t indicators[]{
  GPIO_NUM_18, //Door is Open
  GPIO_NUM_5, //Door is Closed
};

void set_up_pins(){
  for(int i = 0; i < 4; i++){
    gpio_reset_pin(buttons[i]);
    gpio_set_direction(buttons[i], GPIO_MODE_INPUT);
  }
  for(int i = 0; i < 2; i++){
    gpio_reset_pin(indicators[i]);
    gpio_set_direction(indicators[i],GPIO_MODE_OUTPUT);
  }
}

const char * enum_to_string(State state){
  switch(state){
    case Floor1DoorOpen: return "Door is open! Currently on Floor 1!";
    case Floor1DoorClosed: return "Door is closed! Currently on Floor 1!";
    case Floor2DoorOpen: return "Door is open! Currently on Floor 2!";
    case Floor2DoorClosed: return "Door is closed! Currently on Floor 2!";
  }

}

void event_handler(Event current_event, State *current_state){
  //printf("reaching handler\n");
  for(int i = 0; i < 9; i++){
    if(*current_state == all_transitions[i].old_state && current_event == all_transitions[i].trigger){
      *current_state = all_transitions[i].new_state;
      printf("%s\n", enum_to_string(all_transitions[i].new_state));
    }
  }
}


void button_input(void *(arg)){
  State current_state = Floor1DoorOpen;
  //printf("Initial state is: %s\n", enum_to_string(current_state));
  while(1){
    if(gpio_get_level(GPIO_NUM_15) == 0){
      event_handler(DoorOpen,&current_state);
      gpio_set_level(GPIO_NUM_18,1);
      gpio_set_level(GPIO_NUM_5,0);
    }
    if(gpio_get_level(GPIO_NUM_2) == 0){
      event_handler(DoorClosed,&current_state);
      gpio_set_level(GPIO_NUM_5,1);
      gpio_set_level(GPIO_NUM_18,0);
    }
    if(gpio_get_level(GPIO_NUM_13) == 0){
      event_handler(Up,&current_state);
    }
    if(gpio_get_level(GPIO_NUM_12) == 0){
      event_handler(Down,&current_state);
    }
    vTaskDelay(500/portTICK_PERIOD_MS);
  }
}


extern "C" void app_main(){
  set_up_pins();
  printf("Hello!\nWelcome to elevator simulator, press red button to close door and green to open it.\n");
  printf("Once inside please make sure to close the door before hitting Up (blue button) or Down (yellow button)!\n");
  gpio_set_level(GPIO_NUM_18,1);
  xTaskCreate(&button_input, "inputelevator",2048, NULL, 5, NULL);


}