#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <Bounce2.h>
#include <avr/wdt.h>



// ***************************************
// 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", "SET_RACE"};
//const int goRaceCOUNT = sizeof goRace / sizeof goRace[0];
int iGoRaceIndex = 0;
int iMode = 0;
int clk = digitalRead(ENCODER_CLK);     //clockwise)
bool setVariable = false;



// void nextMode() {
//   switch (mode) {
//     case SET_LAPS:
//       mode = SET_TIME;
//       break;

//     case SET_TIME:
//       mode = SET_RACE;
//       break;    
      
//       case SET_RACE:
//       mode = SET_LAPS;
//       break;
//   }
// }



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




//void updateValue(int delta) {
//   Serial.print("Mode = ");
//   Serial.print(mode[iMode]);
//   switch (iMode) {
//     case 0:
//       iNumLaps = constrain(iNumLaps + delta, 0, 99);
//       break;

//     case 1:
//       iTime = constrain(iTime + delta, 0, 99);
//       break;

//     case 2:
//       iGoRaceIndex = constrain(iGoRaceIndex + delta, 0, 1);
//       break;

  
//   }
// }






// 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 LCD
LiquidCrystal_I2C lcd(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 = " Slingshot2L v3b";




// 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 LCD
char line1Display [NUM_LANES][16];
char line2Display [NUM_LANES][16];







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

  Serial.begin(9600);
  //Initialise the LCD
  lcd.init();
  lcd.backlight();

  // Set the interrupts for lane sensors
  pinMode(LANE1_PIN, INPUT_PULLUP);
  pinMode(LANE2_PIN, INPUT_PULLUP);

  // 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);
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), readEncoder, FALLING);
  randomSeed(analogRead(0));
  
}







void readEncoder() {
  //int clk = digitalRead(ENCODER_CLK);     //clockwise
  Serial.println("readEncoder entered");
  
    //Serial.print("clk = ");
    //Serial.println(clk);

  //if(clk != prevClk && clk == LOW) {
    int dt = digitalRead(ENCODER_DT);     //anti-clockwise
    int delta = dt == HIGH ? 1 : -1;
    Serial.print("dt = ");
    Serial.println(dt);
    //updateValue(delta);

  switch (iMode) {
    case 0:
      iNumLaps = constrain(iNumLaps + delta, 0, 99);
      break;

    case 1:
      iTime = constrain(iTime + delta, 0, 99);
      break;

    case 2:
      iGoRaceIndex = constrain(iGoRaceIndex + delta, 0, 1);
      break;
  //}
  //updateDisplay();
  }
  //prevClk = clk;
  
}







void updateDisplay() {
  
  lcd.setCursor(1, 0);
  lcd.print("Laps:");
  if (iMode == 0) {
    lcd.setCursor(0, 0);
    lcd.print("*");
    lcd.setCursor(0, 1);
    lcd.print(" ");
    lcd.setCursor(9, 1);
    lcd.print(" ");
    lcd.setCursor(7, 0);
    lcd.print(iNumLaps);
    
  }

  lcd.setCursor(1, 1);
  lcd.print("Time:");
  lcd.setCursor(7, 1);
  lcd.print(iTime);
  if (iMode == 1) {
    lcd.setCursor(0, 1);
    lcd.print("*");
    lcd.setCursor(0, 0);
    lcd.print(" ");
    lcd.setCursor(9, 1);
    lcd.print(" ");

  }
   lcd.setCursor(10, 1);
   lcd.print("Race:");
   lcd.setCursor(15, 1);
  lcd.print(goRace[iGoRaceIndex]);
   if (iMode == 2) {
    lcd.setCursor(9, 1);
    lcd.print("*");
    lcd.setCursor(0, 0);
    lcd.print(" ");
    lcd.setCursor(0, 1);
    lcd.print(" ");
       
  }

}





