#include <Bounce2.h>
#include <avr/wdt.h>
#include <Wire.h> //I2C library
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>


#define SCREEN_WIDTH        128 // OLED display width, in pixels
#define SCREEN_HEIGHT        64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET            4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ***************************************
// This first section is stuff that can be easily changed
// ***************************************
// Defines the max lanes we are supporting 
#define NUM_LANES      2

// Uses 1 button for selection of options

#define BTN_SELECT  5       //Define the select button
#define ENCODER_CLK 6       //Define Clockwise rotation
#define ENCODER_DT  7       //Define anti-clockwise rotation
#define ENCODER_SW  8       //Define the button press



// typedef enum {
//   SET_LAPS,
//   SET_TIME,
//   SET_RACE,
//   } Mode;

//Mode mode = SET_LAPS;

long int modeLastChanged = 0;
int prevClk = HIGH;

// char * goRace[] = {"N", "Y"};
char * mode[] = {"SET_LAPS", "SET_TIME"};
//const int goRaceCOUNT = sizeof goRace / sizeof goRace[0];
// int iGoRaceIndex = 0;
int iMode = 0;
int clk = digitalRead(ENCODER_CLK);     //clockwise)
bool setVariable = false;
bool countdown = true;
unsigned long Countdown_start;



// Number of laps or time to race
int iNumLaps = 1;
int iTime = 5;



// Minimum lap time in ms
#define MIN_LAPTIME    1000

// Input Pins & intrupt(?) numbers for lane sensors
#define LANE1_PIN      2
#define LANE2_PIN      3
#define LANE1_INTR     0
#define LANE2_INTR     1


// Unique adress of the display
//LiquidCrystal_I2C display(0x27,16,2);



// Define the output pins to be used for start lights
const int iLightPin [] =            //could this be attached to the LED strips???
{
  A0,A1,A2
};







// ***************************************
// End of editable stuff
// ***************************************

// Name & Version
char *sVersion = "CRM RMS";




// Store all records to be displayed later
struct
{
  unsigned long tReactionTime [NUM_LANES];
  unsigned long tTotalRaceTime [NUM_LANES];
  unsigned long tBestLapTime [NUM_LANES];
  char sLanePos [NUM_LANES][9];
} 
tRecs;






// Store all race state info
struct 
{
  int iLastLaneSignal [NUM_LANES];
  unsigned long tLastLaneSignalTime [NUM_LANES];    //unsigned longa are positive big numbers
  unsigned long lastLapTime[NUM_LANES];
  int iRaceState;                         
  // Decides what state the system is in  
  // 0 - Initialiased  
  // 1 - Setup  
  // 2 - Starting  
  // 3 - Started  
  // 4 - Finished
  int iLastRecordDisplayed;               //keeps track of which screen we are on/information is being displayed??
  int iRecordToDisplay;                   //??
  int iLapCount [NUM_LANES];              // Number of laps run
  int iPosition;                          // Keep track of position and finish time
  unsigned long tFinishTime;
  int needToDisplay;
} 
tState;





int iPinValue = LOW;
// Number of stats screens
int iNumStatsToDisplay = 6;

// Lines to display on display
char line1Display [NUM_LANES][16];
char line2Display [NUM_LANES][16];
char line3Display [NUM_LANES][16];






/*
 * Setup everything we need
 */
void setup()
{

  // Serial.begin(9600); // Any baud rate should work
  // Serial.println("Hello Arduino\n");
  //Initialise the display
  //display.init();
  //display.backlight();
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  // Set the interrupts for lane sensors
  pinMode(LANE1_PIN, INPUT_PULLUP);
  pinMode(LANE2_PIN, INPUT_PULLUP);
Serial.println("text");


  // Set up the outputs for start lights
  for (int i=0; i<sizeof(iLightPin); i++)
  {
    pinMode(iLightPin[i], OUTPUT);
  }

  // Initialize encoder pins
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  pinMode(BTN_SELECT, INPUT_PULLUP);
  //attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), targetMenu, FALLING);
  //attachInterrupt(digitalPinToInterrupt(BTN_SELECT), doRaceStart, FALLING);
  randomSeed(analogRead(0));
  
}


