#include <LiquidCrystal.h>

#define FEET_PER_MILE 5280
#define TRACK_LEN 1320
#define NUM_LANES 2
#define RESET_BTN 47
const byte RS = 52;
const byte EN = 49;
const byte D4 = 53;
const byte D5 = 50;
const byte D6 = 51;
const byte D7 = 48;
const byte BAUD_RATE = 9600;
const byte LED_Prestage[2] ={ 8,11};
const byte LED_Stage[2] ={ 7,12};
const byte LED_Y1 = 6;
const byte LED_Y2 = 5;
const byte LED_Y3 = 4;
const byte LED_Start = 3;
const byte LED_RED_Light = 2;

const byte Pre_Stage_Sensor[NUM_LANES] = {A0, A3};
const byte Stage_Sensor[NUM_LANES] = {A1, A4};
const byte Finish_Sensor[NUM_LANES] = {A2, A5};

const byte Start_Button = 46;
//how long is the countdown
const int  CountDownFin = 2000;

//define missing slash char for Flip ani..
uint8_t slash[8] = {
  0b10000,
  0b10000,
  0b01000,
  0b00100,
  0b00100,
  0b00010,
  0b00001,
  0b00001,
};

byte flip = 0;
unsigned long AniMilli = 0;
int IntervalAni = 50;

int state = 0;
bool StartFlag = false;
unsigned long countdownStart;
unsigned long raceStart;

bool Launched[NUM_LANES] = {false, false};
unsigned long reactionTime[NUM_LANES];
unsigned long vehicleStart[NUM_LANES];
bool FinishFlag[NUM_LANES];
unsigned long FinishET[NUM_LANES];


//float ReactSec;


int Pre_Stage_Sensor_Value[NUM_LANES];
int Stage_Sensor_Value[NUM_LANES];
int Finish_Sensor_Value[NUM_LANES];

bool Staged[NUM_LANES] = {false,false};
bool lastStaged[NUM_LANES] = {true,true};
bool needClear = true;



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

void setup() {
  pinMode(LED_Y1, OUTPUT);
  pinMode(LED_Y2, OUTPUT);
  pinMode(LED_Y3, OUTPUT);
  pinMode(LED_Start, OUTPUT);
  pinMode(LED_RED_Light, OUTPUT);
  for (int i = 0; i < NUM_LANES; i++)
  {
    pinMode(LED_Prestage[i], OUTPUT);
    pinMode(LED_Stage[i], OUTPUT);
    pinMode(Pre_Stage_Sensor[i], INPUT);
    pinMode(Stage_Sensor[i], INPUT);
    pinMode(Finish_Sensor[i], INPUT);
  }
  pinMode(Start_Button, INPUT_PULLUP);
  pinMode(RESET_BTN, INPUT_PULLUP);
  //Serial.begin(BAUD_RATE);
  Serial.begin(115200);
  lcd.begin(20, 4);
  lcd.createChar(1, slash);
  lcd.setCursor(0, 1);
  lcd.print(F("System Ready.."));
  delay(2000);

  lcd.clear();
}

