#include <LiquidCrystal.h>

#define FEET_PER_MILE 5280
#define TRACK_LEN 1320
#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 = 8;
const byte LED_Stage = 7;
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 = A0;
const byte Stage_Sensor = A1;
const byte Finish_Sensor = A2;
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;
unsigned long reactionTime;
unsigned long vehicleStart;
bool FinishFlag;
unsigned long FinishET;
//float ReactSec;


int Pre_Stage_Sensor_Value;
int Stage_Sensor_Value;
int Finish_Sensor_Value;

bool Staged = false;



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);
  pinMode(RESET_BTN, INPUT_PULLUP);
  //Serial.begin(BAUD_RATE);
  Serial.begin(115200);
  lcd.begin(16, 2);
  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..

  Pre_Stage_Sensor_Value = analogRead(Pre_Stage_Sensor);
  Stage_Sensor_Value = analogRead(Stage_Sensor);
  Finish_Sensor_Value = analogRead (Finish_Sensor);
  /*
    Serial.print("PreStage: ");
    Serial.println(Pre_Stage_Sensor_Value);
    Serial.print("Stage: ");
    Serial.println(Stage_Sensor_Value);
    Serial.print("Finish: ");
    Serial.println(Finish_Sensor_Value);
    Serial.print("State: ");
    Serial.println(state);
    Serial.println();
    delay(2000);
  */
  //enter the state machine..
  switch (state) {
    case 0: //prestasge state..
      if (Pre_Stage_Sensor_Value > 500) {
        digitalWrite(LED_Prestage, LOW);
      }
      else {
        digitalWrite(LED_Prestage, HIGH);
        state++;
      }
      break;

    case 1: // Vehicle Staging State
      if (Stage_Sensor_Value > 500) {
        digitalWrite(LED_Stage, LOW);
      }
      else {
        digitalWrite(LED_Stage, HIGH);

      }

      if (Stage_Sensor_Value < 500) {
        lcd.clear();
        lcd.print("Vehicle Ready");
        state++;
      }
      else {
        if (!Staged) {
          lcd.clear();
          lcd.print("Please Stage");
          Staged = true;
          state--;
        }
      }

      break;

    case 2: //check stage sensor and roll state back
      if (Stage_Sensor_Value > 501) {
        state--;
        Staged = false;
      }
      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..
      if (Stage_Sensor_Value > 500 && StartFlag)
      {
        vehicleStart = millis();
        reactionTime = millis();
        state++;
      }

      break;

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

    case 6:
      if (Finish_Sensor_Value < 500)
      { if (!FinishFlag)
        { FinishFlag = true;
          FinishET = millis() - vehicleStart;
          lcd.clear();
          lcd.print("ET:");
          float secs = float(FinishET) / 1000;
          lcd.print(secs);
          lcd.setCursor(8, 0);
          float ReactSecs = float(reactionTime - raceStart) / 1000;
          lcd.print("RT:"); lcd.print(ReactSecs, 2);
          lcd.setCursor(0, 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 FlipAni();
      } else FlipAni();

      break;


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


  }
}// END LOOP BRACKETS

bool CheckEarlyStart()
{
  bool early = false;
  if (Stage_Sensor_Value > 501) {
    reactionTime = CountDownFin - (millis() - countdownStart);
    digitalWrite(LED_Y3, LOW);
    digitalWrite(LED_Prestage, LOW);
    digitalWrite(LED_Stage, LOW);
    digitalWrite(LED_RED_Light, HIGH);
    float ReactSecs = (float(reactionTime) / 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(15, 0); lcd.print("|"); break;
      case 1: flip = 2; lcd.setCursor(0, 0); lcd.print("/"); lcd.setCursor(15, 0); lcd.print("/"); break;
      case 2: flip = 3; lcd.setCursor(0, 0); lcd.print("-"); lcd.setCursor(15, 0); lcd.print("-"); break;
      case 3: flip = 4; lcd.setCursor(0, 0); lcd.print("\x01"); lcd.setCursor(15, 0); lcd.print("\x01"); break;
      case 4: flip = 0; lcd.setCursor(0, 0); lcd.print("|"); lcd.setCursor(15, 0); lcd.print("|"); break;
    }
  }
}