#include <LiquidCrystal.h>

enum States
{
  RESET,
  WAITING_FOR_PRE_STAGE,
  WAITING_FOR_STAGE,
  WAITING_FOR_START_BUTTON,
  WAITING_FOR_COUNTDOWN_1,
  WAITING_FOR_COUNTDOWN_2,
  WAITING_FOR_COUNTDOWN_3,
  WAITING_FOR_COUNTDOWN_4,
  FALSE_START,
  WAITING_FOR_GO,
  WAITING_FOR_FINISH,
  FINISHED,
  WAITING_FOR_RESET_BUTTON
};

const float FEET_PER_MILE = 5280;
const float TRACK_LEN = 1320;

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 LED_PRE_STAGE_PIN =  8;
const byte LED_STAGE_PIN = 7;
const byte LED_Y1_PIN = 6;
const byte LED_Y2_PIN = 5;
const byte LED_Y3_PIN = 4;
const byte LED_START_PIN = 3;
const byte LED_RED_LIGHT_PIN = 2;
const byte PRE_STAGE_SENSOR_PIN = A0;
const byte STAGE_SENSOR_PIN = A1;
const byte FINISH_SENSOR_PIN = A2;
const byte START_BUTTON_PIN = 46;
const byte RESET_BUTTON_PIN = 47;

const uint8_t BACKSLASH[8] =
{
  0b00000,
  0b10000,
  0b01000,
  0b00100,
  0b00010,
  0b00001,
  0b00000,
  0b00000,
};

States presentState = -1;  // Purposefully initialised to an invalid state.
States nextState = RESET;

unsigned long timestamp;
int preStageSensorValue;
int stageSensorValue;
int finishSensorValue;
bool startButtonActive;
bool resetButtonActive;

unsigned long timerStartTime;
unsigned long timerElapsedTime;
bool countdown1Active;
bool countdown2Active;
bool countdown3Active;
bool countdown4Active;

unsigned long raceStartTime;
unsigned long raceEndTime;
unsigned long reactionTime;

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

void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(LED_PRE_STAGE_PIN, OUTPUT);
  pinMode(LED_STAGE_PIN, OUTPUT);
  pinMode(LED_Y1_PIN, OUTPUT);
  pinMode(LED_Y2_PIN, OUTPUT);
  pinMode(LED_Y3_PIN, OUTPUT);
  pinMode(LED_START_PIN, OUTPUT);
  pinMode(LED_RED_LIGHT_PIN, OUTPUT);

  pinMode(PRE_STAGE_SENSOR_PIN, INPUT);
  pinMode(STAGE_SENSOR_PIN, INPUT);
  pinMode(FINISH_SENSOR_PIN, INPUT);
  pinMode(START_BUTTON_PIN, INPUT_PULLUP);
  pinMode(RESET_BUTTON_PIN, INPUT_PULLUP);

  Serial.begin(115200);
  Serial.println();
  Serial.println(F("Setup"));
  Serial.println();

  lcd.createChar(1, BACKSLASH);
  lcd.begin(16, 2);
  lcd.setCursor(0, 1);
  lcd.print(F("System Ready"));
  delay(2000);
  lcd.clear();
}

void loop()
{
  if (presentState != nextState)
  {
    presentState = nextState;
    OutputLogic();
  }
  InputLogic();
  NextStateLogic();
  Animation();
  BlinkLed();
}

void InputLogic()
{
  timestamp = millis();
  preStageSensorValue = analogRead(PRE_STAGE_SENSOR_PIN);
  stageSensorValue = analogRead(STAGE_SENSOR_PIN);
  finishSensorValue = analogRead(FINISH_SENSOR_PIN);
  startButtonActive = digitalRead(START_BUTTON_PIN) == LOW;
  resetButtonActive = digitalRead(RESET_BUTTON_PIN) == LOW;
  timerElapsedTime = timestamp - timerStartTime;
  countdown1Active = timerElapsedTime > 500;
  countdown2Active = timerElapsedTime > 1000;
  countdown3Active = timerElapsedTime > 1500;
  countdown4Active = timerElapsedTime > 2000;
}

