#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

// Ultrasonic sensors
// Northbound
const int trigPinN = 50;
const int echoPinN = 51;
long durationN;
int distanceN;

// Southbound
const int trigPinS = 52;
const int echoPinS = 53;
long durationS;
int distanceS;

// Eastbound
const int trigPinE = 48;
const int echoPinE = 49;
long durationE;
int distanceE;

// Westbound
const int trigPinW = 46;
const int echoPinW = 47;
long durationW;
int distanceW;

int prevDistanceN = 0, prevDistanceS = 0, prevDistanceE = 0, prevDistanceW = 0;


// Traffic lights
const int nsGreen = 31;
const int nsYellow = 32;
const int nsRed = 33;
const int ewGreen = 34;
const int ewYellow = 35;
const int ewRed = 36;

// Blue lights
const int blueLightNorth = 37;
const int blueLightSouth = 38;
const int blueLightEast = 39;
const int blueLightWest = 40;

// Seven segment displays
const int nsA = 2;
const int nsB = 3;
const int nsC = 4;
const int nsD = 5;
const int nsE = 6;
const int nsF = 7;
const int nsG = 8;
const int ewA = 22;
const int ewB = 23;
const int ewC = 24;
const int ewD = 25;
const int ewE = 26;
const int ewF = 27;
const int ewG = 28;

// Push button
const int buttonPin = 41;
int buttonState = 0;

// Timer variables
unsigned long lastChangeTime = 0;
const unsigned long changeInterval = 1000; // 1 second for timer
int nsTimer = 9, ewTimer = 9; // Timers for NS and EW

// States
enum State {NS_GREEN, NS_YELLOW, EW_GREEN, EW_YELLOW};
State currentState = NS_GREEN;

void setup() {
  lcd.init();
  lcd.clear();
  lcd.backlight();

  // Initialize ultrasonic sensors
  pinMode(trigPinN, OUTPUT);
  pinMode(echoPinN, INPUT);
  pinMode(trigPinS, OUTPUT);
  pinMode(echoPinS, INPUT);
  pinMode(trigPinE, OUTPUT);
  pinMode(echoPinE, INPUT);
  pinMode(trigPinW, OUTPUT);
  pinMode(echoPinW, INPUT);

  // Initialize traffic lights
  pinMode(nsGreen, OUTPUT);
  pinMode(nsYellow, OUTPUT);
  pinMode(nsRed, OUTPUT);
  pinMode(ewGreen, OUTPUT);
  pinMode(ewYellow, OUTPUT);
  pinMode(ewRed, OUTPUT);

  // Initialize blue lights
  pinMode(blueLightNorth, OUTPUT);
  pinMode(blueLightSouth, OUTPUT);
  pinMode(blueLightEast, OUTPUT);
  pinMode(blueLightWest, OUTPUT);

  // Initialize seven segment displays
  pinMode(nsA, OUTPUT);
  pinMode(nsB, OUTPUT);
  pinMode(nsC, OUTPUT);
  pinMode(nsD, OUTPUT);
  pinMode(nsE, OUTPUT);
  pinMode(nsF, OUTPUT);
  pinMode(nsG, OUTPUT);
  pinMode(ewA, OUTPUT);
  pinMode(ewB, OUTPUT);
  pinMode(ewC, OUTPUT);
  pinMode(ewD, OUTPUT);
  pinMode(ewE, OUTPUT);
  pinMode(ewF, OUTPUT);
  pinMode(ewG, OUTPUT);

  // Initialize button
  pinMode(buttonPin, INPUT);

  Serial.begin(9600);

  // Set initial traffic light states
  digitalWrite(nsGreen, HIGH);
  digitalWrite(nsRed, LOW);
  digitalWrite(ewGreen, LOW);
  digitalWrite(ewRed, HIGH);
}