void topMenu(){
  int dt = digitalRead(ENCODER_DT);     //anti-clockwise
  if (dt == HIGH) {
    iMode ++;
  }else{
    iMode --; 
  }
  if(iMode > 1){
    iMode = 0;
  }
  if(iMode < 0) {
    iMode = 1;
  }
  updateDisplay();
}




void targetMenu() {
  
  int dt = digitalRead(ENCODER_DT);     //anti-clockwise
  int delta = dt == HIGH ? 1 : -1;
    switch (iMode) {
    case 0:
      iNumLaps = iNumLaps + delta;
      if(iNumLaps > 99){iNumLaps = 0;}
      if(iNumLaps < 0){iNumLaps = 99;}

      iTime = 0;
      break;
    case 1:
      iTime = iTime + delta;
      if(iTime > 99){iTime = 0;}
      if(iTime < 0){iTime = 99;}

      iNumLaps = 0;
      break;
    // case 2:
    //   iGoRaceIndex = iGoRaceIndex + delta;
    //   if(iGoRaceIndex > 1){iGoRaceIndex = 0;}
    //   if(iGoRaceIndex < 0){iGoRaceIndex = 1;}
    //   break;
  }
  updateDisplay();
}







void updateDisplay() {
  
  display.setCursor(50, 20);  
  display.setTextColor(WHITE);      //display laps settings
  display.print("Laps:");
  if (iMode == 0){
    if(setVariable){
      display.fillRect(20,20,20,10,BLACK);
      display.setCursor(20, 20);    //if in edit mode, add another asterisk
      display.print("**");
    }
    else{
      display.fillRect(20,20,20,10,BLACK);
      display.setCursor(20, 20);    //else we are in the top menu
      display.print(" *");
    }
    display.fillRect(20,50,25,10,BLACK);
    display.fillRect(80,20,20,10,BLACK);
    display.setCursor(80, 20);
    display.print(iNumLaps);
    display.fillRect(80,50,20,10,BLACK);
    display.setCursor(80, 50);
    display.print(iTime);   
  }

  display.setCursor(50, 50);
  display.print("Time:");
  if (iMode == 1) {
      if(setVariable){
      display.fillRect(20,50,20,10,BLACK);
      display.setCursor(20, 50);
      display.print("**");
    }
    else{
      display.fillRect(20,50,20,10,BLACK);
      display.setCursor(20, 50);
      display.print(" *");
    }
    display.fillRect(20,20,20,10,BLACK);
    display.fillRect(80,20,20,10,BLACK);
    display.setCursor(80, 20);
    display.print(iNumLaps);
    display.fillRect(80,50,20,10,BLACK);
    display.setCursor(80, 50);
    display.print(iTime);  

  }

display.display();
}





/*
 * Main loop will loop around reading any input pins
 */
void loop()
{
//   int clk = digitalRead(ENCODER_CLK); 
//   int dt = digitalRead(ENCODER_DT);
//   int sw = digitalRead(ENCODER_SW);
//   int btn = digitalRead(BTN_SELECT);


//   if (digitalRead(ENCODER_SW) == LOW && millis() - modeLastChanged > 300) {
//     modeLastChanged = millis();
//     setVariable = !setVariable;
    
//     updateDisplay();
//   }

//   if ((clk == LOW || dt == LOW) && millis() - modeLastChanged > 300){
//     modeLastChanged = millis();
//     if (!setVariable) {
//       topMenu();
//     }else{
//       targetMenu();
//     }
//   }
  
//   if (digitalRead(BTN_SELECT) == LOW && millis() - modeLastChanged > 300) {       //the button has been pressed, BTN_SELECT = LOW
//     modeLastChanged = millis();
//     tState.iRaceState = 2;
   

//   }
//   // Initialise everything at the start
//   if (tState.iRaceState == 0)
//   {
//     doInit();
//   }


//   if (tState.iRaceState == 2)
//   {
//     doRaceStart();
//   }

//   // Read each of the lanes in turn only if we have started
//   if (tState.iRaceState == 3)
//   {
//     // Check for anything to display
//     if (tState.needToDisplay > 0)
//     {
//       displayRaceInfo();
//       tState.needToDisplay--;

//       // Check for race over
//       isRaceOver();
//     }
//   }

//   // Race finished display stats
//   if (tState.iRaceState == 4)
//   {
//     if (tState.iRecordToDisplay != tState.iLastRecordDisplayed)
//     {
//       tState.iLastRecordDisplayed = displayRaceRecords(tState.iRecordToDisplay);
//     }
//   }
// }

  if(iTime*60000 >(millis()-Countdown_start)){
    display.clearDisplay();
    display.setCursor(5,0);
    display.setTextSize(2);            
    display.setTextColor(SSD1306_WHITE);
    display.println(timeLeft((iTime*60000-(millis()-Countdown_start)))); 
    display.display();
  } 
}









