//Libraries
#include <TinyGPS++.h>
#include <LiquidCrystal.h>
#include <Chrono.h>
#include <Keyboard.h>
#include <Keypad.h>
//---------------------------------------------------------------

//Keypad
const byte numRows= 4; //number of rows on the keypad
const byte numCols= 4; //number of columns on the keypad

char keymap[numRows][numCols]=
{
{'1', '2', '3', 'A'},
{'4', '5', '6', 'B'},
{'7', '8', '9', 'C'},
{'*', '0', '#', 'D'}
};

byte rowPins[numRows] = {23,25,27,29}; //Rows 0 to 3
byte colPins[numCols]= {31,33,35,37}; //Columns 0 to 3


Keypad myKeypad= Keypad(makeKeymap(keymap), rowPins, colPins, numRows, numCols);
//---------------------------------------------------------------

//GPS Shield
static const uint32_t GPSBaud = 9600;
//---------------------------------------------------------------

// The TinyGPS++ object
TinyGPSPlus gps;
//---------------------------------------------------------------

//LCD
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
LiquidCrystal lcd2(10, 9, 26, 28, 30, 32);
//---------------------------------------------------------------

//Chrono
Chrono myChrono1(Chrono::MICROS); 
//----------------------------------------------------------------

//Variables
double prevLatitude, prevLongitude = 0;
double totalM = 0;
double trip2 = 0;
int start = 0; 
int fastart = 0; //To go to the second menu, without scrolling text (and give accurate start)
int scr = 0; //To avoid blinking screen in fastart menu
int tr2 = 0;
int menu =0;
double cons = 0; //Target speed in KpH
//----------------------------------------------------------------

//SETUP
void setup()
{

  Serial1.begin(GPSBaud);
  
  myChrono1.start();

  lcd.begin(16, 2);
  lcd2.begin(16, 2);
  lcd.setCursor(0, 0);
  lcd2.setCursor(0, 0);
  
  pinMode(40, OUTPUT);
  pinMode(42, OUTPUT);
  pinMode(44, OUTPUT);
  pinMode(46, OUTPUT);
  pinMode(48, OUTPUT);
  pinMode(50, OUTPUT);
  pinMode(52, OUTPUT);

  lcd.print(" # RallyTimer #");
  lcd.setCursor(0,1);
  lcd.print("  >----------<");

  for (int df = 0; df < 10; df++)
    {
      lcd2.print("* * * * * * * * * * * * * * * *");
      lcd2.setCursor(0,1);
      lcd2.print(" * * * * * * * * * * * * * * *");
      lcd2.scrollDisplayLeft(); 
      delay(500);
    }
  
  lcd.clear();
  lcd2.clear();
}
//----------------------------------------------------------------------