void NextStateLogic()
{
  switch (presentState)
  {
    case RESET:
      nextState = WAITING_FOR_PRE_STAGE;
      break;
    case WAITING_FOR_PRE_STAGE:
      if (preStageSensorValue < 500)
      {
        nextState = WAITING_FOR_STAGE;
      }
      break;
    case WAITING_FOR_STAGE:
      if (stageSensorValue < 500)
      {
        nextState = WAITING_FOR_START_BUTTON;
      }
      else if (preStageSensorValue > 500)
      {
        nextState = WAITING_FOR_PRE_STAGE;
      }
      break;
    case WAITING_FOR_START_BUTTON:
      if (stageSensorValue > 500)
      {
        nextState = WAITING_FOR_STAGE;
      }
      else if (startButtonActive)
      {
        timerStartTime = timestamp;
        nextState = WAITING_FOR_COUNTDOWN_1;
      }
      break;
    case WAITING_FOR_COUNTDOWN_1:
      if (countdown1Active)
      {
        nextState = WAITING_FOR_COUNTDOWN_2;
      }
      break;
    case WAITING_FOR_COUNTDOWN_2:
      if (countdown2Active)
      {
        nextState = WAITING_FOR_COUNTDOWN_3;
      }
      break;
    case WAITING_FOR_COUNTDOWN_3:
      if (countdown3Active)
      {
        nextState = WAITING_FOR_COUNTDOWN_4;
      }
      break;
    case WAITING_FOR_COUNTDOWN_4:
      if (countdown4Active)
      {
        // Check for false start.
        if (stageSensorValue > 500)
        {
          nextState = FALSE_START;
        }
        else
        {
          raceStartTime = timestamp;
          nextState = WAITING_FOR_GO;
        }
      }
      break;
    case FALSE_START:
      nextState = WAITING_FOR_RESET_BUTTON;
      break;
    case WAITING_FOR_GO:
      if (stageSensorValue > 500)
      {
        reactionTime = timestamp - raceStartTime;
        nextState = WAITING_FOR_FINISH;
      }
      break;
    case WAITING_FOR_FINISH:
      if (finishSensorValue < 500)
      {
        raceEndTime = timestamp;
        nextState = FINISHED;
      }
      break;
    case FINISHED:
      nextState = WAITING_FOR_RESET_BUTTON;
      break;
    case WAITING_FOR_RESET_BUTTON:
      if (resetButtonActive)
      {
        nextState = RESET;
      }
      break;
  }
}