// ***************************************
// Handle the lane interupts
//
// This uses interrupts for lane signals since the 
// display write stuff is so slow i.e. 30ms we'd miss lane signals
//
// ***************************************
void processLaneIntr()
{
  if (tState.iRaceState != 3)
  {
    return;
  }
  
  int iPinValue = LOW;
  iPinValue = digitalRead(LANE1_PIN);
  if (iPinValue != tState.iLastLaneSignal[0])
  {
    tState.iLastLaneSignal[0] = iPinValue;
    processLane(0);
  }
  iPinValue = digitalRead(LANE2_PIN);
  if (iPinValue != tState.iLastLaneSignal[1])
  {
    tState.iLastLaneSignal[1] = iPinValue;
    processLane(1);
  }
}













// ***************************************
// Process any lane signals
// laneNo - The lane to send to RC
// ***************************************
void processLane(int laneNo)
{
  unsigned long tNow = millis();

  // Don't process it was too quick
  if ((tNow - tState.tLastLaneSignalTime[laneNo] < MIN_LAPTIME) && tState.iLapCount[laneNo] != -1)
  {
    return;
  }

  // If this lane has finished then don't log anything else
  if(countdown) {
    if (tState.iLapCount[laneNo] == iNumLaps)
    {
      return;
    }  
  }
  else{
    if (iTime*60000 <=(millis()-Countdown_start)){
      return;
    }
  }
  tState.lastLapTime[laneNo] = tNow - tState.tLastLaneSignalTime[laneNo];

  // If this lane has finished the race
  if(countdown) {
    if (++tState.iLapCount[laneNo] == iNumLaps)
    {
      sprintf(line1Display[laneNo], "%d ", (iNumLaps-tState.iLapCount[laneNo]));  
      strcpy(line2Display[laneNo], "Finish  ");
      
      // Set the position
      sprintf(tRecs.sLanePos[laneNo], "%d%s ", ++tState.iPosition, tState.iPosition == 1 ? "nd" : "st");
      if (tState.iPosition == 1)
      {
        tState.tFinishTime = tRecs.tTotalRaceTime[laneNo] + tState.lastLapTime[laneNo];
      }
    }
    else
    {
    
      sprintf(line1Display[laneNo], "%d ", (iNumLaps-tState.iLapCount[laneNo]));  
      sprintf(line2Display[laneNo], "%d ", tState.iLapCount[laneNo]);
    }
  }
  else{
  // If this lane has finished the race
    if (iTime*60000 > (millis()-Countdown_start)){
      display.setCursor(55, 0);
      display.print(timeLeft(iTime*60000-(millis()-Countdown_start)));  
      display.display();
      sprintf(line2Display[laneNo], "%d ", tState.iLapCount[laneNo]);
    }
    else
    {
      sprintf(line1Display[laneNo], "%3d.%03d ", timeLeft(iTime*60000-(millis()-Countdown_start)));
      strcpy(line2Display[laneNo], "Finish  ");
      
      // Set the position
      sprintf(tRecs.sLanePos[laneNo], "%d%s ", ++tState.iPosition, tState.iPosition == 1 ? "nd" : "st");
      if (tState.iPosition == 1)
      {
        tState.tFinishTime = tRecs.tTotalRaceTime[laneNo] + tState.lastLapTime[laneNo];
      }
    }
  }
  // Set the totals
  if ((tState.lastLapTime[laneNo] < tRecs.tBestLapTime[laneNo]) && (tState.iLapCount[laneNo] > 0))
  {
    tRecs.tBestLapTime[laneNo] = tState.lastLapTime[laneNo];
  }

  // Save reaction time
  if (tState.iLapCount[laneNo] == 0)
  {
    tRecs.tReactionTime[laneNo] = tState.lastLapTime[laneNo];
  }

  tRecs.tTotalRaceTime[laneNo] += tState.lastLapTime[laneNo];
  tState.tLastLaneSignalTime[laneNo] = tNow;
  tState.needToDisplay++;
}











