// #include's
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "driver/rtc_io.h"
#include <ESP32Servo.h>
#include "fsm.h"

// #define's
#define DOOR1_SERVO GPIO_NUM_13
#define DOOR2_SERVO GPIO_NUM_18
#define ELEVATOR_SERVO GPIO_NUM_14
#define SERVO_MIN_PULSE 500
#define SERVO_MAX_PULSE 2400
#define DEBOUNCE_TIME_MS 250
#define MAX_NAME_SIZE 32
#define BUTTON1_GPIO GPIO_NUM_15
#define BUTTON2_GPIO GPIO_NUM_5
#define BUTTON3_GPIO GPIO_NUM_12
#define BUTTON4_GPIO GPIO_NUM_26
#define LED_GPIO GPIO_NUM_2
#define LED2_GPIO GPIO_NUM_4
#define BUTTON_ISR_PRIORITY 2
#define QUEUE_LENGTH 10
#define OPEN 1
#define CLOSE 0
#define UP OPEN
#define DOWN CLOSE
#define ON 1
#define OFF 0
#define DELAY_TIME 100


// Function Prototypes
extern "C" void print_fsm_graph(Fsm *);
extern "C" void init_fsm(Fsm *);
extern "C" void process_event(Fsm *, Event);
void init_pins();
static void IRAM_ATTR button_isr_handler(void *);
void handle_exit();
void handle_event();
void open_door_1();
void close_door_1();
void open_door_2();
void close_door_2();
void send_elevator_down();
void send_elevator_up();
void send_event(Event e);
void servo_action(Servo, int);

// Globals
// GPIO's
gpio_num_t button_gpio[] = { BUTTON1_GPIO, BUTTON2_GPIO, BUTTON3_GPIO, BUTTON4_GPIO};
gpio_num_t led_gpio[] = { LED_GPIO, LED2_GPIO};
// Queue's
QueueHandle_t intr_queue;
QueueHandle_t elevator_event_queue;
// Servo's
Servo servo_door1, servo_door2, servo_elevator;
// FSM, States, Events
// List of States
State elevator_states[] = {
  {"INIT"},
  {"IN FLOOR 1 DOOR OPENED"},
  {"IN FLOOR 1 DOOR CLOSED"},
  {"IN FLOOR 2 DOOR OPENED"},
  {"IN FLOOR 2 DOOR CLOSED"},
  {"GOING UP"},
  {"GOING DOWN"},
  {"OPENING DOOR 1"},
  {"CLOSING DOOR 1"},
  {"OPENING DOOR 2"},
  {"CLOSING DOOR 2"}
};
// List of Events
Event elevator_events[] = {
  {"UP PRESSED"},
  {"DOWN PRESSED"},
  {"ARRIVED AT FLOOR 1"},  
  {"ARRIVED AT FLOOR 2"}, 
  {"OPEN DOOR 1 PRESSED"},
  {"CLOSE DOOR 1 PRESSED"},
  {"DOOR 1 OPENED"},  
  {"DOOR 1 CLOSED"},
  {"OPEN DOOR 2 PRESSED"},
  {"CLOSE DOOR 2 PRESSED"},
  {"DOOR 2 OPENED"},  
  {"DOOR 2 CLOSED"} 
};
// Transitions 
Transition elevator_transitions[] = {
  {"IN FLOOR 1 DOOR OPENED",    "CLOSE DOOR 1 PRESSED",   "CLOSING DOOR 1",            close_door_1, handle_exit},
  {"CLOSING DOOR 1",            "DOOR 1 CLOSED",          "IN FLOOR 1 DOOR CLOSED",    handle_event, handle_exit},
  {"IN FLOOR 1 DOOR CLOSED",    "OPEN DOOR 1 PRESSED",    "OPENING DOOR 1",            open_door_1, handle_exit},
  {"OPENING DOOR 1",            "DOOR 1 OPENED",          "IN FLOOR 1 DOOR OPENED",    handle_event, handle_exit},  

  {"IN FLOOR 2 DOOR OPENED",    "CLOSE DOOR 2 PRESSED",   "CLOSING DOOR 2",            close_door_2, handle_exit},
  {"CLOSING DOOR 2",            "DOOR 2 CLOSED",          "IN FLOOR 2 DOOR CLOSED",    handle_event, handle_exit},
  {"IN FLOOR 2 DOOR CLOSED",    "OPEN DOOR 2 PRESSED",    "OPENING DOOR 2",            open_door_2, handle_exit},
  {"OPENING DOOR 2",            "DOOR 2 OPENED",          "IN FLOOR 2 DOOR OPENED",    handle_event, handle_exit},

  {"IN FLOOR 1 DOOR CLOSED",    "UP PRESSED",             "GOING UP",                  send_elevator_up, handle_exit},
  {"GOING UP",                  "ARRIVED AT FLOOR 2",      "OPENING DOOR 2",           open_door_2, handle_exit},  
  {"IN FLOOR 2 DOOR CLOSED",    "DOWN PRESSED",           "GOING DOWN",                send_elevator_down, handle_exit},
  {"GOING DOWN",                "ARRIVED AT FLOOR 1",     "OPENING DOOR 1",            open_door_1, handle_exit}

}; 
// FSM
Fsm elevator_fsm = {
    elevator_states,
    elevator_events,
    elevator_transitions,
    sizeof(elevator_states)/sizeof(State),
    sizeof(elevator_events)/sizeof(Event),
    sizeof(elevator_transitions) / sizeof(Transition),  
    "IN FLOOR 1 DOOR OPENED"
  };

