//includes


#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include "SPI.h"
#include "TFT_eSPI.h"



//defines

#define TFT_DC 2
#define TFT_CS 15
#define BTN_SELECT  5       //Define the select button
#define ENCODER_CLK 27       //Define Clockwise rotation
#define ENCODER_DT  14       //Define anti-clockwise rotation
#define ENCODER_SW  12       //Define the button press
#define NUM_LANES      2
#define MIN_LAPTIME    1000 // Minimum lap time in ms
#define LANE1_PIN      25   // Input Pins & intrupt(?) numbers for lane sensors
#define LANE2_PIN      26

//Adafruit works in simulation
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

//eSPI works in practice
//TFT_eSPI tft = TFT_eSPI();

//variables

long int modeLastChanged = 0;
char * mode[] = {"SET_LAPS", "SET_TIME"};
int iMode = 0;
//int clk = digitalRead(ENCODER_CLK);     //clockwise)
bool setVariable = false;
bool countdown = true;
unsigned long Countdown_start;
int iNumLaps = 1;
int iTime = 5;
char *sVersion = "CRM RMS";
int iPinValue = LOW;
int iNumStatsToDisplay = 6;   // Number of stats screens
char line1Display [NUM_LANES][16];    // Lines to display on display
char line2Display [NUM_LANES][16];
char line3Display [NUM_LANES][16];
volatile bool sensorTriggered = false;
volatile bool rotaryEncoder = false;       // Flag from interrupt routine (moved=true)

//structs
// 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;

//Start of Methods
//************************


// Interrupt routine just sets a flag when rotation is detected
void IRAM_ATTR sensor()
{
    sensorTriggered = true;
}

void IRAM_ATTR rotary() // Interrupt routine just sets a flag when rotation is detected
{
    rotaryEncoder = true;
}

int8_t checkRotaryEncoder()
{
    // Reset the flag that brought us here (from ISR)
    rotaryEncoder = false;

    static uint8_t lrmem = 3;
    static int lrsum = 0;
    static int8_t TRANS[] = {0, -1, 1, 14, 1, 0, 14, -1, -1, 14, 0, 1, 14, 1, -1, 0};

    // Read BOTH pin states to deterimine validity of rotation (ie not just switch bounce)
    int8_t l = digitalRead(ENCODER_DT);
    int8_t r = digitalRead(ENCODER_CLK);

    // Move previous value 2 bits to the left and add in our new values
    lrmem = ((lrmem & 0x03) << 2) + 2 * l + r;

    // Convert the bit pattern to a movement indicator (14 = impossible, ie switch bounce)
    lrsum += TRANS[lrmem];

        /* encoder not in the neutral (detent) state */
    if (lrsum % 4 != 0)
    {
        return 0;
    }

    /* encoder in the neutral state - clockwise rotation*/
    if (lrsum == 4)
    {
        lrsum = 0;
        return 1;
    }

    /* encoder in the neutral state - anti-clockwise rotation*/
    if (lrsum == -4)
    {
        lrsum = 0;
        return -1;
    }

    // An impossible rotation has been detected - ignore the movement
    lrsum = 0;
    return 0;
}

void setup() {

  Serial.begin(9600); 

  //screen stuff
  tft.begin();
  tft.setRotation(1);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);



  // Set the interrupts for lane sensors
  pinMode(LANE1_PIN, INPUT);
  pinMode(LANE2_PIN, INPUT);
  attachInterrupt(LANE1_PIN, sensor, CHANGE);
  attachInterrupt(LANE2_PIN, sensor, CHANGE);

  // Initialize encoder pins
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  // We need to monitor both pins, rising and falling for all states
  attachInterrupt(ENCODER_DT, rotary, CHANGE);
  attachInterrupt(ENCODER_CLK, rotary, CHANGE);

  pinMode(BTN_SELECT, INPUT_PULLUP);
  
  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() {
  
  tft.setCursor(50, 20);  
  tft.setTextColor(ILI9341_WHITE);      //display laps settings
  tft.print("Laps:");
  if (iMode == 0){                              //Laps Mode
    if(setVariable){                            //edit mode
      tft.fillRect(20,20,20,20,ILI9341_BLACK);  //erase the single asterisk against Laps
      tft.setCursor(20, 20);                    //if in edit mode, add another asterisk against Laps
      tft.print("**");                          //write a double asterisk
    }
    else{                                       //else we are in the top menu
      tft.fillRect(20,20,20,20,ILI9341_BLACK);  //erase the whatever is there
      tft.setCursor(20, 20);                    //set the cursor for the single asterisk
      tft.print(" *");
    }
    tft.fillRect(20,50,25,20,ILI9341_BLACK);    
    tft.fillRect(120,20,25,20,ILI9341_BLACK);
    tft.setCursor(120, 20);
    tft.print(iNumLaps);
    tft.fillRect(120,50,25,20,ILI9341_BLACK);
    tft.setCursor(120, 50);
    tft.print(iTime);   
  }

  tft.setCursor(50, 50);
  tft.print("Time:");
  if (iMode == 1) {                             //Timer mode
      if(setVariable){                          //edit mode
      tft.fillRect(20,50,20,20,ILI9341_BLACK);
      tft.setCursor(20, 50);
      tft.print("**");
    }
    else{                                      //else we are in the top menu
      tft.fillRect(20,50,20,20,ILI9341_BLACK);
      tft.setCursor(20, 50);
      tft.print(" *");
    }
    tft.fillRect(20,20,25,20,ILI9341_BLACK);
    tft.fillRect(120,20,25,20,ILI9341_BLACK);
    tft.setCursor(120, 20);
    tft.print(iNumLaps);
    tft.fillRect(120,50,25,20,ILI9341_BLACK);
    tft.setCursor(120, 50);
    tft.print(iTime);  

  }


}