void loop() {
  checkDistanceDetectors();
 // handlePedestrianTimer();
 // checkButton();

  lcd.print("                       ");
  lcd.setCursor(0, 0);
  lcd.print(distanceN);
  lcd.print(" cmN   ");
  lcd.setCursor(9, 0);
  lcd.print(distanceS);
  lcd.print(" cmS   ");
  lcd.setCursor(0, 1);
  lcd.print(distanceE);
  lcd.print(" cmE   ");
  lcd.setCursor(9, 1);
  lcd.print(distanceW);
  lcd.print(" cmW   ");
}

void checkDistanceDetectors() {
  distanceN = getDistance(trigPinN, echoPinN);
  distanceS = getDistance(trigPinS, echoPinS);
  distanceE = getDistance(trigPinE, echoPinE);
  distanceW = getDistance(trigPinW, echoPinW);

  bool carCrossedN = hasCarCrossed(prevDistanceN, distanceN);
  bool carCrossedS = hasCarCrossed(prevDistanceS, distanceS);
  bool carCrossedE = hasCarCrossed(prevDistanceE, distanceE);
  bool carCrossedW = hasCarCrossed(prevDistanceW, distanceW);

  digitalWrite(blueLightNorth, (isNSRed() && carCrossedN) ? HIGH : LOW);
  digitalWrite(blueLightSouth, (isNSRed() && carCrossedS) ? HIGH : LOW);
  digitalWrite(blueLightEast, (isEWRed() && carCrossedE) ? HIGH : LOW);
  digitalWrite(blueLightWest, (isEWRed() && carCrossedW) ? HIGH : LOW);

  if ((currentState == NS_GREEN && (carCrossedE || carCrossedW)) || 
      (currentState == EW_GREEN && (carCrossedN || carCrossedS))) {
    lastChangeTime = millis();
  }

  prevDistanceN = distanceN;
  prevDistanceS = distanceS;
  prevDistanceE = distanceE;
  prevDistanceW = distanceW;
}

int getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}

bool hasCarCrossed(int prevDistance, int currDistance) {
  return (currDistance < prevDistance);
}


void handlePedestrianTimer() {
  if (millis() - lastChangeTime >= changeInterval) {
    lastChangeTime = millis();
    if (currentState == NS_GREEN || currentState == NS_YELLOW) {
      nsTimer--;
      displayNS(nsTimer);
      if (nsTimer < 0) {
        blinkZeroNS();
        changeLights();
        nsTimer = 9;
      }
    } else if (currentState == EW_GREEN || currentState == EW_YELLOW) {
      ewTimer--;
      displayEW(ewTimer);
      if (ewTimer < 0) {
        blinkZeroEW();
        changeLights();
        ewTimer = 9;
      }
    }
  }
}

void checkButton() {
  if (digitalRead(buttonPin) == HIGH) {
    if (currentState == NS_GREEN || currentState == NS_YELLOW) {
      nsTimer = 0;
      blinkZeroNS();
      changeLights();
    } else if (currentState == EW_GREEN || currentState == EW_YELLOW) {
      ewTimer = 0;
      blinkZeroEW();
      changeLights();
    }
  }
}

void changeLights() {
  switch (currentState) {
    case NS_GREEN:
      currentState = NS_YELLOW;
      digitalWrite(nsGreen, LOW);
      digitalWrite(nsYellow, HIGH);
      delay(1000);
      currentState = EW_GREEN;
      digitalWrite(nsYellow, LOW);
      digitalWrite(nsRed, HIGH);
      delay(1000);
      digitalWrite(ewRed, LOW);
      digitalWrite(ewGreen, HIGH);
      break;
    case EW_GREEN:
      currentState = EW_YELLOW;
      digitalWrite(ewGreen, LOW);
      digitalWrite(ewYellow, HIGH);
      delay(1000);
      currentState = NS_GREEN;
      digitalWrite(ewYellow, LOW);
      digitalWrite(ewRed, HIGH);
      delay(1000);
      digitalWrite(nsRed, LOW);
      digitalWrite(nsGreen, HIGH);
      break;
    default:
      break;
  }
  lastChangeTime = millis();
}

