#include <stdio.h>

#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

// Define number of elements GPIO pin arrays
#define NUM_LEDS 3        // number of LEDs in each direction
#define NUM_DIRECTIONS 2  // 2 directions the one intersection

// Traffic light GPIO pins
int traffic_lights[NUM_DIRECTIONS][NUM_LEDS] = {
    {37, 36, 35},  // Direction 1: RED1, YELLOW1, GREEN1
    {18, 8, 3}     // Direction 2: RED2, YELLOW2, GREEN2
};

// Gets index number of the colors of traffic_lights array
int red_led = 0;
int yellow_led = 1;
int green_led = 2;

// Get length of traffic_lights array and the length of arrays within it
int directions_length = sizeof(traffic_lights) / sizeof(traffic_lights[0]);
int leds_length = sizeof(traffic_lights[0]) / sizeof(traffic_lights[0][0]);

// Pedestrian GPIO pins
#define PIR_SENSOR 5
#define PUSH_BUTTON 45
#define START_BUTTON 4
int white_leds[2] = {38, 17};  // WHITE1 (Dir1), WHITE2 (Dir2)

// Get length of white_leds array
int white_leds_length = sizeof(white_leds) / sizeof(white_leds[0]);

// Function Declarations
void initialize_gpio();
void normal_traffic_sequence();
void pedestrian_signal(int same_traffic[], int opposite_traffic[], int white_led);

// Main function
void app_main() {
    initialize_gpio();
    while (1) {
        int pir_sens_state = gpio_get_level(PIR_SENSOR);
        int push_btn_state = gpio_get_level(PUSH_BUTTON);
        int start_btn_state = gpio_get_level(START_BUTTON);

        if (start_btn_state == 1) {
            printf("Start btn\n");
            normal_traffic_sequence();
        } else if (pir_sens_state == 1) {
            printf("PIR sens\n");
            pedestrian_signal(traffic_lights[1], traffic_lights[0], white_leds[1]);
        } else if (push_btn_state == 1) {
            printf("Push btn\n");
            pedestrian_signal(traffic_lights[0], traffic_lights[1], white_leds[0]);
        }

        vTaskDelay(100 / portTICK_PERIOD_MS);
    }  // end of while(1)
}

/*
- "initialize_gpio" function doesn't have a return type or any parameters.
- Initializes the pins for the traffic LEDs and white LEDs as OUTPUT
- Initializes the pins for PIR motion sensor, push button, and start button as INPUT
*/
void initialize_gpio() {
    // Initialize GPIO pins for traffic lights, white LEDs
    for (int row = 0; row < directions_length; row++) {
        gpio_set_direction(white_leds[row], GPIO_MODE_OUTPUT);
        for (int col = 0; col < leds_length; col++) {
            gpio_set_direction(traffic_lights[row][col], GPIO_MODE_OUTPUT);
        }
    }

    // Initialize GPIO pins for inputs and PIR sensor
    gpio_set_direction(PIR_SENSOR, GPIO_MODE_INPUT);
    gpio_set_direction(PUSH_BUTTON, GPIO_MODE_INPUT);
    gpio_set_direction(START_BUTTON, GPIO_MODE_INPUT);
}

/*
- "normal_traffic_sequence" function doesn't have a return type or any parameters
- Displays regular flow of traffic lights using Red, Yellow, and Green LEDs
*/
void normal_traffic_sequence() {
    //***Sequence 1***
    // RED1 ON
    // YELLOW1 OFF
    // GREEN1 OFF
    gpio_set_level(traffic_lights[0][red_led], 1);
    gpio_set_level(traffic_lights[0][yellow_led], 0);
    gpio_set_level(traffic_lights[0][green_led], 0);

    // RED2 OFF
    // YELLOW2 OFF
    // GREEN2 ON
    gpio_set_level(traffic_lights[1][red_led], 0);
    gpio_set_level(traffic_lights[1][yellow_led], 0);
    gpio_set_level(traffic_lights[1][green_led], 1);
    // 5 seconds delay
    vTaskDelay(5000 / portTICK_PERIOD_MS);

    //***Sequence 2***
    // RED1 ON
    // YELLOW1 OFF
    // GREEN1 OFF
    gpio_set_level(traffic_lights[0][red_led], 1);
    gpio_set_level(traffic_lights[0][yellow_led], 0);
    gpio_set_level(traffic_lights[0][green_led], 0);

    // RED2 OFF
    // YELLOW2 ON
    // GREEN2 OFF
    gpio_set_level(traffic_lights[1][red_led], 0);
    gpio_set_level(traffic_lights[1][yellow_led], 1);
    gpio_set_level(traffic_lights[1][green_led], 0);
    // 2 seconds delay
    vTaskDelay(2000 / portTICK_PERIOD_MS);

    //***Sequence 3***
    // RED1 OFF
    // YELLOW1 OFF
    // GREEN1 ON
    gpio_set_level(traffic_lights[0][red_led], 0);
    gpio_set_level(traffic_lights[0][yellow_led], 0);
    gpio_set_level(traffic_lights[0][green_led], 1);

    // RED2 ON
    // YELLOW2 OFF
    // GREEN2 OFF
    gpio_set_level(traffic_lights[1][red_led], 1);
    gpio_set_level(traffic_lights[1][yellow_led], 0);
    gpio_set_level(traffic_lights[1][green_led], 0);
    // 5 seconds delay
    vTaskDelay(5000 / portTICK_PERIOD_MS);

    //***Sequence 4***
    // RED1 OFF
    // YELLOW1 ON
    // GREEN1 OFF
    gpio_set_level(traffic_lights[0][red_led], 0);
    gpio_set_level(traffic_lights[0][yellow_led], 1);
    gpio_set_level(traffic_lights[0][green_led], 0);

    // RED2 ON
    // YELLOW2 OFF
    // GREEN2 OFF
    gpio_set_level(traffic_lights[1][red_led], 1);
    gpio_set_level(traffic_lights[1][yellow_led], 0);
    gpio_set_level(traffic_lights[1][green_led], 0);
    // 2 seconds delay
    vTaskDelay(2000 / portTICK_PERIOD_MS);

    //***Sequence 5***
    // RED1 OFF
    // YELLOW1 OFF
    // GREEN1 OFF
    gpio_set_level(traffic_lights[0][red_led], 0);
    gpio_set_level(traffic_lights[0][yellow_led], 0);
    gpio_set_level(traffic_lights[0][green_led], 0);

    // RED2 OFF
    // YELLOW2 OFF
    // GREEN2 OFF
    gpio_set_level(traffic_lights[1][red_led], 0);
    gpio_set_level(traffic_lights[1][yellow_led], 0);
    gpio_set_level(traffic_lights[1][green_led], 0);
}