// Function Declarations
static void IRAM_ATTR button_isr_handler(void *arg) {
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  static int64_t last_interrupt_time = 0;
  int64_t current_time = esp_timer_get_time();

  // Check if the debounce time has passed since the last interrupt
  if ((current_time - last_interrupt_time) > (DEBOUNCE_TIME_MS * 1000)) {
    xQueueSendFromISR(intr_queue, arg, &xHigherPriorityTaskWoken);
    if (xHigherPriorityTaskWoken) {
      portYIELD_FROM_ISR();
    }
  }
  last_interrupt_time = current_time;
}

void handle_event() { /*printf("handle_event:: Entering state %s \n", elevator_fsm.current_state);*/ }
void handle_exit() { /*printf("handle_exit:: Exiting state %s \n\n\n", elevator_fsm.current_state);*/ }

void send_event(Event e){
  //printf("send_event:: event %s is being sent!\n", e.name);
  xQueueSend(elevator_event_queue, &e, (TickType_t) 0);
}


void servo_action(Servo * s, int action) {
  int start_position = (action == OPEN)? 180: 0;
  int end_position = (action == OPEN)? 0: 180;
  int direction = (action == OPEN)? -1 : 1;
  int pos;
  //printf("servo action:: start: %d   end: %d   direction: %d\n", start_position, end_position, direction);
  for (pos = start_position; pos != end_position; pos += direction) {
    s->write(pos);
    delay(20);
  }
}
void led_on(gpio_num_t gpio) { gpio_set_level(gpio, ON);}
void led_off(gpio_num_t gpio) { gpio_set_level(gpio, OFF );}


void open_door_1() {servo_action(&servo_door1, OPEN);send_event((Event) {"DOOR 1 OPENED"}); led_on(LED2_GPIO);}
void close_door_1() {servo_action(&servo_door1, CLOSE); send_event((Event) {"DOOR 1 CLOSED"}); led_on(LED2_GPIO);}
void open_door_2() {servo_action(&servo_door2, OPEN); send_event((Event) {"DOOR 2 OPENED"}); led_on(LED_GPIO);}
void close_door_2() {servo_action(&servo_door2, CLOSE); send_event((Event) {"DOOR 2 CLOSED"}); led_on(LED_GPIO);}
void send_elevator_down() {servo_action(&servo_elevator, DOWN);send_event((Event) {"ARRIVED AT FLOOR 1"});led_off(LED_GPIO);led_on(LED2_GPIO);}
void send_elevator_up() {servo_action(&servo_elevator, UP );send_event((Event) {"ARRIVED AT FLOOR 2"});led_off(LED2_GPIO);led_on(LED_GPIO);}