void loop()
{

  //read in our sensors start of every loop..
  //don't need anymore analogRead anywhere else but here..
  for (int i = 0; i < NUM_LANES; i++) {
    Pre_Stage_Sensor_Value[i] = analogRead(Pre_Stage_Sensor[i]);
    Stage_Sensor_Value[i] = analogRead(Stage_Sensor[i]);
    Finish_Sensor_Value[i] = analogRead (Finish_Sensor[i]);
  }
  //used later in the loop
  int j = 0;



  /*
    Serial.print("PreStage: ");
    Serial.println(Pre_Stage_Sensor_Value[0]);
    Serial.print("Stage: ");
    Serial.println(Stage_Sensor_Value[0]);
    Serial.print("Finish: ");
    Serial.println(Finish_Sensor_Value[0]);
    Serial.print("State: ");
    Serial.println(state);
    Serial.println();
  */


  //enter the state machine..
  switch (state) {
    case 0: //prestasge state..
      if (Pre_Stage_Sensor_Value[0] > 500 || Pre_Stage_Sensor_Value[1] > 500 ) {
       if (Pre_Stage_Sensor_Value[0] > 500)
       { 
          lcd.setCursor(0,1);
          lcd.print("L1:Please Pre-stage "); 
        digitalWrite(LED_Prestage[0], LOW);
        } else {
          lcd.setCursor(0,1);
          lcd.print("L1:Pre-staged       "); 
        }
       if (Pre_Stage_Sensor_Value[1] > 500)
       { 
          lcd.setCursor(0,2);
          lcd.print("L2:Please Pre-stage "); 
        digitalWrite(LED_Prestage[1], LOW);
       } else {
          lcd.setCursor(0,2);
          lcd.print("L2:Pre-staged       "); 
       }
      }
      else {
        digitalWrite(LED_Prestage[0], HIGH);
        digitalWrite(LED_Prestage[1], HIGH);
       // lcd.clear();
        state++;
      }
      break;

    case 1: // Vehicle Staging State
      if (Stage_Sensor_Value[0] > 500 || Stage_Sensor_Value[1] > 500 ) {
       if (Stage_Sensor_Value[0] > 500){  
        digitalWrite(LED_Stage[0], LOW);
        Staged[0] = false;
        } else {
        digitalWrite(LED_Stage[0], HIGH);
        Staged[0] = true;
        }
       if (Stage_Sensor_Value[1] > 500){  
        digitalWrite(LED_Stage[1], LOW);
       Staged[1] = false;
       } else {
        digitalWrite(LED_Stage[1], HIGH);
       Staged[1] = true;
       }

        if (Staged[0]!= lastStaged[0]) {
          lcd.setCursor(0,1);
         if (Stage_Sensor_Value[0] > 500)  
          lcd.print("Lane1 : Please Stage"); else
          lcd.print("Lane1 : Staged      "); 
          lastStaged[0] = Staged[0];
        }
        if (Staged[1]!= lastStaged[1]){
          lcd.setCursor(0,2);
         if (Stage_Sensor_Value[1] > 500)  
          lcd.print("Lane2 : Please Stage"); else
          lcd.print("Lane2 : Staged      "); 
          lastStaged[1] = Staged[1];
          }
         // Staged = true;
          state--;
      }
      else {
        digitalWrite(LED_Stage[0], HIGH);
        digitalWrite(LED_Stage[0], HIGH);
        Staged[0] = true;
        Staged[1] = true;
        lcd.clear();
        lcd.print("Vehicles Ready");
        state++;
      }

      break;

    case 2: //check stage sensor and roll state back
      if (Stage_Sensor_Value[0] > 501 || Stage_Sensor_Value[1] > 501) {
        lcd.clear();
        state--;
        if (Stage_Sensor_Value[0] > 501) Staged[0] = false; lastStaged[0] = true;
        if (Stage_Sensor_Value[1] > 501) Staged[1] = false; lastStaged[1] = true;
      }
      else
      { //staged good .. check start button
        if (digitalRead(Start_Button) == LOW)
        {
          countdownStart = millis();
          state++;
        }
      }
      break;

    case 3: //state 3 counts down leds and checks for early start..
      //check for early start first..
      if (!CheckEarlyStart()) {
        //not an early start, countdown
        if (millis() - countdownStart > 2000) //countdown done
        { //check sensor just before dropping the flag
          digitalWrite(LED_Y3, LOW);
          digitalWrite(LED_Start, HIGH);
          StartFlag = true;
          raceStart = millis();//start counting race from here..
          state++;
        }
        else if (millis() - countdownStart > 1500)
        {
          digitalWrite(LED_Y2, LOW);
          digitalWrite(LED_Y3, HIGH);
        }
        else if (millis() - countdownStart > 1000)
        {
          digitalWrite(LED_Y1, LOW);
          digitalWrite(LED_Y2, HIGH);
        }
        else if (millis() - countdownStart > 500)
        {
          digitalWrite(LED_Y1, HIGH);
        }

      }
      break;

    case 4: //stage to get reaction time
      //need to see car move before next state..
      for (j = 0; j < NUM_LANES; j++) {
        if (Stage_Sensor_Value[j] > 500 && StartFlag && !Launched[j])
        {
          vehicleStart[j] = millis();
          reactionTime[j] = millis();
          Launched[j] =  true;
        }
      }
      if (Launched[0] && Launched[1]) state++;
      break;

    case 5:
      //prints go and reaction time..
      {
        float ReactSecs[2] = {float(reactionTime[0] - raceStart) / 1000, float(reactionTime[1] - raceStart) / 1000};
        lcd.clear();
        lcd.setCursor(0, 1);
        lcd.print("RT1:"); lcd.print(ReactSecs[0], 2);
        lcd.setCursor(0, 3);
        lcd.print("RT2:"); lcd.print(ReactSecs[1], 2);
        Serial.print("RT1:"); Serial.println(ReactSecs[0], 2);
        Serial.print("RT2:"); Serial.println(ReactSecs[1], 2);
        state = 6;
      }
      break;

    case 6:
      for (j = 0; j < NUM_LANES; j++) {
        if (Finish_Sensor_Value[j] < 500)
        {
          if (!FinishFlag[j])
          { FinishFlag[j] = true;
            FinishET[j] = millis() - vehicleStart[j];
            if (needClear)
            {
              lcd.clear();
              needClear = false;
            }

            lcd.setCursor(0, j * 2);
            lcd.print("ET:");
            float secs = float(FinishET[j]) / 1000;
            lcd.print(secs);
            lcd.setCursor(8, j * 2);
            float ReactSecs = float(reactionTime[j] - raceStart) / 1000;
            lcd.print("RT:"); lcd.print(ReactSecs, 2);
            lcd.setCursor(0, j * 2 + 1);
            lcd.print("MPH:");
            float fps = (TRACK_LEN / secs);
            Serial.print("ET:"); Serial.println(secs, 2);
            float mph = (fps / FEET_PER_MILE) / 0.00028;
            Serial.print("MPH:"); Serial.println(mph, 2);
            lcd.print(mph, 2);
            // state = 7;
          } else {
            if (!FinishFlag[0] && !FinishFlag[1]) FlipAni();
          }
        } else {
          if (!FinishFlag[0] && !FinishFlag[1]) FlipAni();
        }
      }

      if (FinishFlag[0] && FinishFlag[1]) state = 7;
      break;


    case 7:
      if (digitalRead(RESET_BTN) == LOW)
      {
        digitalWrite(LED_Start, LOW);
        digitalWrite(LED_Stage, LOW);
        digitalWrite(LED_RED_Light, LOW);
        StartFlag = false;
        FinishFlag[0] = false;
        Staged[0] = false;
        Staged[1] = false;
        FinishFlag[1] = false;
        needClear = true;
        state = 0;
      }
      break;


  }
}// END LOOP BRACKETS

