#include <LiquidCrystal.h>

const int minAllowedLapTime = 2000;    // Minimum lap time (for debounce)

// Define pins
const int TRIGGER_PIN = 2;   // Pin that triggers new lap
const int BUTTON_LED = 3;    // LED for ready to start (in button)
const int BLU_LED = A0;
const int GRE_LED = A1;
const int RED_LED = A2;
const int LCD_BACKLIGHT_PIN = 6;
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);  // Set the LCD address to 0x27 for a 20 chars and 4 line display

unsigned long startTimeStamp;
unsigned long lapTimeStamp;
unsigned int lastLapTime;
unsigned int bestLapTime; 
unsigned int totalTime;
int lap = 0;
bool newBestLap = false; 
bool initializeRace = false; 
int lapTimeLedState = HIGH; 

void(* resetFunc) (void) = 0; // Function to reset aduino (when running for > 99 min)

void setup() {
  pinMode(TRIGGER_PIN, INPUT);
  pinMode(LCD_BACKLIGHT_PIN, OUTPUT);
  pinMode(BUTTON_LED, OUTPUT);
  pinMode(BLU_LED, OUTPUT);
  pinMode(GRE_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);
  digitalWrite(BLU_LED, HIGH);
  digitalWrite(GRE_LED, HIGH);
  digitalWrite(RED_LED, HIGH);
  
  attachInterrupt (digitalPinToInterrupt (TRIGGER_PIN), passGoal , HIGH);   // Interrupt for goalpost trigger

  // Initialize LCD display 
  lcd.begin(20, 4);
  digitalWrite(LCD_BACKLIGHT_PIN, HIGH);  // Turn on backlight 
  lcd.setCursor(2,0);
  lcd.print("Klart f");
  lcd.write(239);   // o with umlaut 
  lcd.print("r start!");
  lcd.setCursor(0,2);
  lcd.print("Tiden startar n");
  lcd.write(225);   // a with umlaut 
  lcd.print("r du");
  lcd.setCursor(0,3);
  lcd.print("passerar startlinjen");

  // Turn on green LED in button and RGB LED 
  digitalWrite(BUTTON_LED, HIGH);
  digitalWrite(GRE_LED, LOW);
}

void loop() {
  if (initializeRace) {   // Start race
    lcd.clear();
    initializeRace = false; 
    digitalWrite(BUTTON_LED, LOW);
    digitalWrite(GRE_LED, HIGH);

  }

  // Reset after about an hour and a half (can't display longer times)
  if (millis() - startTimeStamp > 5999999 ) { 
    resetFunc();
  }

  if (lap > 0) {  // If race has started 
    // Print total time
    lcd.setCursor(0,0);
    lcd.print("     Total: ");
    if (totalTime) {              // If there is a static time set last time we passed the goalpost, display that
      lcd.print(millisToTime(totalTime));
    } else {                      // If not, display running time (should only be during lap 1)
      lcd.print(millisToTime(millis() - startTimeStamp));
    }

    // Print running lap time for current lap
    // Place cursor depending on number of digits in lap 
    if (lap < 10) {
      lcd.setCursor(4,1);
    } else if (lap < 100 ) {
      lcd.setCursor(3,1);
    } else {
      lcd.setCursor(2,1);
    }
    lcd.print("Varv ");
    lcd.print(lap);
    lcd.print(": ");
    lcd.print(millisToTime(millis() - lapTimeStamp));

    // Print lap time for last complete lap 
    // Place cursor depending on number of digits in lap 
    if (lap - 1 < 10) {
      lcd.setCursor(4,2);
    } else if (lap - 1 < 100 ) {
      lcd.setCursor(3,2);
    } else {
      lcd.setCursor(2,2);
    }
    if (lap > 1) {    // If there's a previous lap to compare to
      lcd.print("Varv ");
      lcd.print(lap - 1);
      lcd.print(": ");
      lcd.print(millisToTime(lastLapTime));
    } else {
        lcd.print("                    ");
    }
    
    // Print best lap time and flash if new best  
    lcd.setCursor(0,3);
    // If we set a new best lap, flash the "Best lap" row and the LED for 5 seconds
    // 
    if (newBestLap && (millis() - lapTimeStamp < 5000) && (millis() / 50 % 10 < 5) ) {
        lcd.print("                    ");  // Blank out row
    } else {
      lcd.print(" B");
      lcd.write(225);   // a with umlaut 
      lcd.print("st varv: ");
      if (bestLapTime) {
        lcd.print(millisToTime(bestLapTime));
      } else {
        lcd.print("        ");    // If ther is no time, blank out the space
      }
      if (newBestLap) {
        lapTimeLedState = LOW;  // Turn LED on
      } else { 
        lapTimeLedState = HIGH;   // Turn LED off
      }

    }
    digitalWrite(BLU_LED, lapTimeLedState);  // set the LED with the ledState of the variable
    
  }

  delay(10);  // Update every 1/100 second, because that's the resolution we use to display time 
}

void passGoal () {    // Interrupt function when passing goalpost
    if (lap == 0) {        // If race isn't started, set all times to this timestamp
      startTimeStamp = millis();
      lapTimeStamp = startTimeStamp;
      initializeRace = true;  // Order a wipe of the LCD 
      lap++;
    }
  if (lap > 0 && millis() > lapTimeStamp + minAllowedLapTime) {   // Debounce 
    long goalPostTimeStamp = millis();    // Save the time we passed the goalpost 
    lastLapTime = goalPostTimeStamp - lapTimeStamp;   // Calculate the time for the last lap 
    totalTime = goalPostTimeStamp - startTimeStamp;   // Calculate the total time for the race so far
    
    if (bestLapTime == NULL) {     // If no previous lap, this is fastest but not "new best", because we don't have anything to compare to
      bestLapTime = lastLapTime; 
    } else if (lastLapTime < bestLapTime) {   // If this lap is faster than the current saved best lap
      bestLapTime = lastLapTime;              // Save this lap time as the best lap
      newBestLap = true;            // Say last lap was the fastest          
    } else {
      newBestLap = false;           // Say last lap wasn't the fastet
    }
    
    lapTimeStamp = goalPostTimeStamp;       // Save the time stamp to use next time
    lap++;    // Increase the lap number
  }
}



String millisToTime (long result) {   // Convert milliseconds to human-readable time
  result = result / 10;   // Convert to hundreds instead of milliseconds
  int minutes = result / 6000;    // Hundreds to minutes
  result = result % 6000;         // Remaining hundreds 
  int seconds = result / 100;    // Hundreds to seconds 
  int hundreds = result % 100;          // Remaining hundreds
  //lcd.print(formatDigits(minutes) + ":" + formatDigits(seconds) + "," + formatDigits(hundreds) );
  String formattedTime = formatDigits(minutes) + ":" + formatDigits(seconds) + "," + formatDigits(hundreds); 
  return formattedTime;
}

String formatDigits(int digits){  // Add leading 0 to values with fewer than 2 digits
  String returnValue = "";
  if(digits < 10)
    returnValue.concat('0');
  returnValue.concat(digits);
  return returnValue;
  
}