/*
- "pedestrian_signal" function doesn't have a return type
- Function has parameters:
  - same_traffic[]: array for traffic lights of where the pedestrian is
  - opposite_traffic[]: array for traffic lights parallel of the pedestrian
  - white_led: white led for the pedestrian on the side of the traffic light it is currently on
- Displays regular flow of traffic lights with a pedestrian crossing using Red, Yellow, Green, and White LEDs
*/
void pedestrian_signal(int same_traffic[], int opposite_traffic[], int white_led) {
    printf("--- PEDESTRIAN CROSSING ---\n");
    /*
    - SAME RED ON
    - SAME YELLOW OFF
    - SAME GREEN ON
    */
    gpio_set_level(same_traffic[red_led], 1);
    gpio_set_level(same_traffic[yellow_led], 0);
    gpio_set_level(same_traffic[green_led], 0);

    /*
    - OPPOSITE RED OFF
    - OPPOSITE YELLOW ON
    - OPPOSITE GREEN OFF
    */
    gpio_set_level(opposite_traffic[red_led], 0);
    gpio_set_level(opposite_traffic[yellow_led], 1);
    gpio_set_level(opposite_traffic[green_led], 0);
    // 2 seconds delay
    vTaskDelay(2000 / portTICK_PERIOD_MS);

    /*
    - SAME RED OFF
    - SAME YELLOW OFF
    - SAME GREEN ON
    */
    gpio_set_level(same_traffic[red_led], 0);
    gpio_set_level(same_traffic[yellow_led], 0);
    gpio_set_level(same_traffic[green_led], 1);

    /*
    - OPPOSITE RED ON
    - OPPOSITE YELLOW ON
    - OPPOSITE GREEN OFF
    */
    gpio_set_level(opposite_traffic[red_led], 1);
    gpio_set_level(opposite_traffic[yellow_led], 0);
    gpio_set_level(opposite_traffic[green_led], 0);
    // 1 second delay
    vTaskDelay(1000 / portTICK_PERIOD_MS);

    // Blinks white LED for 5 seconds (ON 0.5s / OFF 0.5s)
    for (int i = 5; i >= 1; i--) {
        // Displays in seconds how long the pedestrian has left to cross
        printf("%d\n", i);
        // ON for 0.5s
        gpio_set_level(white_led, 1);
        vTaskDelay(500 / portTICK_PERIOD_MS);

        // OFF for 0.5s
        gpio_set_level(white_led, 0);
        vTaskDelay(500 / portTICK_PERIOD_MS);
    }
    printf("0\n");

    /*
    - SAME RED OFF
    - SAME YELLOW ON
    - SAME GREEN OFF
    */
    gpio_set_level(same_traffic[red_led], 0);
    gpio_set_level(same_traffic[yellow_led], 1);
    gpio_set_level(same_traffic[green_led], 0);

    /*
    - OPPOSITE RED ON
    - OPPOSITE YELLOW ON
    - OPPOSITE GREEN OFF
    */
    gpio_set_level(opposite_traffic[red_led], 1);
    gpio_set_level(opposite_traffic[yellow_led], 0);
    gpio_set_level(opposite_traffic[green_led], 0);
    // 2 seconds delay
    vTaskDelay(2000 / portTICK_PERIOD_MS);

    /*
    - SAME RED ON
    - SAME YELLOW OFF
    - SAME GREEN OFF
    */
    gpio_set_level(same_traffic[red_led], 1);
    gpio_set_level(same_traffic[yellow_led], 0);
    gpio_set_level(same_traffic[green_led], 0);

    /*
    - OPPOSITE RED OFF
    - OPPOSITE YELLOW OFF
    - OPPOSITE GREEN ON
    */
    gpio_set_level(opposite_traffic[red_led], 0);
    gpio_set_level(opposite_traffic[yellow_led], 0);
    gpio_set_level(opposite_traffic[green_led], 1);
    // 2 seconds delay
    vTaskDelay(3000 / portTICK_PERIOD_MS);

    // Turn OFF all LEDs
    gpio_set_level(same_traffic[red_led], 0);
    gpio_set_level(same_traffic[yellow_led], 0);
    gpio_set_level(same_traffic[green_led], 0);
    gpio_set_level(opposite_traffic[red_led], 0);
    gpio_set_level(opposite_traffic[yellow_led], 0);
    gpio_set_level(opposite_traffic[green_led], 0);
}