#include <Arduino.h>
#include "FS.h"

#define left_sensor1 27
#define left_sensor2 26
#define left_sensor3 25

#define vehicle_g_led 13
#define vehicle_y_led 12
#define vehicle_r_led 14
#define pedestrian_g_led 33
#define pedestrian_r_led 32

#define right_sensor1 15
#define right_sensor2 2
#define right_sensor3 4

#define L_button 35
#define R_button 34

int button;
int number_of_pedestrian = 0;

bool changes_button_press = true;
bool startDelayVehicle = false;
bool startDelayPedestrian = false;
bool startBlinkingVehicle = false;
bool startYellowVehicle = false;
bool startDelayCoutingPedestrian = false;
bool startBlinkingPedestrian = false;

int delayVehicle;
int delayPedestrian;
int previousTime;
int blinkingDelay = 500;
int countBlinking = 9;
int howManyBlink = 1;
int yellowTime = 1000;

void setup()
{
  Serial.begin(115200);

  pinMode(left_sensor1, INPUT);
  pinMode(left_sensor2, INPUT);
  pinMode(left_sensor3, INPUT);

  pinMode(vehicle_g_led, OUTPUT);
  pinMode(vehicle_y_led, OUTPUT);
  pinMode(vehicle_r_led, OUTPUT);
  pinMode(pedestrian_g_led, OUTPUT);
  pinMode(pedestrian_r_led, OUTPUT);

  pinMode(right_sensor1, INPUT);
  pinMode(right_sensor2, INPUT);
  pinMode(right_sensor3, INPUT);

  pinMode(L_button, INPUT);
  pinMode(R_button, INPUT);
}

void vehicles_go()
{
  digitalWrite(vehicle_g_led, HIGH);
  digitalWrite(vehicle_y_led, LOW);
  digitalWrite(vehicle_r_led, LOW);

  digitalWrite(pedestrian_g_led, LOW);
  digitalWrite(pedestrian_r_led, HIGH);
}

void pedestrian_go()
{
  digitalWrite(vehicle_g_led, LOW);
  digitalWrite(vehicle_y_led, LOW);
  digitalWrite(vehicle_r_led, HIGH);

  digitalWrite(pedestrian_g_led, HIGH);
  digitalWrite(pedestrian_r_led, LOW);
}

void loop()
{
  if (changes_button_press)
  {
    int button_press = digitalRead(L_button) + digitalRead(R_button);
    if (button_press == 0)
    {
      vehicles_go();
    }
    else
    {
      changes_button_press = false;
      Serial.println("Button Pressed");
      // For vehicle
      int l1 = digitalRead(left_sensor1);
      int l2 = digitalRead(left_sensor2);
      int l3 = digitalRead(left_sensor3);

      int r1 = digitalRead(right_sensor1);
      int r2 = digitalRead(right_sensor2);
      int r3 = digitalRead(right_sensor3);

      int total_vehicle_left = l1 + l2 + l3;
      int total_vehicle_right = r1 + r2 + r3;

      if (total_vehicle_left == 0 || total_vehicle_right == 0)
      {
        delayVehicle = 10000;
        Serial.println("Very Jam!");
      }
      else if (total_vehicle_left == 1 || total_vehicle_right == 1)
      {
        delayVehicle = 5000;
        Serial.println("Less Jam!");
      }
      else
      {
        delayVehicle = 1000;
        Serial.println("No Jam!");
      }
      previousTime = millis();
      startDelayVehicle = true;
    }
  }
  else
  {
    int currentTime = millis();
    if (startDelayVehicle)
    {
      if (currentTime - previousTime >= delayVehicle)
      {
        startDelayVehicle = false;
        previousTime = currentTime;
        startBlinkingVehicle = true;
        howManyBlink = 1;
      }
    }

    if (startBlinkingVehicle)
    {
      if (currentTime - previousTime >= blinkingDelay)
      {
        previousTime = currentTime;
        if (howManyBlink <= countBlinking)
        {
          if (howManyBlink % 2 == 1)
          {
            digitalWrite(vehicle_g_led, LOW);
          }
          else
          {
            digitalWrite(vehicle_g_led, HIGH);
            Serial.println("Stopping vehicles...");
          }
          howManyBlink++;
        }
        else
        {
          startBlinkingVehicle = false;
          startYellowVehicle = true;
          digitalWrite(vehicle_y_led, HIGH);
        }
      }
    }

    if (startYellowVehicle)
    {
      if (currentTime - previousTime >= yellowTime)
      {
        startYellowVehicle = false;
        startDelayCoutingPedestrian = true;
        pedestrian_go();
        Serial.println("Pedestrian crossing");
        previousTime = currentTime;
      }
    }

    if (startDelayCoutingPedestrian)
    {
      if (currentTime - previousTime >= 5000)
      {
        startDelayCoutingPedestrian = false;
        startDelayPedestrian = true;
        Serial.println("Start counting pedestrian");
        previousTime = currentTime;
      }
    }

    
    if (startDelayPedestrian)
    {
      delayPedestrian = 15000;     
      if ((number_of_pedestrian > 5) && (millis() - previousTime <= delayPedestrian))
      {
        Serial.println(number_of_pedestrian);
      }

      else
      {
        Serial.println("Less people or time's up");
        startDelayPedestrian = false;
        startBlinkingPedestrian = true;
        previousTime = currentTime;
        howManyBlink = 1;
      }
      
    }


    if (startBlinkingPedestrian)
    {
      if (currentTime - previousTime >= blinkingDelay)
      {
        previousTime = currentTime;
        if (howManyBlink <= countBlinking)
        {
          if (howManyBlink % 2 == 1)
          {
            digitalWrite(pedestrian_g_led, LOW);
          }
          else
          {
            digitalWrite(pedestrian_g_led, HIGH);
            Serial.println("Stopping pedestrians...");
          }
          howManyBlink++;
        }
        else
        {
          startBlinkingPedestrian = false;
          vehicles_go();
          Serial.println("Vehicle crossing");
          changes_button_press = true;
        }
      }
    }
  }
}