// Update NS seven-segment display
void displayNS(int num) {
  nsClearAll();
  switch (num) {
    case 0: nsDigitZero(); break;
    case 1: nsDigitOne(); break;
    case 2: nsDigitTwo(); break;
    case 3: nsDigitThree(); break;
    case 4: nsDigitFour(); break;
    case 5: nsDigitFive(); break;
    case 6: nsDigitSix(); break;
    case 7: nsDigitSeven(); break;
    case 8: nsDigitEight(); break;
    case 9: nsDigitNine(); break;
    default: nsDigitZero(); break;
  }
}

// Update EW seven-segment display
void displayEW(int num) {
  ewClearAll();
  switch (num) {
    case 0: ewDigitZero(); break;
    case 1: ewDigitOne(); break;
    case 2: ewDigitTwo(); break;
    case 3: ewDigitThree(); break;
    case 4: ewDigitFour(); break;
    case 5: ewDigitFive(); break;
    case 6: ewDigitSix(); break;
    case 7: ewDigitSeven(); break;
    case 8: ewDigitEight(); break;
    case 9: ewDigitNine(); break;
    default: ewDigitZero(); break;
  }
}

// Blink zero on NS display
void blinkZeroNS() {
  for (int i = 0; i < 3; i++) {
    nsDigitZero();
    delay(200);
    nsClearAll();
    delay(200);
  }
}

// Blink zero on EW display
void blinkZeroEW() {
  for (int i = 0; i < 3; i++) {
    ewDigitZero();
    delay(200);
    ewClearAll();
    delay(200);
  }
}

// Seven segment display functions
void nsClearAll() {
  digitalWrite(nsA, LOW);
  digitalWrite(nsB, LOW);
  digitalWrite(nsC, LOW);
  digitalWrite(nsD, LOW);
  digitalWrite(nsE, LOW);
  digitalWrite(nsF, LOW);
  digitalWrite(nsG, LOW);
}

void nsDigitZero() {
  nsClearAll();
  digitalWrite(nsA, HIGH);
  digitalWrite(nsB, HIGH);
  digitalWrite(nsC, HIGH);
  digitalWrite(nsD, HIGH);
  digitalWrite(nsE, HIGH);
  digitalWrite(nsF, HIGH);
}

void nsDigitOne() {
  nsClearAll();
  digitalWrite(nsB, HIGH);
  digitalWrite(nsC, HIGH);
}

void nsDigitTwo() {
  nsClearAll();
  digitalWrite(nsA, HIGH);
  digitalWrite(nsB, HIGH);
  digitalWrite(nsD, HIGH);
  digitalWrite(nsE, HIGH);
  digitalWrite(nsG, HIGH);
}

void nsDigitThree() {
  nsClearAll();
  digitalWrite(nsA, HIGH);
  digitalWrite(nsB, HIGH);
  digitalWrite(nsC, HIGH);
  digitalWrite(nsD, HIGH);
  digitalWrite(nsG, HIGH);
}

void nsDigitFour() {
  nsClearAll();
  digitalWrite(nsB, HIGH);
  digitalWrite(nsC, HIGH);
  digitalWrite(nsF, HIGH);
  digitalWrite(nsG, HIGH);
}

void nsDigitFive() {
  nsClearAll();
  digitalWrite(nsA, HIGH);
  digitalWrite(nsC, HIGH);
  digitalWrite(nsD, HIGH);
  digitalWrite(nsF, HIGH);
  digitalWrite(nsG, HIGH);
}

void nsDigitSix() {
  nsClearAll();
  digitalWrite(nsA, HIGH);
  digitalWrite(nsC, HIGH);
  digitalWrite(nsD, HIGH);
  digitalWrite(nsE, HIGH);
  digitalWrite(nsF, HIGH);
  digitalWrite(nsG, HIGH);
}

void nsDigitSeven() {
  nsClearAll();
  digitalWrite(nsA, HIGH);
  digitalWrite(nsB, HIGH);
  digitalWrite(nsC, HIGH);
}