void OutputLogic()
{
  Serial.print(F("timestamp="));
  Serial.print(timestamp);
  Serial.print(F(";  State="));
  Serial.println(presentState);
  switch (presentState)
  {
    case RESET:
      digitalWrite(LED_PRE_STAGE_PIN, LOW);
      digitalWrite(LED_STAGE_PIN, LOW);
      digitalWrite(LED_START_PIN, LOW);
      digitalWrite(LED_RED_LIGHT_PIN, LOW);
      digitalWrite(LED_Y1_PIN, LOW);
      digitalWrite(LED_Y2_PIN, LOW);
      digitalWrite(LED_Y3_PIN, LOW);
      lcd.clear();
      lcd.print(F("Please Pre Stage"));
      break;
    case WAITING_FOR_PRE_STAGE:
      digitalWrite(LED_PRE_STAGE_PIN, LOW);
      digitalWrite(LED_STAGE_PIN, LOW);
      digitalWrite(LED_START_PIN, LOW);
      digitalWrite(LED_RED_LIGHT_PIN, LOW);
      lcd.setCursor(0, 0);
      lcd.print(F("Please Pre Stage"));
      break;
    case WAITING_FOR_STAGE:
      digitalWrite(LED_PRE_STAGE_PIN, HIGH);
      digitalWrite(LED_STAGE_PIN, LOW);
      lcd.setCursor(0, 0);
      lcd.print(F("Please Stage    "));
      break;
    case WAITING_FOR_START_BUTTON:
      digitalWrite(LED_STAGE_PIN, HIGH);
      lcd.setCursor(0, 0);
      lcd.print(F("Vehicle Ready   "));
      break;
    case WAITING_FOR_COUNTDOWN_1:
      break;
    case WAITING_FOR_COUNTDOWN_2:
      digitalWrite(LED_Y1_PIN, HIGH);
      break;
    case WAITING_FOR_COUNTDOWN_3:
      digitalWrite(LED_Y1_PIN, LOW);
      digitalWrite(LED_Y2_PIN, HIGH);
      break;
    case WAITING_FOR_COUNTDOWN_4:
      digitalWrite(LED_Y2_PIN, LOW);
      digitalWrite(LED_Y3_PIN, HIGH);
      break;
    case FALSE_START:
      digitalWrite(LED_Y3_PIN, LOW);
      digitalWrite(LED_PRE_STAGE_PIN, LOW);
      digitalWrite(LED_STAGE_PIN, LOW);
      digitalWrite(LED_RED_LIGHT_PIN, HIGH);
      lcd.clear();
      lcd.print(F("!!False Start!!"));
      break;
    case WAITING_FOR_GO:
      digitalWrite(LED_Y3_PIN, LOW);
      digitalWrite(LED_START_PIN, HIGH);
      lcd.clear();
      lcd.print(F("       GO        "));
      lcd.print(raceStartTime);
      break;
    case WAITING_FOR_FINISH:
      break;
    case FINISHED:
      lcd.clear();
      lcd.print(F("E"));
      float etSeconds = float(raceEndTime - raceStartTime) / 1000.0f;
      lcd.print(etSeconds, 2);
      float rtSeconds = reactionTime / 1000.0f;
      lcd.setCursor(10, 0);
      lcd.print("R");
      lcd.print(rtSeconds, 3);
      lcd.setCursor(0, 1);
      lcd.print(F("MPH:"));
      float fps = TRACK_LEN / etSeconds;
      Serial.print(F("fps:")); Serial.println(fps, 4);
      float mph = (fps / FEET_PER_MILE) / 0.00028;
      Serial.print(F("MPH:")); Serial.println(mph, 4);
      lcd.print(mph, 4);

      Serial.print(F("Reaction time:"));
      Serial.println(reactionTime / 1000.0f, 3);
      break;
    case WAITING_FOR_RESET_BUTTON:
      break;
  }
}

void Animation()
{
  if (presentState == WAITING_FOR_FINISH)
  {
    {
      static byte flip = 0;
      const unsigned long INTERVAL = 50;
      static unsigned long previous = 0;
      if (timestamp - previous >= INTERVAL)
      {
        switch (flip)
        {
          case 0: lcd.setCursor(0, 0); lcd.print(F("|")); lcd.setCursor(15, 0); lcd.print(F("|")); break;
          case 1: lcd.setCursor(0, 0); lcd.print(F("/")); lcd.setCursor(15, 0); lcd.print(F("/")); break;
          case 2: lcd.setCursor(0, 0); lcd.print(F("-")); lcd.setCursor(15, 0); lcd.print(F("-")); break;
          case 3: lcd.setCursor(0, 0); lcd.print(F("\x01")); lcd.setCursor(15, 0); lcd.print(F("\x01")); break;
          case 4: lcd.setCursor(0, 0); lcd.print(F("|")); lcd.setCursor(15, 0); lcd.print(F("|")); break;
        }
        flip = ++flip % 5;
        previous += INTERVAL;
      }
    }
    {
      const unsigned long INTERVAL = 100;
      static unsigned long previous = 0;
      if (timestamp - previous >= INTERVAL)
      {
        lcd.setCursor(0, 1);
        lcd.print(float(timestamp - raceStartTime) / 1000.0f, 1);
        previous += INTERVAL;
      }
    }
  }
}

void BlinkLed()
{
  const unsigned long INTERVAL = 500;
  static unsigned long previous = 0;
  if (timestamp - previous >= INTERVAL)
  {
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
    previous += INTERVAL;
  }
}