// ***************************************
// Display the race info if needed
// ***************************************
void displayRaceInfo()
{
  // set the laptime for display on line 2
  // Clear the display and reprint all details
  for (int i=0; i<NUM_LANES; i++)
  {
    formatLapTime(i, tState.lastLapTime[i]);
  }

  display.clearDisplay();

  //Line 1 Info
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("Remains");
  display.setCursor(55, 0);
  display.print(line1Display[0]);   //Lane 0
  display.setCursor(98, 0);
  display.print(line1Display[1]);   //Lane 1


  //Line 2 Info
  display.setTextSize(1);
  display.setCursor(0, 20);
  display.print("Curr Lap");
  display.setCursor(55, 20);
  display.print(line2Display[0]);   //Lane 0
  display.setCursor(98, 20);
  display.print(line2Display[1]);   //Lane 1


  //Line3 Info
  display.setCursor(0, 40);
  display.print("Last Lap");
  display.setCursor(42, 40);
  display.print(line3Display[0]);   //Lane 0
  display.setCursor(85, 40);
  display.print(line3Display[1]);   //Lane 1

  display.display();
}














// ***************************************
// Format a lap time that is in millis()
// and return a formatted string.
// ***************************************
void formatLapTime(int laneNo, unsigned long lapTime)
{
  // Only display upto 999 seconds
  int lapSecs = (lapTime / 1000) > 999 ? 999 : lapTime / 1000;

  // Times of 99999999 are just init values and don't need displaying 
  if (lapTime != 99999999 && lapTime != 0)
  {
    sprintf(line3Display[laneNo], "%3d.%03d ", lapSecs, lapTime % 1000);
  }
  else
  {
    strcpy(line3Display[laneNo], "        ");
  }
}











// ***************************************
// Process select button presses, increments state
// ***************************************
void processSelect()
{
  if (++tState.iRaceState >= 5)
  {
    tState.iRaceState = 0;
  }
}









// ***************************************
// Process advance button presses, action depends on state
// ***************************************
void processAdvance()
{


  // Race state 4 lets us select the records to display
  if (tState.iRaceState == 4)
  {
    // Which record should we display?
    if (++tState.iRecordToDisplay > iNumStatsToDisplay)
      tState.iRecordToDisplay = 1;
  }
}












// ***************************************
// Do the start sequence
// ***************************************
void doRaceStart()
{
    // Read the pin values to start
  tState.iLastLaneSignal[0] = digitalRead(LANE1_PIN);
  tState.iLastLaneSignal[1] = digitalRead(LANE2_PIN);

  display.clearDisplay();
 
  display.setCursor(20, 20);    //if in edit mode, add another asterisk
  // display.print("Drivers, ready..");
  // display.display();
  // delay(1000);
  // display.clearDisplay();
  // display.setCursor(20, 20);
  // display.print("3...");
  // display.display();
  // digitalWrite(iLightPin[0], HIGH);
  // delay(1000);
  // display.setCursor(50, 20);
  // display.print("2...");    
  // display.display();
  // digitalWrite(iLightPin[1], HIGH);
  // delay(1000);
  // display.setCursor(80, 20);
  // display.print("1...");
  // display.display();
  // digitalWrite(iLightPin[2], HIGH);
  // delay(random(1000, 2000));
  // display.clearDisplay();
  // display.setCursor(40, 20);
  display.setTextSize(5);
  display.print("GO");
  display.display();
  // Enable lane interrupts
  attachInterrupt(LANE1_INTR, processLaneIntr, CHANGE);
  attachInterrupt(LANE2_INTR, processLaneIntr, CHANGE);

  // On Go then put all lights out
  digitalWrite(iLightPin[0], LOW);
  digitalWrite(iLightPin[1], LOW);
  digitalWrite(iLightPin[2], LOW);

  // Once start complete set the start time 
  unsigned long tNow = millis();
  Countdown_start = tNow;
  for (int i=0; i<NUM_LANES; i++)
  {
    tState.tLastLaneSignalTime[i] = tNow;
  }
  if (iNumLaps>iTime){
    countdown = true;
  }
  else{
    countdown = false;
  }
  tState.needToDisplay = 0;
  tState.iRaceState++;
}