void init_pins() {
  // initialize LED PINs
  for(int i = 0; i < sizeof(led_gpio)/sizeof(gpio_num_t); i++) {
    gpio_reset_pin(led_gpio[i]);
    gpio_set_direction(led_gpio[i], GPIO_MODE_OUTPUT);
  }
  gpio_set_level(LED2_GPIO, ON);

  // Initialize Button Pin and install interrupt service
  gpio_install_isr_service(0);
  for(int i = 0; i < sizeof(button_gpio)/sizeof(gpio_num_t); i++){
    gpio_reset_pin(button_gpio[i]);
    gpio_set_direction(button_gpio[i], GPIO_MODE_INPUT);
    gpio_set_intr_type(button_gpio[i], GPIO_INTR_NEGEDGE);
    gpio_isr_handler_add(button_gpio[i], button_isr_handler, (void *)&button_gpio[i]);
  }
  // Initialize servo motors
  servo_door1.attach(DOOR1_SERVO, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  servo_door1.write(0);
  servo_door2.attach(DOOR2_SERVO, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  servo_door2.write(180);
  servo_elevator.attach(ELEVATOR_SERVO, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  servo_elevator.write(180);
}

void intr_isr_task (void *p) {
  gpio_num_t gpio_num; 
  typedef struct { gpio_num_t gpio; char state[32]; Event e;} pinEvent;
  
  pinEvent event_to_send[] = {
    { BUTTON1_GPIO, "IN FLOOR 1 DOOR OPENED", {"CLOSE DOOR 1 PRESSED"} },
    { BUTTON1_GPIO, "IN FLOOR 1 DOOR CLOSED", {"OPEN DOOR 1 PRESSED"} },
    { BUTTON2_GPIO, "IN FLOOR 2 DOOR OPENED", {"CLOSE DOOR 2 PRESSED"} },
    { BUTTON2_GPIO, "IN FLOOR 2 DOOR CLOSED", {"OPEN DOOR 2 PRESSED"} },
    { BUTTON3_GPIO, "IN FLOOR 2 DOOR CLOSED", {"DOWN PRESSED"} },
    { BUTTON4_GPIO, "IN FLOOR 1 DOOR CLOSED", {"UP PRESSED"} }
  };

  while(1) {
    if (xQueueReceive(intr_queue, &gpio_num, portMAX_DELAY)) {
      //printf("intr_isr_task:: button pressed: %d current state: %s\n", gpio_num, elevator_fsm.current_state );
      for(int i = 0 ; i < sizeof(event_to_send)/sizeof(pinEvent); i++) {
        if ((gpio_num == event_to_send[i].gpio) && 
            (strcmp(elevator_fsm.current_state, event_to_send[i].state) == 0) ) {
              //printf("intr_isr_task:: will send %s\n", event_to_send[i].e.name);
              send_event(event_to_send[i].e);
              break;
        }
      }
    } 
  }
}

void elevator_fsm_task(void *p) {
    init_fsm(&elevator_fsm);
    Event e;

    while(1){
      xQueueReceive(elevator_event_queue, &e, portMAX_DELAY);  
      if (strlen(e.name) > 0) {
        //printf("elevator_fsm_task:: event %s was received and is being processed\n", e.name);
        process_event(&elevator_fsm, e);  
      }
    }
}

void app_main() {
  intr_queue = xQueueCreate(QUEUE_LENGTH, sizeof(gpio_num_t));
  elevator_event_queue = xQueueCreate(QUEUE_LENGTH, sizeof(Event));

  printf("main:: Hello from the elevator sim!\n");

  if (intr_queue == NULL || elevator_event_queue == NULL) {
    printf("Error creating the queues.\n");
    return;
  }

  init_pins();
  xTaskCreate(&intr_isr_task, "intr_isr_task", 8048,NULL,5,NULL );
  xTaskCreate(&elevator_fsm_task, "elevator_fsm_task", 8048,NULL,5,NULL );
}

void setup() { app_main();}; 
void loop() {vTaskDelay(2000 / portTICK_PERIOD_MS);};