#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.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
//Initialize the traffic_lights array with GPIO pin numbers of the corresponding LEDs.
int traffic_lights[NUM_DIRECTIONS][NUM_LEDS] = {
    {45, 47, 20}, // Direction 1: RED1, YELLOW1, GREEN1
    {2, 41, 39}   // Direction 2: RED2, YELLOW2, GREEN2
};
//Set the number of elements in the white_leds array to the NUM_DIRECTIONS 
//Initialize the elements of this array to the GPIO pins connected to the White LEDs (for pedestrian light).
int white_leds[NUM_DIRECTIONS] = {35, 37}; // Direction 1: WHITE1 ESP35 Direction 2: WHITE2 ESP37


// Pedestrian GPIO pins
#define PIR_SENSOR_GPIO GPIO_NUM_4
#define PUSH_BUTTON_GPIO GPIO_NUM_5
#define START_BUTTON_GPIO GPIO_NUM_6

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

// Main function
void app_main() {

    initialize_gpio();    

    while (1) {
    // Small delay to avoid rapid state changes
    vTaskDelay(500 / portTICK_PERIOD_MS);
    
    if (gpio_get_level(START_BUTTON_GPIO) == 1) {
    normal_traffic_sequence();
    }

    else if ((gpio_get_level(PIR_SENSOR_GPIO) == 1) || (gpio_get_level(PUSH_BUTTON_GPIO) == 1)) {
      if (gpio_get_level(PIR_SENSOR_GPIO) == 1) {
        pedestrian_signal(1, traffic_lights[0], traffic_lights[1], white_leds[0]);
      }
      else if (gpio_get_level(PUSH_BUTTON_GPIO) == 1) {
        pedestrian_signal(1, traffic_lights[1], traffic_lights[0], white_leds[1]);
      }
    }

    } // while loop
}

// Function Definitions

// The function has no parameters and does not return anything 
// but is called to initiate the program with the buttons and sensor 
// being set as inputs and the LEDs being set as outputs
void initialize_gpio() {
  // set the start button to be an input
      gpio_set_direction(START_BUTTON_GPIO, GPIO_MODE_INPUT);

  // use a nested for loop to iterate through the three traffic lights
  // in each of the two directions, such that each LED is set
  // to an output
  for (int i = 0; i < NUM_DIRECTIONS; ++i) {
    for (int j = 0; j < NUM_LEDS; ++j) {
      gpio_set_direction(traffic_lights[i][j], GPIO_MODE_OUTPUT);
    } // for loop
  } // for loop
  // set the PIR sensor to be an input
      gpio_set_direction(PIR_SENSOR_GPIO, GPIO_MODE_INPUT);
  // set the push button to be an input
      gpio_set_direction(PUSH_BUTTON_GPIO, GPIO_MODE_INPUT);

  // set the White LEDs to be outputs
      gpio_set_direction(white_leds[0], GPIO_MODE_OUTPUT);
      gpio_set_direction(white_leds[1], GPIO_MODE_OUTPUT);
}

void normal_traffic_sequence() {
    //***Sequence 1***
    // RED1 ON
    // YELLOW1 OFF
    // GREEN1 OFF

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

    //***Sequence 2***
    // RED1 ON
    // YELLOW1 OFF
    // GREEN1 OFF

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

    //***Sequence 3***
    // RED1 OFF
    // YELLOW1 OFF
    // GREEN1 ON

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

    //***Sequence 4***
    // RED1 OFF
    // YELLOW1 ON
    // GREEN1 OFF

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

    //***Sequence 5***
    // RED1 OFF
    // YELLOW1 OFF
    // GREEN1 OFF

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

//pedestrian_signal function
//return type: void
//Parameters:
//pedestrian_input high/low 1/0
//same_traffic
//opposite_traffic
//white_led
void pedestrian_signal(int pedestrian_input, int same_traffic[], int opposite_traffic[], int white_led) {

  if (pedestrian_input == 1) {
    //prints "Pedestrians Crossing! if the sensor or push button is triggered"
    printf("Pedestrians Crossing!\n");

    gpio_set_level(same_traffic[0], 0); //Turns the Red LED of the same_traffic OFF
    gpio_set_level(same_traffic[2], 1); //Turns the Green LED of the same_traffic ON
    gpio_set_level(opposite_traffic[0], 1); //Turns the Red LED of the opposite_traffic ON

    //using for loop; Blinks the pedestrian white_led 5 times (ON for 0.5 seconds and OFF for 0.5 seconds in each blink)
    for (int i = 0; i < 5; i++) {
    gpio_set_level(white_led, 1); 
    vTaskDelay(500 / portTICK_PERIOD_MS); 
    gpio_set_level(white_led, 0); 
    vTaskDelay(500 / portTICK_PERIOD_MS); 
    }

    gpio_set_level(opposite_traffic[0], 0); //Turns the Red LED of the opposite_traffic OFF
    gpio_set_level(same_traffic[2], 0); //Turns the Green LED of the same_traffic OFF
    gpio_set_level(same_traffic[0], 1); //Turns the Red LED of the same_traffic ON

    vTaskDelay(2000 / portTICK_PERIOD_MS); //2s delay
    gpio_set_level(same_traffic[0], 0); //Turns the Red LED of the same_traffic OFF
  }

}