String timeLeft(unsigned long MsLeft){
  String Result;
  int M;
  int S;
  M=(long)MsLeft/60000;

  if (M<10) Result=(String)"0"+ M + ":";else Result=(String)M+":";
  S=(long)((MsLeft-M*60000)/1000);
  if (S<10) Result=(String)Result + "0"+ S ;else Result=(String)Result +S;
  return Result;
  
  
}










// ***************************************
// Initialise everything that needs resetting before the race
// ***************************************
void doInit()
{
  display.clearDisplay();
  // display.print(sVersion);
  // delay(1000);
  

  updateDisplay();

  for (int i=0; i<NUM_LANES; i++)
  {
    // -1 assumes that sensor is after start so won't count first crossong of sensor as a lap
    // setting to 0 will assume start is after sensor.
    tState.iLapCount[i] = -1;
    tState.lastLapTime[i] = 0;

    // Initialise the records
    tRecs.tBestLapTime[i] = 99999999;
    tRecs.tTotalRaceTime[i] = 0;
    tRecs.tReactionTime[i] = 0;

  }
  tState.iRecordToDisplay = 1;
  tState.iLastRecordDisplayed = 0;
  tState.iPosition = 0;

  tState.iRaceState++;          //moves iRaceState from 0 to 1
  
}














// ***************************************
// Check if the race is over
// ***************************************
void isRaceOver()
{
  // Race is over if all lanes are on iNumLaps or 1 lane 
  // has completed and all others <0 (i.e. hasnt started yet)
  int iLanesComplete = 0;
  boolean bOneLaneComplete = false;
  for (int i=0; i<NUM_LANES; i++)
  {
    if (tState.iLapCount[i] == iNumLaps)
    {
      iLanesComplete++;
      bOneLaneComplete = true;      
    }
    if (tState.iLapCount[i] < 0)
    {
      iLanesComplete++;
    }
  }

  // Advance race state if all lanes are finished
  if ((iLanesComplete == NUM_LANES) && (bOneLaneComplete))
  {
    tState.iRaceState++;
    detachInterrupt(0);
    detachInterrupt(1);
  }
}

// ***************************************
// Display the race Record
// ***************************************
int displayRaceRecords(int recordType)
{  
  display.clearDisplay();
  char * recordTitle = "";
  for (int i=0; i<NUM_LANES; i++)
  {
    switch (recordType)
    {
    case 1:
      recordTitle = "Position";
      strcpy(line3Display[i], tRecs.sLanePos[i]);
      break;
      
    case 2:
      recordTitle = "Total";
      formatLapTime(i, tRecs.tTotalRaceTime[i]);
      break;
    case 3:
      recordTitle = "Best";
      formatLapTime(i, tRecs.tBestLapTime[i]);
      break;
    case 4:
      recordTitle = "Gap";
      formatLapTime(i, tRecs.tTotalRaceTime[i] > 0 ? tRecs.tTotalRaceTime[i] - tState.tFinishTime : 0);
      break;
    case 5:
      recordTitle = "Average";
      formatLapTime(i, tRecs.tTotalRaceTime[i]/iNumLaps);
      break;
    case 6:
      recordTitle = "Reaction";
      formatLapTime(i, tRecs.tReactionTime[i]);
      break;
    }
  }  
  delay(1000);
  display.fillRect(20,5,30,20,BLACK);
  display.setCursor(20, 5);
  display.print(recordTitle); 
  display.setTextSize(1);
  display.fillRect(20,35,30,20,BLACK);
  display.setCursor(20, 35);
  display.print(line3Display[0]);
  display.fillRect(80,35,30,20,BLACK);
  display.setCursor(80, 35);
  display.print(line3Display[1]); 
  display.display();

  return(recordType);
}