/*
Elevator FSM by Mariana Pérez Méndez
Code Rundown:
This code implements an FSM to control the behavior of an elevator that only goes from Floor1 to Floor2. 
It uses buttons to open/close doors and go up/down between floors, a led to indicate if the elevator is 
in motion and a one digit seven segment display to indicate the floor number.

It has,
6 states: Floor1 Doors Opened, Floor1 Doors Closed, Floor2 Doors Opened, Floor2 Doors Opened, Going up, and Going down.
4 Events: open, close, up, and down

Components connected to the ESP32:
1. led
2. resistor
3. One digit Seven Segment Display
4. Open and Close button (Blue)
5. Up and Down button (Green)

Methods:
1. handle_event(): prints the entering state.
2. handle_exit(): prints the exiting state.
3. init_fsm(): Initializes the first state, which is "Floor1 Doors Closed".
4. handle_fsm_event(): Compares the current state and current event to the ones being passed on as parameters. Updates the current state to the next state to complete the transition.
5. elevator_moving_indicator(): Turns on and off the led. The led turned on indicates that the elevator is in motion.
6. display_Floor1(): Displays the number one, indicating that the elevator is on Floor1.
7. display_Floor2(): Displays the number two, indicating that the elevator is on Floor2.
8. reset_display(): Resets the one digit seven segment display.
9. fsm_tasks(): Prints to console.
10. app_main(): Executes everything.

*/


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

#define ON 1
#define OFF 0

#define DELAY_TIME 1000

#define A GPIO_NUM_25
#define B GPIO_NUM_33
#define C GPIO_NUM_27
#define D GPIO_NUM_14
#define E GPIO_NUM_12
#define G GPIO_NUM_32
#define COM GPIO_NUM_2

#define BUTTON_UP_DOWN GPIO_NUM_4
#define BUTTON_OPEN_CLOSE GPIO_NUM_22

char* current_state = "Floor1 Doors Closed";
char * event;

typedef struct {
    char* current_state;
    char* current_event;
    char* next_state;
    void (*handle_event)();
    void (*handle_exit)();
} Transition;

void handle_event() { 
    printf("Entering state: %s \n", current_state); 
}

void handle_exit() { 
    printf("Exiting state: %s \n", current_state); 
}

Transition elevator_transitions[] = {
  {"Floor1 Doors Opened", "open", "Floor1 Doors Opened", handle_event, handle_exit},
  {"Floor1 Doors Opened", "close", "Floor1 Doors Closed", handle_event, handle_exit},
  {"Floor1 Doors Closed", "close", "Floor1 Doors Closed", handle_event, handle_exit},
  {"Floor1 Doors Closed", "open", "Floor1 Doors Opened", handle_event, handle_exit},
  {"Floor1 Doors Closed", "up", "Going up", handle_event, handle_exit},
  {"Floor1 Doors Closed", "down", "Floor1 Doors Closed", handle_event, handle_exit}, //Assuming there are no floors below Floor1
  {"Going up", "up", "Floor2 Doors Closed", handle_event, handle_exit},
  {"Going down", "down", "Floor1 Doors Closed", handle_event, handle_exit},
  {"Floor2 Doors Closed", "close", "Floor2 Doors Closed", handle_event, handle_exit},
  {"Floor2 Doors Closed", "open", "Floor2 Doors Opened", handle_event, handle_exit},
  {"Floor2 Doors Closed", "up", "Floor2 Doors Closed", handle_event, handle_exit}, //Assuming there are only 2 floors and Floor2 is the last floor
  {"Floor2 Doors Closed", "down", "Going down", handle_event, handle_exit},
  {"Floor2 Doors Opened", "open", "Floor2 Doors Opened", handle_event, handle_exit},
  {"Floor2 Doors Opened", "close", "Floor2 Doors Closed", handle_event, handle_exit},

};

void init_fsm(){
    current_state = "Floor1 Doors Closed";
}

void handle_fsm_event(char * event, char * state){
    int i;
    for(i=0; i<sizeof(elevator_transitions)/sizeof(elevator_transitions[0]); i++){
        Transition transition = elevator_transitions[i];
        if(strcmp(transition.current_state, state) == 0 && strcmp(transition.current_event, event) == 0){
            transition.handle_exit();
            current_state = transition.next_state;
            transition.handle_event();
            if(strcmp(transition.next_state, "Floor1 Doors Opened" ) == 0 || strcmp(transition.next_state, "Floor1 Doors Closed" ) == 0 ){
                display_Floor1();
            }else if(strcmp(transition.next_state, "Floor2 Doors Opened" ) == 0 || strcmp(transition.next_state, "Floor2 Doors Closed" ) == 0 ){
              display_Floor2();
            }else{
              reset_display();
            }
            break;
        }
    }
}

void elevator_moving_indicator(){
    gpio_set_direction(GPIO_NUM_18,GPIO_MODE_OUTPUT);

    //Turning on in motion indicator
    gpio_set_level(GPIO_NUM_18,ON);
    vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);

    printf(". . . . . . \n");
    vTaskDelay(DELAY_TIME/ portTICK_PERIOD_MS);

    //Turning off in motion indicator
    vTaskDelay(DELAY_TIME/ portTICK_PERIOD_MS);
    gpio_set_level(GPIO_NUM_18,OFF);

}