/*
 * 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);
 
    if (clk != prevClk && clk == LOW && millis() - modeLastChanged > 300) {
    modeLastChanged = millis();
    
    int dtValue = digitalRead(ENCODER_DT);
    if (dtValue == HIGH) {
      iMode ++;
      }
      if (dtValue == LOW) {
      iMode --;
      }
      
      if(iMode > 2){
        iMode = 0;
      }
      if(iMode < 0) {
        iMode = 2;
      }
      updateDisplay();
    }
  prevClk = clk;
  } 


  if (digitalRead(ENCODER_SW) == LOW && millis() - modeLastChanged > 300) {         //button is pressed
  
    Serial.println("Encoder switch");
    if(setVariable) {
      setVariable=false;
    }
    else{
      setVariable=true;
    }  
  Serial.print("SetVariable = ");
  Serial.println(setVariable);
    int clk = digitalRead(ENCODER_CLK); 
    int dt = digitalRead(ENCODER_DT);
     
    if (clk != prevClk && clk == LOW && millis() - modeLastChanged > 300) {
    modeLastChanged = millis();
    
    if(!setVariable){
      

    int dtValue = digitalRead(ENCODER_DT);
    if (dtValue == HIGH) {
      iMode ++;
      }
      if (dtValue == LOW) {
      iMode --;
      }
      
      if(iMode > 2){
        iMode = 0;
      }
      if(iMode < 0) {
        iMode = 2;
      }
      
    }
    else{
      readEncoder();
    }
    updateDisplay();
  prevClk = clk;
    }
  modeLastChanged = millis();

  updateDisplay();
  }



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

  // Display start lights if we are starting
  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);
    }
  }
}












// ***************************************
// Handle the lane interupts
//
// This uses interrupts for lane signals since the 
// LCD 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 (tState.iLapCount[laneNo] == iNumLaps)
  {
    return;
  }  

  tState.lastLapTime[laneNo] = tNow - tState.tLastLaneSignalTime[laneNo];

  // If this lane has finished the race
  if (++tState.iLapCount[laneNo] == iNumLaps)
  {
    strcpy(line1Display[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], "Lap %3d ", tState.iLapCount[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]);
  }

  lcd.clear();

  lcd.print(line1Display[0]);
  lcd.print(line1Display[1]); 

  lcd.setCursor(0, 1);
  lcd.print(line2Display[0]);
  lcd.print(line2Display[1]);
}














// ***************************************
// 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(line2Display[laneNo], "%3d.%03d ", lapSecs, lapTime % 1000);
  }
  else
  {
    strcpy(line2Display[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);

  lcd.clear();
  lcd.print("   3 .");
  digitalWrite(iLightPin[0], HIGH);
  delay(1000);
  lcd.print(" 2 .");    
  digitalWrite(iLightPin[1], HIGH);
  delay(1000);
  lcd.print(" 1 .");
  digitalWrite(iLightPin[2], HIGH);
  delay(random(1000, 3000));
  lcd.clear();
  lcd.print("       GO");

  // 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();
  for (int i=0; i<NUM_LANES; i++)
  {
    tState.tLastLaneSignalTime[i] = tNow;
  }

  tState.needToDisplay = 0;
  tState.iRaceState++;
}












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

  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;
    strcpy(tRecs.sLanePos[i], "        ");

    strcpy(line1Display[i], "        ");
    strcpy(line2Display[i], "        ");
  }
  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)
{  
  lcd.clear();
  char * recordTitle = "";
  for (int i=0; i<NUM_LANES; i++)
  {
    switch (recordType)
    {
    case 1:
      recordTitle = "Position";
      strcpy(line2Display[i], tRecs.sLanePos[i]);
      break;
    case 2:
      recordTitle = "Total Race Time";
      formatLapTime(i, tRecs.tTotalRaceTime[i]);
      break;
    case 3:
      recordTitle = "Best Lap";
      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 Lap Time";
      formatLapTime(i, tRecs.tTotalRaceTime[i]/iNumLaps);
      break;
    case 6:
      recordTitle = "Reaction Time";
      formatLapTime(i, tRecs.tReactionTime[i]);
      break;
    }
  }  

  lcd.print(recordTitle); 

  lcd.setCursor(0, 1);
  lcd.print(line2Display[0]);
  lcd.print(line2Display[1]);  

  return(recordType);
}