void nsDigitEight() {
  nsClearAll();
  digitalWrite(nsA, HIGH);
  digitalWrite(nsB, HIGH);
  digitalWrite(nsC, HIGH);
  digitalWrite(nsD, HIGH);
  digitalWrite(nsE, HIGH);
  digitalWrite(nsF, HIGH);
  digitalWrite(nsG, HIGH);
}

void nsDigitNine() {
  nsClearAll();
  digitalWrite(nsA, HIGH);
  digitalWrite(nsB, HIGH);
  digitalWrite(nsC, HIGH);
  digitalWrite(nsD, HIGH);
  digitalWrite(nsF, HIGH);
  digitalWrite(nsG, HIGH);
}

void ewClearAll() {
  digitalWrite(ewA, LOW);
  digitalWrite(ewB, LOW);
  digitalWrite(ewC, LOW);
  digitalWrite(ewD, LOW);
  digitalWrite(ewE, LOW);
  digitalWrite(ewF, LOW);
  digitalWrite(ewG, LOW);
}

void ewDigitZero() {
  ewClearAll();
  digitalWrite(ewA, HIGH);
  digitalWrite(ewB, HIGH);
  digitalWrite(ewC, HIGH);
  digitalWrite(ewD, HIGH);
  digitalWrite(ewE, HIGH);
  digitalWrite(ewF, HIGH);
}

void ewDigitOne() {
  ewClearAll();
  digitalWrite(ewB, HIGH);
  digitalWrite(ewC, HIGH);
}

void ewDigitTwo() {
  ewClearAll();
  digitalWrite(ewA, HIGH);
  digitalWrite(ewB, HIGH);
  digitalWrite(ewD, HIGH);
  digitalWrite(ewE, HIGH);
  digitalWrite(ewG, HIGH);
}

void ewDigitThree() {
  ewClearAll();
  digitalWrite(ewA, HIGH);
  digitalWrite(ewB, HIGH);
  digitalWrite(ewC, HIGH);
  digitalWrite(ewD, HIGH);
  digitalWrite(ewG, HIGH);
}

void ewDigitFour() {
  ewClearAll();
  digitalWrite(ewB, HIGH);
  digitalWrite(ewC, HIGH);
  digitalWrite(ewF, HIGH);
  digitalWrite(ewG, HIGH);
}

void ewDigitFive() {
  ewClearAll();
  digitalWrite(ewA, HIGH);
  digitalWrite(ewC, HIGH);
  digitalWrite(ewD, HIGH);
  digitalWrite(ewF, HIGH);
  digitalWrite(ewG, HIGH);
}

void ewDigitSix() {
  ewClearAll();
  digitalWrite(ewA, HIGH);
  digitalWrite(ewC, HIGH);
  digitalWrite(ewD, HIGH);
  digitalWrite(ewE, HIGH);
  digitalWrite(ewF, HIGH);
  digitalWrite(ewG, HIGH);
}

void ewDigitSeven() {
  ewClearAll();
  digitalWrite(ewA, HIGH);
  digitalWrite(ewB, HIGH);
  digitalWrite(ewC, HIGH);
}

void ewDigitEight() {
  ewClearAll();
  digitalWrite(ewA, HIGH);
  digitalWrite(ewB, HIGH);
  digitalWrite(ewC, HIGH);
  digitalWrite(ewD, HIGH);
  digitalWrite(ewE, HIGH);
  digitalWrite(ewF, HIGH);
  digitalWrite(ewG, HIGH);
}

void ewDigitNine() {
  ewClearAll();
  digitalWrite(ewA, HIGH);
  digitalWrite(ewB, HIGH);
  digitalWrite(ewC, HIGH);
  digitalWrite(ewD, HIGH);
  digitalWrite(ewF, HIGH);
  digitalWrite(ewG, HIGH);
}

bool isNSRed() {
  return digitalRead(nsRed) == HIGH;
}

bool isEWRed() {
  return digitalRead(ewRed) == HIGH;
}