#define e_pin1 6
#define e_pin2 7
#define e_pin3 8
#define e_pin4 9

const int t_pin1 = 0;
const int t_pin2 = 1;
const int t_pin3 = 2;
const int t_pin4 = 3;
const int r  = 4;
const int redLight2 = 5;
const int yellowLight2 = 6;
const int greenLight2 = 7;



const int redLight1 = 3;
const int yellowLight1 = 4;
const int greenLight1 = 5;

const int redLight3 = A0;
const int yellowLight3 = A1;
const int greenLight3 = A2;

const int redLight4 = A3;
const int yellowLight4 = A4;
const int greenLight4 = A5;


long duration1;
long duration2;
long duration3;
long duration4;
int dis1;
int dis2;
int dis3;
int dis4;

void setup() {

  pinMode(e_pin1, INPUT);
  pinMode(e_pin2, INPUT);
  pinMode(e_pin3, INPUT);
  pinMode(e_pin4, INPUT);



  pinMode(greenLight1, OUTPUT);
  pinMode(yellowLight1, OUTPUT);
  pinMode(redLight1, OUTPUT);

  pinMode(greenLight3, OUTPUT);
  pinMode(yellowLight3, OUTPUT);
  pinMode(redLight3, OUTPUT);


  pinMode(greenLight4, OUTPUT);
  pinMode(yellowLight4, OUTPUT);
  pinMode(redLight4, OUTPUT);

  for (int x = 10; x < 14; x++) pinMode(x, OUTPUT);


  
  Serial.begin(9600);
}

