#include <LiquidCrystal.h>

const int RS = 12;
const int EN = 10;
const int D4 = 5;
const int D5 = 4;
const int D6 = 3;
const int D7 = 2;
const int BAUD_RATE = 9600;

const int LED_Prestage = 13;
const int LED_Stage = 13;

const int LED_Y1 = 9;
const int LED_Y2 = 8;
const int LED_Y3 = 7;
const int LED_Start = 6;
const int LED_RED_Light =A5;

const int Pre_Stage_Sensor = A0;
const int Stage_Sensor = A1;
const int Finish_Sensor = A2;
const int Start_Button = A3;


const String (MESSAGE) = "System Ready.";

LiquidCrystal lcd(RS, EN, D4, D5, D6, D7);

void setup() {
  pinMode(LED_Prestage,OUTPUT);
  pinMode(LED_Stage,OUTPUT);
  pinMode(LED_Y1,OUTPUT);
  pinMode(LED_Y2,OUTPUT);
  pinMode(LED_Y3,OUTPUT);
  pinMode(LED_Start,OUTPUT);
  pinMode(LED_RED_Light,OUTPUT);
  pinMode(Pre_Stage_Sensor,INPUT);
  pinMode(Stage_Sensor,INPUT);
  pinMode(Finish_Sensor,INPUT);
  pinMode(Start_Button,INPUT_PULLUP);
  Serial.begin(9600);
  lcd.begin(16, 2);
  lcd.setCursor(0, 1);
  lcd.print(MESSAGE);
  

  delay(2000);
  lcd.clear();
}

void loop() {
  int Pre_Stage_Sensor_Value = analogRead(Pre_Stage_Sensor);
  int Stage_Sensor_Value = analogRead(Stage_Sensor);
  boolean StartFlag = true;

  if (Pre_Stage_Sensor_Value < 500) {
    digitalWrite(LED_Prestage, HIGH);
  } else {
    digitalWrite(LED_Prestage, LOW);
  }

  if (Stage_Sensor_Value < 500) {
    digitalWrite(LED_Stage, HIGH);
  } else {
    digitalWrite(LED_Stage, LOW);
  }

  if ((Pre_Stage_Sensor_Value <500) && (Stage_Sensor_Value <500)) {
    lcd.clear();
    lcd.setCursor(0,1);
    lcd.print("Vehicle Staged");
    delay(500);
  } else {
    lcd.clear();
    lcd.begin(16,2);
    lcd.setCursor(0,1);
    lcd.print("Please Stage");
    delay(500);
  }

  if ((Stage_Sensor_Value < 500) && (digitalRead(Start_Button) == LOW)) {
    delay (500);
    digitalWrite (LED_Y1, HIGH);
    delay (500);
    digitalWrite (LED_Y1, LOW);
    digitalWrite (LED_Y2, HIGH);
    delay (500);
    digitalWrite (LED_Y2, LOW);
    digitalWrite (LED_Y3, HIGH);
    delay (500);
    digitalWrite (LED_Y3, LOW);
    digitalWrite (LED_Start, HIGH);
    StartFlag = true; // set StartFlag to true
  }
  else if (Stage_Sensor_Value > 500) { // check if the car has left the stage
    digitalWrite(LED_Start, LOW);
    StartFlag = false;
  }

  if (StartFlag == false) {
    digitalWrite(LED_RED_Light, HIGH);
  } else {
    digitalWrite(LED_RED_Light, LOW);
  }
}