void loop()
{



  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)
  {

    if(sensorTriggered)
    {
    processLaneIntr();
    sensorTriggered = false;
    }
    
    // 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)){
  //   tft.fillScreen(ILI9341_BLACK);
  //   tft.setCursor(5,0);
  //   tft.setTextSize(2);            
  //   tft.setTextColor(ILI9341_WHITE);
  //   tft.println(timeLeft((iTime*60000-(millis()-Countdown_start)))); 
    
  // } 
// }





// ***************************************
// 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 (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 ? "st" : "nd");
    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]);
  }

  tft.fillScreen(ILI9341_BLACK);

  //Line 1 Info
  tft.setTextSize(2);
  tft.setCursor(5, 20);
  tft.print("Remains");
  tft.setCursor(100, 20);
  tft.print(line1Display[0]);   //Lane 0
  tft.setCursor(200, 20);
  tft.print(line1Display[1]);   //Lane 1


  //Line 2 Info
  tft.setTextSize(2);
  tft.setCursor(5, 100);
  tft.print("Curr Lap");
  tft.setCursor(100, 100);
  tft.print(line2Display[0]);   //Lane 0
  tft.setCursor(200, 100);
  tft.print(line2Display[1]);   //Lane 1


  //Line3 Info
  tft.setTextSize(2);
  tft.setCursor(5, 180);
  tft.print("Last Lap");
  tft.setCursor(100, 180);
  tft.print(line3Display[0]);   //Lane 0
  tft.setCursor(200, 180);
  tft.print(line3Display[1]);   //Lane 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(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;
  }
}





// ***************************************
// Initialise everything that needs resetting before the race
// ***************************************
void doInit()
{
  tft.fillScreen(ILI9341_BLACK);
  // tft.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
  
}






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

  tft.fillScreen(ILI9341_BLACK);
 
  // tft.setCursor(30, 20);    //if in edit mode, add another asterisk
  // tft.print("Drivers, ready..");
 
  // delay(1000);
  // tft.fillScreen(ILI9341_BLACK);
  // tft.setCursor(50, 120);
  // tft.print("3...");

  // // digitalWrite(iLightPin[0], HIGH);
  // delay(1000);
  // tft.setCursor(150, 120);
  // tft.print("2...");    

  // // digitalWrite(iLightPin[1], HIGH);
  // delay(1000);
  // tft.setCursor(250, 120);
  // tft.print("1...");

  // // digitalWrite(iLightPin[2], HIGH);
  // delay(random(1000, 2000));
  // tft.fillScreen(ILI9341_BLACK);
  tft.setCursor(130, 100);
  tft.setTextSize(6);
  tft.print("GO");
  
  // Enable lane interrupts
  // attachInterrupt(LANE1_PIN, processLaneIntr, CHANGE);
  // attachInterrupt(LANE2_PIN, 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;
  
  
}









// ***************************************
// 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)
{  
  tft.fillScreen(ILI9341_BLACK);
  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);
  tft.fillRect(20,5,30,20,ILI9341_BLACK);
  tft.setCursor(20, 5);
  tft.print(recordTitle); 
  tft.setTextSize(2);
  tft.fillRect(20,35,30,20,ILI9341_BLACK);
  tft.setCursor(100, 35);
  tft.print(line3Display[0]);
  tft.fillRect(80,35,30,20,ILI9341_BLACK);
  tft.setCursor(200, 35);
  tft.print(line3Display[1]); 
  

  return(recordType);
}