void loop() {


  delay(2);
  for (int z = 0; z < 4; z++)
  {
    digitalWrite(10, bitRead(t_pin1, 0));
    digitalWrite(11, bitRead(t_pin1, 1));
    digitalWrite(12, bitRead(t_pin1, 2));
    digitalWrite(13, bitRead(t_pin1, 3));
  }
  delay(10);
  for (int z = 0; z < 4; z++)
  {
    digitalWrite(10, bitRead(r, 0));
    digitalWrite(11, bitRead(r, 1));
    digitalWrite(12, bitRead(r, 2));
    digitalWrite(13, bitRead(r, 3));
  }
  duration1 = pulseIn(e_pin1, HIGH);
  dis1 = (duration1 * 0.034) / 2;

  delay(2);
  for (int z = 0; z < 4; z++)
  {
    digitalWrite(10, bitRead(t_pin2, 0));
    digitalWrite(11, bitRead(t_pin2, 1));
    digitalWrite(12, bitRead(t_pin2, 2));
    digitalWrite(13, bitRead(t_pin2, 3));
  }
  delay(10);
  for (int z = 0; z < 4; z++)
  {
    digitalWrite(10, bitRead(r, 0));
    digitalWrite(11, bitRead(r, 1));
    digitalWrite(12, bitRead(r, 2));
    digitalWrite(13, bitRead(r, 3));
  }
  duration2 = pulseIn(e_pin2, HIGH);
  dis2 = (duration2 * 0.034) / 2;

  delay(2);
  for (int z = 0; z < 4; z++)
  {
    digitalWrite(10, bitRead(t_pin3, 0));
    digitalWrite(11, bitRead(t_pin3, 1));
    digitalWrite(12, bitRead(t_pin3, 2));
    digitalWrite(13, bitRead(t_pin3, 3));
  }
  delay(10);
  for (int z = 0; z < 4; z++)
  {
    digitalWrite(10, bitRead(r, 0));
    digitalWrite(11, bitRead(r, 1));
    digitalWrite(12, bitRead(r, 2));
    digitalWrite(13, bitRead(r, 3));
  }
  duration3 = pulseIn(e_pin3, HIGH);
  dis3 = (duration3 * 0.034) / 2;

  delay(2);
  for (int z = 0; z < 4; z++)
  {
    digitalWrite(10, bitRead(t_pin4, 0));
    digitalWrite(11, bitRead(t_pin4, 1));
    digitalWrite(12, bitRead(t_pin4, 2));
    digitalWrite(13, bitRead(t_pin4, 3));
  }
  delay(10);
  for (int z = 0; z < 4; z++)
  {
    digitalWrite(10, bitRead(r, 0));
    digitalWrite(11, bitRead(r, 1));
    digitalWrite(12, bitRead(r, 2));
    digitalWrite(13, bitRead(r, 3));
  }
  duration4 = pulseIn(e_pin4, HIGH);
  dis4 = (duration4 * 0.034) / 2;

  Serial.print("Distance 1: ");
  Serial.println(dis1);
  Serial.print("Distance 2: ");
  Serial.println(dis2);
  Serial.print("Distance 3: ");
  Serial.println(dis3);
  Serial.print("Distance 4: ");
  Serial.println(dis4);


  for (int i = 0; i < 4; i++) {
    Serial.print(i);
    Serial.print(dis1);
    Serial.print(dis2);
    Serial.print(dis3);
    Serial.println(dis4);


    if (dis1 < dis2 && dis1 < dis3 && dis1 < dis4)
    {

      digitalWrite(greenLight1, HIGH);
      digitalWrite(yellowLight1, LOW);
      digitalWrite(redLight1, LOW);

      if (dis2 < dis3 && dis2 < dis4)
      {


        for (int z = 0; z < 4; z++)
        {
          digitalWrite(10, bitRead(yellowLight2, 0));
          digitalWrite(11, bitRead(yellowLight2, 1));
          digitalWrite(12, bitRead(yellowLight2, 2));
          digitalWrite(13, bitRead(yellowLight2, 3));
        }

        digitalWrite(greenLight3, LOW);
        digitalWrite(yellowLight3, LOW);
        digitalWrite(redLight3, HIGH);

        digitalWrite(greenLight4, LOW);
        digitalWrite(yellowLight4, LOW);
        digitalWrite(redLight4, HIGH);
      }

      else if (dis3 < dis2 && dis3 < dis4)
      {

        digitalWrite(greenLight3, LOW);
        digitalWrite(yellowLight3, HIGH);
        digitalWrite(redLight3, LOW);

        for (int z = 0; z < 4; z++)
        {
          digitalWrite(10, bitRead(redLight2, 0));
          digitalWrite(11, bitRead(redLight2, 1));
          digitalWrite(12, bitRead(redLight2, 2));
          digitalWrite(13, bitRead(redLight2, 3));
        }

        digitalWrite(greenLight4, LOW);
        digitalWrite(yellowLight4, LOW);
        digitalWrite(redLight4, HIGH);
      }

      else
      {

        digitalWrite(greenLight4, LOW);
        digitalWrite(yellowLight4, HIGH);
        digitalWrite(redLight4, LOW);

        digitalWrite(greenLight3, LOW);
        digitalWrite(yellowLight3, LOW);
        digitalWrite(redLight3, HIGH);

        for (int z = 0; z < 4; z++)
        {
          digitalWrite(10, bitRead(redLight2, 0));
          digitalWrite(11, bitRead(redLight2, 1));
          digitalWrite(12, bitRead(redLight2, 2));
          digitalWrite(13, bitRead(redLight2, 3));
        }
      }
      delay(3000);
      dis1 = 1000;
    }

    else if (dis2 < dis1 && dis2 < dis3 && dis2 < dis4)
    {

      for (int z = 0; z < 4; z++)
      {
        digitalWrite(10, bitRead(greenLight2, 0));
        digitalWrite(11, bitRead(greenLight2, 1));
        digitalWrite(12, bitRead(greenLight2, 2));
        digitalWrite(13, bitRead(greenLight2, 3));
      }

      if (dis1 < dis3 && dis1 < dis4)
      {

        digitalWrite(greenLight1, LOW);
        digitalWrite(yellowLight1, HIGH);
        digitalWrite(redLight1, LOW);

        digitalWrite(greenLight3, LOW);
        digitalWrite(yellowLight3, LOW);
        digitalWrite(redLight3, HIGH);

        digitalWrite(greenLight4, LOW);
        digitalWrite(yellowLight4, LOW);
        digitalWrite(redLight4, HIGH);
      }

      else if (dis3 < dis1 && dis3 < dis4)
      {

        digitalWrite(greenLight3, LOW);
        digitalWrite(yellowLight3, HIGH);
        digitalWrite(redLight3, LOW);

        digitalWrite(greenLight1, LOW);
        digitalWrite(yellowLight1, LOW);
        digitalWrite(redLight1, HIGH);

        digitalWrite(greenLight4, LOW);
        digitalWrite(yellowLight4, LOW);
        digitalWrite(redLight4, HIGH);
      }

      else
      {

        digitalWrite(greenLight4, LOW);
        digitalWrite(yellowLight4, HIGH);
        digitalWrite(redLight4, LOW);

        digitalWrite(greenLight3, LOW);
        digitalWrite(yellowLight3, LOW);
        digitalWrite(redLight3, HIGH);

        digitalWrite(greenLight1, LOW);
        digitalWrite(yellowLight1, LOW);
        digitalWrite(redLight1, HIGH);
      }
      delay(3000);
      dis2 = 1000;
    }

    else if (dis3 < dis1 && dis3 < dis2 && dis3 < dis4)
    {

      digitalWrite(greenLight3, HIGH);
      digitalWrite(yellowLight3, LOW);
      digitalWrite(redLight3, LOW);

      if (dis1 < dis2 && dis1 < dis4)
      {

        digitalWrite(greenLight1, LOW);
        digitalWrite(yellowLight1, HIGH);
        digitalWrite(redLight1, LOW);

        for (int z = 0; z < 4; z++)
        {
          digitalWrite(10, bitRead(redLight2, 0));
          digitalWrite(11, bitRead(redLight2, 1));
          digitalWrite(12, bitRead(redLight2, 2));
          digitalWrite(13, bitRead(redLight2, 3));
        }

        digitalWrite(greenLight4, LOW);
        digitalWrite(yellowLight4, LOW);
        digitalWrite(redLight4, HIGH);
      }

      else if (dis2 < dis1 && dis2 < dis4)
      {

        for (int z = 0; z < 4; z++)
        {
          digitalWrite(10, bitRead(yellowLight2, 0));
          digitalWrite(11, bitRead(yellowLight2, 1));
          digitalWrite(12, bitRead(yellowLight2, 2));
          digitalWrite(13, bitRead(yellowLight2, 3));
        }

        digitalWrite(greenLight1, LOW);
        digitalWrite(yellowLight1, LOW);
        digitalWrite(redLight1, HIGH);

        digitalWrite(greenLight4, LOW);
        digitalWrite(yellowLight4, LOW);
        digitalWrite(redLight4, HIGH);
      }

      else
      {

        digitalWrite(greenLight4, LOW);
        digitalWrite(yellowLight4, HIGH);
        digitalWrite(redLight4, LOW);

        digitalWrite(greenLight1, LOW);
        digitalWrite(yellowLight1, LOW);
        digitalWrite(redLight1, HIGH);

        for (int z = 0; z < 4; z++)
        {
          digitalWrite(10, bitRead(redLight2, 0));
          digitalWrite(11, bitRead(redLight2, 1));
          digitalWrite(12, bitRead(redLight2, 2));
          digitalWrite(13, bitRead(redLight2, 3));
        }
      }
      delay(3000);
      dis3 = 1000;
    }

    else
    {

      digitalWrite(greenLight4, HIGH);
      digitalWrite(yellowLight4, LOW);
      digitalWrite(redLight4, LOW);

      if (dis1 < dis2 && dis1 < dis3)
      {

        digitalWrite(greenLight1, LOW);
        digitalWrite(yellowLight1, HIGH);
        digitalWrite(redLight1, LOW);

        digitalWrite(greenLight3, LOW);
        digitalWrite(yellowLight3, LOW);
        digitalWrite(redLight3, HIGH);

        for (int z = 0; z < 4; z++)
        {
          digitalWrite(10, bitRead(redLight2, 0));
          digitalWrite(11, bitRead(redLight2, 1));
          digitalWrite(12, bitRead(redLight2, 2));
          digitalWrite(13, bitRead(redLight2, 3));
        }
      }

      else if (dis2 < dis1 && dis2 < dis3)
      {

        for (int z = 0; z < 4; z++)
        {
          digitalWrite(10, bitRead(yellowLight2, 0));
          digitalWrite(11, bitRead(yellowLight2, 1));
          digitalWrite(12, bitRead(yellowLight2, 2));
          digitalWrite(13, bitRead(yellowLight2, 3));
        }

        digitalWrite(greenLight1, LOW);
        digitalWrite(yellowLight1, LOW);
        digitalWrite(redLight1, HIGH);

        digitalWrite(greenLight3, LOW);
        digitalWrite(yellowLight3, LOW);
        digitalWrite(redLight3, HIGH);
      }

      else
      {

        digitalWrite(greenLight3, LOW);
        digitalWrite(yellowLight3, HIGH);
        digitalWrite(redLight3, LOW);

        digitalWrite(greenLight1, LOW);
        digitalWrite(yellowLight1, LOW);
        digitalWrite(redLight1, HIGH);

        for (int z = 0; z < 4; z++)
        {
          digitalWrite(10, bitRead(redLight2, 0));
          digitalWrite(11, bitRead(redLight2, 1));
          digitalWrite(12, bitRead(redLight2, 2));
          digitalWrite(13, bitRead(redLight2, 3));
        }
      }
      delay(3000);
      dis4 = 1000;
    }

  }



  delay(5000);
}