void display_Floor1(){
  reset_display();
  // Display 1
  gpio_set_direction(B, GPIO_MODE_OUTPUT);
  gpio_set_direction(C, GPIO_MODE_OUTPUT);
  gpio_set_level(B, OFF);
  gpio_set_level(C, OFF);
}

void display_Floor2(){
  reset_display();
  // Display 2
  gpio_set_direction(A, GPIO_MODE_OUTPUT);
  gpio_set_direction(B, GPIO_MODE_OUTPUT);
  gpio_set_direction(G, GPIO_MODE_OUTPUT);
  gpio_set_direction(E, GPIO_MODE_OUTPUT);
  gpio_set_direction(D, GPIO_MODE_OUTPUT);
  gpio_set_level(A, OFF);
  gpio_set_level(B, OFF);
  gpio_set_level(G, OFF);
  gpio_set_level(E, OFF);
  gpio_set_level(D, OFF);

}

void reset_display(){
  gpio_set_level(A, ON);
  gpio_set_level(B, ON);
  gpio_set_level(C, ON);
  gpio_set_level(D, ON);
  gpio_set_level(E, ON);
  gpio_set_level(G, ON);
}



void fsm_tasks(void *) {

    printf("Initial State: %s \n", current_state);
    vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);

    printf("Open/Close doors with blue button or go up/down with green button.\n");
    printf("Remember you can't go up or down with the doors opened :) \n");

    gpio_set_direction(BUTTON_UP_DOWN, GPIO_MODE_INPUT);
    gpio_set_pull_mode(BUTTON_UP_DOWN, GPIO_PULLUP_ONLY);

    gpio_set_direction(BUTTON_OPEN_CLOSE, GPIO_MODE_INPUT);
    gpio_set_pull_mode(BUTTON_OPEN_CLOSE, GPIO_PULLUP_ONLY);

    while (1) {
        int button_state_U_D = gpio_get_level(BUTTON_UP_DOWN);
        int button_state_O_C = gpio_get_level(BUTTON_OPEN_CLOSE);
        if (button_state_U_D == OFF) {
            // Green Button is pressed
            if (strcmp(current_state, "Floor1 Doors Closed") == 0) {
                event = "up";
            } else if (strcmp(current_state, "Floor2 Doors Closed") == 0) {
                event = "down";
            } else {
                // Invalid state for the button press
                continue;
            }
            printf("\nEvent: %s\n", event);
            handle_fsm_event(event, current_state);
            elevator_moving_indicator();
            handle_fsm_event(event, current_state);
            vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);

        }
        else if (button_state_O_C == OFF) {
            // Blue Button is pressed
            if (strcmp(current_state, "Floor1 Doors Closed") == 0 || strcmp(current_state, "Floor2 Doors Closed") == 0) {
                event = "open";
            } else if (strcmp(current_state, "Floor1 Doors Opened") == 0 || strcmp(current_state, "Floor2 Doors Opened") == 0) {
                event = "close";
            } else {
                // Invalid state for the button press
                continue;
            }
            printf("\nEvent: %s\n", event);
            handle_fsm_event(event, current_state);
            vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);

        }

    }

}


extern "C" void app_main() {

  init_fsm();

  gpio_set_direction(COM, GPIO_MODE_OUTPUT);
  gpio_set_level(COM, ON);

  xTaskCreate(&fsm_tasks, "fsm_tasks", 3000, NULL, 10, NULL);

}

// El primer fsm_tasks que hice:
// void fsm_tasks(void *) {
//     // char * event;

//     printf("Initial State: %s \n", current_state);
//     vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);
    
//     // Opening elevator doors on Floor1, person enters elevator
//     event = "open";

//     printf("\nEvent: %s\n", event);
//     handle_fsm_event(event, current_state);
//     vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);

//     // Closing elevator doors on Floor1
//     event = "close";

//     printf("\nEvent: %s\n", event);
//     handle_fsm_event(event, current_state);
//     vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);

//     // Moving up from Floor1 to Floor2
//     event = "up";

//     printf("\nEvent: %s\n", event);
//     handle_fsm_event(event, current_state);
//     elevator_moving_indicator();
//     handle_fsm_event(event, current_state);
//     vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);


//     // Opening elevator doors on Floor2, person exits elevator
//     event = "open";
//     printf("\nEvent: %s\n", event);
//     handle_fsm_event(event, current_state);
//     vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);

//     // Closing elevator doors on Floor2
//     event = "close";
//     printf("\nEvent: %s\n", event);
//     handle_fsm_event(event, current_state);
//     vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);

//     // Moving down from Floor2 to Floor1
//     event = "down";''
 
//     printf("\nEvent: %s\n", event);
//     handle_fsm_event(event, current_state);
//     elevator_moving_indicator();
//     handle_fsm_event(event, current_state);
//     vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);
  

//     // Opening elevator doors on Floor1
//     event = "open";

//     printf("\nEvent: %s\n", event);
//     handle_fsm_event(event, current_state);
//     vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);
    
//     vTaskDelete(NULL);
// }