bool CheckEarlyStart()
{
  bool early = false;
  if (Stage_Sensor_Value[0] > 501 || Stage_Sensor_Value[1] > 501) {
    reactionTime[0] = CountDownFin - (millis() - countdownStart);
    digitalWrite(LED_Y3, LOW);
    digitalWrite(LED_Prestage, LOW);
    digitalWrite(LED_Stage, LOW);
    digitalWrite(LED_RED_Light, HIGH);
    float ReactSecs = (float(reactionTime[0]) / 1000) * -1;
    lcd.clear();
    lcd.print("!!Bad Start!!");
    lcd.setCursor(0, 1);
    lcd.print("RT:"); lcd.print(ReactSecs, 2);
    Serial.print("RT:"); Serial.println(ReactSecs, 2);
    state = 7;//bad start .. wait for reset..
    early = true;
  }
  return early;
}



void FlipAni() {
  if (millis() - AniMilli >= IntervalAni)
  {
    AniMilli = millis();
    switch (flip) {
      case 0: flip = 1; lcd.setCursor(0, 0); lcd.print("|"); lcd.setCursor(19, 0); lcd.print("|"); break;
      case 1: flip = 2; lcd.setCursor(0, 0); lcd.print("/"); lcd.setCursor(19, 0); lcd.print("/"); break;
      case 2: flip = 3; lcd.setCursor(0, 0); lcd.print("-"); lcd.setCursor(19, 0); lcd.print("-"); break;
      case 3: flip = 4; lcd.setCursor(0, 0); lcd.print("\x01"); lcd.setCursor(19, 0); lcd.print("\x01"); break;
      case 4: flip = 0; lcd.setCursor(0, 0); lcd.print("|"); lcd.setCursor(19, 0); lcd.print("|"); break;
    }
  }
}