//Main Loop
void loop()
{

   double elapsed = myChrono1.elapsed()/1000000;  
   double disthr;
   double delta;
   char keypressed = myKeypad.getKey();

   //Menu principal
   if (menu == 0)
    {
      lcd.setCursor(0, 0);
      lcd2.setCursor(0, 0);
      lcd.print("TIMER : ");
      lcd.print(cons,0);
      lcd.print(" KpH");
      lcd.setCursor(0,1);
      lcd.print("Start Line ~ D");  // le "~" donne une flèche à l'écran

      lcd2.print("||1 |2 |3 |4 |5 |6 |7 |8 |9 || SAT : ");
      lcd2.print(gps.satellites.value());
      lcd2.setCursor(0,1);
      lcd2.print("||40|45|50|55|60|65|70|75|80|| ---   ");
      lcd2.scrollDisplayLeft(); 
      delay(500);


      if (keypressed == '1')
         {cons=40;
         lcd.clear();}
      else if (keypressed == '2')
         {cons=45;
         lcd.clear();}
      else if (keypressed == '3')
        {cons=50;    
        lcd.clear();}
      else if (keypressed == '4')
        {cons=55;    
        lcd.clear();}
      else if (keypressed == '5')
       {cons=60;    
       lcd.clear();}
      else if (keypressed == '6')
       {cons=65;    
       lcd.clear();}
      else if (keypressed == '7')
       {cons=70;    
       lcd.clear();}
      else if (keypressed == '8')
       {cons=75;    
       lcd.clear();}
      else if (keypressed == '9')
       {cons=80;    
       lcd.clear();}

     scr = 0;
    }
  //------------------------------------------------------
  
  //Fastart Menu
  if (keypressed =='D' && menu == 0 || fastart == 1)
   {
     menu = 1;
     fastart = 1;
     
      if (scr == 0)   // to avoid blinking screen
         {lcd.clear();
         lcd2.clear();
         scr = 1;}
         
     lcd.setCursor(0, 0);
     lcd2.setCursor(0, 0);
     lcd.print("TIMER : ");
     lcd.print(cons,0);
     lcd.print(" KpH");
     lcd.setCursor(0,1);
     lcd.print("*START  -  MENU#");
     
     if (keypressed == '#')
        {fastart = 0;
         menu = 0;
         lcd.clear();
         lcd2.clear();}
    }
  //------------------------------------------------------
  
  //Functioning
   if (keypressed == '*' && fastart == 1 || start == 1)
    {
     fastart = 0;
     if (start == 0)
        {myChrono1.restart();
         lcd.clear();
         lcd2.clear();}

     start = 1;
     menu = 1;

     //Average distance calculation
     disthr =(cons/3.6)*elapsed;
     //------------
     
     lcd.setCursor(0, 0);
     lcd2.setCursor(0, 0);

    //Travelled distance and delay calculation
     double distanceInM = getDistance(gps);
     totalM = totalM + distanceInM;
     lcd.print("Tot : ");
     lcd.print(totalM/1000,2);
     lcd.print(" Km");
     delta = disthr - totalM;
     //-----------

    lcd2.print("Spd : ");
    lcd2.print(gps.speed.kmph(),1);
    lcd2.print(" KpH");
    
    //Control of the Intermediate Trip Meter
    if (keypressed == 'A')  // Intermediate Trip Start
      {tr2 = 1;}

    if (keypressed == 'B')
      {tr2 = 0;}           // Intermediate Trip Pause (without reset)

    if (keypressed == 'C')  // Intermediate Trip Stop (with reset)
      {tr2 = 0;
      trip2 = 0;}

    //Intermediate Trip
    if (tr2 == 1) 
      {trip2 = trip2 + distanceInM;}
    //------------

    lcd2.setCursor(0,1);
    lcd2.print("Trp : ");
    lcd2.print(trip2/1000,2);
    lcd2.print (" Km");

    //Control of the Led's
    if (delta > -15 && delta < 15)
      {digitalWrite(40, LOW);
      digitalWrite(42, LOW);
      digitalWrite(44,LOW);
      digitalWrite(46,HIGH);
      digitalWrite(48,LOW);
      digitalWrite(50,LOW);
      digitalWrite(52,LOW);}

    else if (delta < -16 && delta > -40)
      {digitalWrite(40, LOW);
      digitalWrite(42, LOW);
      digitalWrite(44,LOW);
      digitalWrite(46,HIGH);
      digitalWrite(48,HIGH);
      digitalWrite(50,LOW);
      digitalWrite(52,LOW);}

    else if (delta > -100 && delta < -41 )
      {digitalWrite(40, LOW);
      digitalWrite(42, LOW);
      digitalWrite(44,LOW);
      digitalWrite(46,HIGH);
      digitalWrite(48,HIGH);
      digitalWrite(50,HIGH);
      digitalWrite(52,LOW);}

    else if (delta < -101)
      {digitalWrite(40, LOW);
      digitalWrite(42, LOW);
      digitalWrite(44,LOW);
      digitalWrite(46,HIGH);
      digitalWrite(48,HIGH);
      digitalWrite(50,HIGH);
      digitalWrite(52,HIGH);}


    else if ( delta > 16 && delta < 40)
      {digitalWrite(40, LOW);
      digitalWrite(42, LOW);
      digitalWrite(44,HIGH);
      digitalWrite(46,HIGH);
      digitalWrite(48,LOW);
      digitalWrite(50,LOW);
      digitalWrite(52,LOW);}

    else if ( delta > 41 && delta < 100)
      {digitalWrite(40, LOW);
      digitalWrite(42, HIGH);
      digitalWrite(44,HIGH);
      digitalWrite(46,HIGH);
      digitalWrite(48,LOW);
      digitalWrite(50,LOW);
      digitalWrite(52,LOW);}

    else if ( delta > 101)
      {digitalWrite(40, HIGH);
      digitalWrite(42, HIGH);
      digitalWrite(44,HIGH);
      digitalWrite(46,HIGH);
      digitalWrite(48,LOW);
      digitalWrite(50,LOW);
      digitalWrite(52,LOW);}
    //-------------

    lcd.setCursor(0, 1);
    lcd.print ("Delay : ");
    lcd.print(delta,0);
    lcd.print(" m  ");

    //Stop and return to main menu
    if (keypressed == '#')
      {start = 0;
      totalM=0;
      menu=0;
      digitalWrite(40, LOW);
      digitalWrite(42, LOW);
      digitalWrite(44,LOW);
      digitalWrite(46,LOW);
      digitalWrite(48,LOW);
      digitalWrite(50,LOW);
      digitalWrite(52,LOW);
      lcd2.clear();
      lcd.clear();
      prevLongitude = 0;
      prevLatitude = 0;} 
      } 
      
    smartDelay(10);

    }
//---------------------------------------------------------


//GPS refresh
static void smartDelay(unsigned long ms)
  {
    unsigned long start = millis();
    do 
      {   while (Serial1.available())
          gps.encode(Serial1.read());
      }         
    while (millis() - start < ms);
  }
//--------------------------------------------------------

//Distance travelled during each loop calculation
double getDistance(TinyGPSPlus &gps)
{
  
  float latitude, longitude;
  latitude = gps.location.lat(),6;
  longitude = gps.location.lng(),6;
  
  if (prevLongitude == 0)
    {
    prevLatitude = latitude;
    prevLongitude = longitude;
    }

  double distanceInM = (double)TinyGPSPlus::distanceBetween(prevLatitude,prevLongitude, latitude, longitude);
    
  prevLatitude = latitude;
  prevLongitude = longitude;
  
  return distanceInM;
  
}
//------------------------------------------------------------