#include <Wire.h>
#include <ds3231.h>
#include <LiquidCrystal_I2C.h>
#include <AT24CX.h>

//#define BUFF_MAX 128

//****************************************Define I2C LCD Display *********************************
#define I2C_ADDR    0x27  // Define I2C Address for the PCF8574T 
//---(Following are the PCF8574 pin assignments to LCD connections )----
// This are different than earlier/different I2C LCD displays

#define Rs_pin  0
#define Rw_pin  1
#define En_pin  2
#define BACKLIGHT_PIN  3
#define D4_pin  4
#define D5_pin  5
#define D6_pin  6
#define D7_pin  7
#define UP_pin 4
#define DOWN_pin 5
#define LEFT_pin 6
#define RIGHT_pin 7
#define OK_pin 8

#define ATMEGA328 //comment out this line if it is not ATMEGA328
//#define DEBUG  //comment out this line to disable for Production(No Serial print)

#define RRcountSIZE 20
#define  count1Address  0
#define  count1count2diffAddress  40 //only one address which is 40*32
//int  count1Address = 0;
//int  count1count2diffAddress = 40;
long count1count2diff = 0;

#define TRIPSIZE 12 //so thestartaddress is 50*32 until (50+12)*32
#define TRIPADDRESS 50

const byte interruptPin = 2;
const byte ledPin = 13;

#ifndef ATMEGA328
  volatile byte ledstate = LOW;
#endif

#define  LED_OFF  0
#define  LED_ON  1

/*-----( Declare objects )-----*/  
LiquidCrystal_I2C  lcd(I2C_ADDR,En_pin,Rw_pin,Rs_pin,D4_pin,D5_pin,D6_pin,D7_pin);
AT24CX mem;
boolean cursorON=true;
boolean blinkON=true;
//************************************ END LCD DISPLAY *******************************************

uint8_t time[8];
//char recv[BUFF_MAX];
unsigned int recv_size = 0;
unsigned long prevsettime, intervalsettime = 250;
unsigned long prevtempint, tempinterval=60000;
unsigned long prevtemprally, tempintervalrally=200;
unsigned long prevcalibrationtime, calibrationtimeinterval = 100;
unsigned long updatecounttime=0;
float temperature;
byte x,y;
unsigned long startmillis, startVSSCount;
unsigned long startsavemillis;

//#include <Button.h>        //https://github.com/JChristensen/Button       //from <Button.h> change to <JC_Button.h>
#include <JC_Button.h>

#define DN_PIN 5           //Connect two tactile button switches (or something similar)
#define UP_PIN 4           //from Arduino pin 2 to ground and from pin 3 to ground.
#define LF_PIN 6
#define RF_PIN 7
#define OK_PIN 8
#define ESC_PIN 10
#define PULLUP true        //To keep things simple, we use the Arduino's internal pullup resistor.
#define INVERT true        //Since the pullup resistor will keep the pin high unless the
                           //switch is closed, this is negative logic, i.e. a high state
                           //means the button is NOT pressed. (Assuming a normally open switch.)
#define DEBOUNCE_MS 40     //A debounce time of 20 milliseconds usually works well for tactile button switches.

#define REPEAT_FIRST 1000   //ms required before repeating on long press
#define REPEAT_INCR 75    //repeat interval for long press
#define MIN_COUNT 0
#define MAX_ROW 3
#define MAX_COL 19
#define MS_IN_HOUR 3600000 //Number of ms in an hour
#define MS_IN_MIN 60000 //Number of ms in a minute
Button btnUP(UP_PIN, PULLUP, INVERT, DEBOUNCE_MS);    //Declare the buttons
Button btnDN(DN_PIN, PULLUP, INVERT, DEBOUNCE_MS);
Button btnLF(LF_PIN, PULLUP, INVERT, DEBOUNCE_MS);
Button btnRF(RF_PIN, PULLUP, INVERT, DEBOUNCE_MS);    //Declare the buttons
Button btnOK(OK_PIN, PULLUP, INVERT, DEBOUNCE_MS);    //Declare the buttons
Button btnESC(ESC_PIN, PULLUP, INVERT, DEBOUNCE_MS);    //Declare the buttons
enum {WAIT, UP, DOWN, LEFT, RIGHT, OK, RALLYEDIT, ESC};              //The possible states for the STATE machine
enum {MAIN, CALIBRATION, SETTIME, INPUTSUBTRAYEK, RALLY, CONSTANTA};              //The possible states for the MENU
enum {FORWARD, FREEZE, BACKWARD};
char *dircount[] = {"Fw", "Zz", "Bw"};

uint8_t MENU, PREVMENU; 
uint8_t STATE, dircount1, dircount2;                        //The current state machine state
byte hour24, min60, sec60;
unsigned long rpt = REPEAT_FIRST;     //A variable time that is used to drive the repeats for long presses
uint8_t prevsec;
float constanta=3.275;
float distance1, distance2;
float prevdistance1, prevdistance2;
long prevcount1, prevcount2;
boolean showTIME = true;

float tempval;
struct ts t;
byte calibrationDistance=1;
byte tempcalibdist=1;
boolean calibrationON=false;

float oneMeter=0;
unsigned long startCalibration=0;
unsigned long Calibration=110400; //for CR-V 2012
unsigned long Calibrationcount;
unsigned long Calibrationtime;
int digit1, digit2, digit3, digit4, digit5, digit6;
byte  calibdigit1, calibdigit2, calibdigit3;
int  dist1digit1, dist1digit2; 
int  dist2digit1, dist2digit2; 
byte starttractsec, starttractmin, starttracthour;
boolean rallyON=false;

volatile unsigned long VSSCount =0;

long count1 = 0; //must be signed as technically distance can go negative
long count2 = 0;
long backcount1 = 0;
long backcount2 = 0;

//The two time variables are used to store reset times in ms
unsigned long time1 = 1;
unsigned long time2 = 1;
int CURSOR_POS = 1;

unsigned long count1Seq = 0;
boolean editDist1 = false;

//to save trayek data
struct RallyData {
  char sub;
  float speed;
  byte startHour;
  byte startMin;
  byte startSec;
//  float distance;
//  unsigned int subTime;
};
 //RallyData Trip={"b",23.342, 11,0,0,12.13};
 RallyData Trip[TRIPSIZE];
//RallyData currentTrip;
byte curTrip=0;
byte prevTrip=0;

void setup()
{
//    Serial.begin(115200);
#ifdef DEBUG
    Serial.begin(115200);
    Serial.println("Starting Rally");
#endif
    Wire.begin();
    pinMode(UP_pin, INPUT_PULLUP);
    pinMode(DOWN_pin, INPUT_PULLUP);
    pinMode(LEFT_pin, INPUT_PULLUP);
    pinMode(RIGHT_pin, INPUT_PULLUP);
    pinMode(OK_pin, INPUT_PULLUP);
    DS3231_init(DS3231_INTCN);
    //memset(recv, 0, BUFF_MAX);
#ifdef DEBUG
    Serial.println("GET time");
#endif
/*
Trip[0].sub = "A";
Trip[0].speed = 24.275;
Trip[0].startHour = 25;
//Trip[0].distance = 12.898;
Trip[9].sub = "A";
Trip[9].speed = 90.2751;
Trip[9].startHour = 22;
Trip[9].startMin = 59;
Trip[9].startSec = 58;
*/
//Trip[9].distance = 12.898;
//Serial.println(Trip[0].sub);Serial.println(Trip[0].speed);Serial.println(Trip[0].startHour);Serial.println(Trip[0].startMin);Serial.println(Trip[0].startSec);
//Serial.println(Trip[0].distance);
//Serial.println(Trip[0].subTime);

for (int i = 0; i<TRIPSIZE; i++) {
  Trip[i].sub = char(i+65);
}

  //**************************LCD Setup********************************
   lcd.begin (20,4);  // initialize the lcd 
   // Switch on the backlight
   lcd.setBacklightPin(BACKLIGHT_PIN,POSITIVE);
   lcd.setBacklight(LED_ON);
   
  //***************************END LCD Setup********************************
#ifdef DEBUG
    Serial.println("Setting time");
#endif    
    char a[]="304022129022016";

    DS3231_get(&t); //Get time

    blinkON=false;
    sec60=t.sec;
    min60=t.min;
    hour24=t.hour;

    delay(30); // delay for reading mem.read
    //mainMenu();
    printUPchar();
    printDOWNchar();
    count1 = memretrieveLong(count1Address, RRcountSIZE, count1Seq); //(int startaddress, byte RRsize, unsigned long seq)
    //Serial.println(count1Seq);
    int countaddresspointer = count1count2diffAddress*32;
//    Serial.print(countaddresspointer);
    count1count2diff = mem.readLong(countaddresspointer);
    //count1count2diff = mem.readLong((count1count2diffAddress*32));
    count2 = count1 - count1count2diff;
    //printmemLong(0,20); //startaddress, size
//    Serial.print("count1=");Serial.print(count1);Serial.print(";count2=");Serial.print(count2);Serial.print("countdiff=");Serial.println(count1count2diff);
    
    //count1Seq = 4294967290;
/*    
    for (int i=0; i<25 ; i++){
      count1Seq++;
      //Serial.println(count1Seq);
      //memwritingLong(rrcountpointer1, count1Seq, count1+1, RRcountSIZE );
      memwritingLong(count1Address, RRcountSIZE, count1Seq, count1);//(int startaddress, byte RRsize, unsigned long seq, unsigned long Data)
    }
 */   
    //mem.writeLong(896, 110025); to reset CALIBRATION value to CRV
    /*
    starttracthour=mem.read(770);
    starttractmin=mem.read(769);
    starttractsec=mem.read(768);
    */
    for (int i = 0; i<TRIPSIZE; i++) {
      memretrieveTrip(i, (TRIPADDRESS+i)*32);
//      Serial.print(Trip[i].sub);Serial.print(Trip[i].speed,5); Serial.print(";"); Serial.print(Trip[i].startHour); Serial.print(":");
//      Serial.print(Trip[i].startMin); Serial.print(":");Serial.print(Trip[i].startSec); Serial.println(";");
    }
    starttracthour=Trip[curTrip].startHour;
    starttractmin=Trip[curTrip].startMin;
    starttractsec=Trip[curTrip].startSec;
    
    if (starttracthour>23) starttracthour = 23;
    if (starttractmin>59) starttractmin = 59;
    if (starttractsec>59) starttractsec = 59;
    //constanta=mem.readFloat(800);
    constanta = Trip[curTrip].speed;
    if ((constanta > 150) || (constanta < 0)) constanta = 60;
    long m = mem.readLong(896); //read calibration value;
    if (m >= 0) {
      Calibration = m;
    }
#ifdef DEBUG
    Serial.println(m);
    Serial.println(Calibration);
    Serial.println(constanta);
#endif
    constantatodigit();

//    Serial.print(digit1);Serial.print(digit2); Serial.print(digit3); Serial.print(digit4);Serial.print(digit5);Serial.println(digit6);
//    Serial.println(constanta,5);
    //Serial.print(digitalRead(UP_pin));Serial.print(digitalRead(DOWN_pin));Serial.print(digitalRead(LEFT_pin));Serial.print(digitalRead(RIGHT_pin));Serial.println(digitalRead(OK_pin));
    
    prevcount1 = count1;
    prevcount2 = count2;

    prevcalibrationtime+=calibrationtimeinterval;
    prevtempint+=tempinterval;
    prevtemprally+=tempintervalrally;
    prevsettime += intervalsettime;
    startmillis+=1000;

    pinMode(ledPin, OUTPUT);
    pinMode(interruptPin, INPUT);  //Pull down using resistor to ground
    attachInterrupt(digitalPinToInterrupt(interruptPin), blink, FALLING);

    mainMenu();
}
void constantatodigit()
{
    digit1=(int)(constanta/10);
    digit2=(int)constanta%10;
    digit3=(int)((constanta - (int)constanta)*10);
    digit4=(int)((constanta*10 - (int)(constanta*10))*10);
    digit5=(int)((constanta*100 - (int)(constanta*100))*10);
    digit6=(int)((constanta*1000 - (int)(constanta*1000))*10);
    //digit6=(int)((constanta*10000 - (int)(constanta*10000))*10);
}    
void memretrieveTrip(int pointer, int startaddress)
{
  //Serial.print (pointer); Serial.println(startaddress);
  int pointeradd;
  Trip[pointer].speed = mem.readFloat(startaddress);
  pointeradd = (startaddress + 6);
  Trip[pointer].startHour = mem.read(pointeradd);
  pointeradd = (startaddress + 5);
  Trip[pointer].startMin = mem.read(pointeradd);
  pointeradd = (startaddress + 4);
  Trip[pointer].startSec = mem.read(pointeradd);
  if (Trip[pointer].startHour >= 24) Trip[pointer].startHour = 23;
  if (Trip[pointer].startMin >= 60) Trip[pointer].startMin = 59;
  if (Trip[pointer].startSec >= 60) Trip[pointer].startSec = 59;
}

unsigned long memretrieveLong(int startaddress, byte rrpointersize, unsigned long &seq )
{
  //Serial.print(seq);Serial.print(";R ");Serial.print(rrpointer[0]);Serial.print("; SizeofArr=");Serial.println(rrpointersize);
    unsigned long largestcountpointer=0;
    int lastpointer = 0;
    unsigned long seqnumber = 0;
    int pointeradd = 0;
    for (int i = 0; i<rrpointersize; i++) {
      pointeradd = startaddress+(i*32);
      seqnumber = mem.readLong(pointeradd);
//      Serial.print(i); Serial.print(":");Serial.print(largestcountpointer); Serial.print(":");Serial.println(seqnumber);
      
      if (largestcountpointer < seqnumber) {
        largestcountpointer = seqnumber;
        lastpointer = i;
      }
    }
    //unsigned int pointerto = mem.readLong(rrpointer[lastpointer]);
    pointeradd = startaddress+(lastpointer*32);
    seq = mem.readLong(pointeradd);
//    Serial.print(seq);Serial.print(";RR ");Serial.print("; "); Serial.println(lastpointer);
    pointeradd = pointeradd + 4;
    return mem.readLong(pointeradd); //returning count1
}

void memwritingLong(int startaddress, byte rrpointersize, unsigned long &seq,  unsigned long data)
{
    int pointer = seq % rrpointersize;
//  Serial.print(seq);Serial.print(";W ");Serial.print(startaddress); Serial.print("; data="); Serial.println(data);Serial.print("; "); Serial.println(pointer);
    int addpointer = startaddress+(pointer*32);
    mem.writeLong(addpointer, seq);
    addpointer = addpointer + 4;
    mem.writeLong(addpointer, data);
}

/*
void printmemLong(int pointer, byte pointersize)
{
  for (int i = 0 ; i < pointersize; i++)
  {
    Serial.print(i);Serial.print("->");Serial.println(mem.readLong(pointer+(i*32)));
  }
}
*/

void blink() {
#ifndef ATMEGA328
  ledstate = !ledstate;
#else
  PORTB ^= B00100000;
#endif
  VSSCount++;
}

#ifdef DEBUG
long prevtimemicros=micros(); //**************************************************************************
#endif
unsigned long rallyEditTime=micros();
byte rallyEditRpt = 0;
boolean rallyEdit =false;
boolean savedCount = true;
unsigned long intervalsavemillis = 3000;

void loop_VSS()
{
#ifdef DEBUG  
    prevtimemicros = micros();
#endif
#ifndef ATMEGA328
    digitalWrite(ledPin, ledstate);
#endif
    char tempF[6]; 
    unsigned long now1 = millis();
    
    btnUP.read();                             //read the buttons
    btnDN.read();
    btnLF.read();                             //read the buttons
    btnRF.read();
    btnOK.read();
    btnESC.read();   
    if (millis()-rallyEditTime >1500) rallyEditRpt = 0;
    switch (STATE) {
        case WAIT:                                //wait for a button event
            if (btnUP.wasPressed())
                STATE = UP;
            else if (btnDN.wasPressed())
                STATE = DOWN;
            else if (btnLF.wasPressed())
                STATE = LEFT;
            else if (btnRF.wasPressed())
                STATE = RIGHT;
            else if (btnUP.wasReleased())         //reset the long press interval
                rpt = REPEAT_FIRST;
            else if (btnDN.wasReleased())
                rpt = REPEAT_FIRST;
            else if (btnLF.wasReleased())         //reset the long press interval
                rpt = REPEAT_FIRST;
            else if (btnRF.wasReleased())
                rpt = REPEAT_FIRST;
            else if (btnUP.pressedFor(rpt)) {     //check for long press
                rpt += REPEAT_INCR;               //increment the long press interval
                STATE = UP;
            }
            else if (btnDN.pressedFor(rpt)) {
                rpt += REPEAT_INCR;
                STATE = DOWN;
            }
            else if (btnLF.pressedFor(rpt)) {     //check for long press
                rpt += REPEAT_INCR;               //increment the long press interval
                STATE = LEFT;
            }
            else if (btnRF.pressedFor(1000)) {
                rpt += REPEAT_INCR;
                STATE = RIGHT;
            }
            else if (btnOK.wasPressed() && (MENU != RALLY)) {
                STATE = OK;
            }
            else if (btnOK.wasPressed() && (CURSOR_POS > 4)) {
                STATE = OK;
            }
            else if (btnOK.wasPressed() && (CURSOR_POS <= 2)) {
                //STATE = OK;
                if (!rallyEditRpt) {
                  rallyEditTime = millis();
                  rallyEditRpt++;
                  //Serial.println("!rallyEditRpt");
                  //Serial.println(rallyEditRpt);
                }
                else if (millis()-rallyEditTime > 1500) {
                  rallyEditRpt = 0;
                  //Serial.println("rallyEditRpt = 0");
                  //Serial.println(rallyEditRpt);
                }
                else if (rallyEditRpt == 2) {
                  //Serial.println("rallyEditRpt = ");Serial.println(rallyEditRpt);
                  STATE = RALLYEDIT;
                  rallyEditRpt = 0;
                }
                else {
                  rallyEditRpt++;
                  //Serial.println("rallyEditRpt++");
                  //Serial.println(rallyEditRpt);
                }
            }
            else if (btnOK.pressedFor(rpt) && (CURSOR_POS <= 4)) {
                STATE = OK;
            }
            else if (btnESC.wasPressed()){
                STATE = ESC;
                //Serial.println("ESC pressed");
            }
            break;

        case DOWN:                                //increment the counter
          switch (MENU) {
            case MAIN:
              y = min(y++, MAX_ROW);      //but not more than the specified maximum
              break;
              
            case RALLY:
              if (blinkON) {
                if (y == 1) {
                  if (x == 0 || x == 1) {
                    dist1digit1 = max(dist1digit1--, 0);
                    lcd.setCursor(0,y);
                    if (dist1digit1 <10) lcd.print("0");
                    lcd.print(dist1digit1);
                  }
                  else if (x == 3 || x == 4 || x == 5) {
                    dist1digit2 = max(dist1digit2-=1, 0);
                    lcd.setCursor(3,y);
                    if (dist1digit2 <10) {
                      lcd.print("00");
                    }else if (dist1digit2 <100) lcd.print("0");
                    lcd.print(dist1digit2);
                  }
                }
                else if (y ==3) {
                  if (x == 0 || x == 1) {
                    dist2digit1 = max(dist2digit1--, 0);
                    lcd.setCursor(0,y);
                    if (dist2digit1 <10) lcd.print("0");
                    lcd.print(dist2digit1);
                  }
                  else if (x == 3 || x == 4 || x == 5) {
                    dist2digit2 = max(dist2digit2-=1, 0);
                    lcd.setCursor(3,y);
                    if (dist2digit2 <10) {
                      lcd.print("00");
                    }else if (dist2digit2 <100) lcd.print("0");
                    lcd.print(dist2digit2);
                  }
                  
                }
                
              }
              else {
                CURSOR_POS=min(CURSOR_POS++,10);
                set_cursor_pos(CURSOR_POS);
              }
              
              break;
              
            case CALIBRATION:
              if (!blinkON)  y = min(y++, MAX_ROW );      //but not more than the specified maximum
              else if (blinkON && y==1) {
                //calibrationDistance=max(calibrationDistance--, 1);
                tempcalibdist=max(tempcalibdist--, 1);
                lcd.print(tempcalibdist);
              }
              else if (blinkON && y==3 && (x==18||x==19)) {
                calibdigit3 = max(calibdigit3--,0);
                lcd.setCursor(18,3);
                if (calibdigit3<10) lcd.print("0");
                lcd.print(calibdigit3);
              }
              else if (blinkON && y==3 && (x==16||x==17)) {
                calibdigit2 = max(calibdigit2--,0);
                lcd.setCursor(16,3);
                if (calibdigit2<10) lcd.print("0");
                lcd.print(calibdigit2);
              }
              else if (blinkON && y==3 && (x==14||x==15)) {
                calibdigit1 = max(calibdigit1--,0);
                lcd.setCursor(14,3);
                if (calibdigit1<10) lcd.print("0");
                lcd.print(calibdigit1);
              }
              break;
              
            case CONSTANTA:
              if (!blinkON) {
                y = min(y++, 3);      //but not less than the specified minimum
                if (y==1) x=15;
              }
              else if (y==1) {
                prevTrip = min(prevTrip++,TRIPSIZE-1);
                //curTrip = min(curTrip++,TRIPSIZE-1);
                lcd.setCursor(14,y);
                lcd.print(Trip[prevTrip].sub);
              }
              else if (x==13 && y==2) {
                digit1 = max(digit1--, MIN_COUNT);
                lcd.print(digit1);
              }
              else if (x==14 && y==2) {
                digit2 = max(digit2--, MIN_COUNT);
                lcd.print(digit2);
              }
              else if (x==16 && y==2) {
                digit3 = max(digit3--, MIN_COUNT);
                lcd.print(digit3);
              }
              else if (x==17 && y==2) {
                digit4 = max(digit4--, MIN_COUNT);
                lcd.print(digit4);
              }
              else if (x==18 && y==2) {
                digit5 = max(digit5--, MIN_COUNT);
                lcd.print(digit5);
              }
              else if (x==19 && y==2) {
                digit6 = max(digit6--, MIN_COUNT);
                lcd.print(digit6);
              }
              else if ((x==12 || x==13) && y==3) {  //******SET CONSTANTA TIME *******
                hour24=max(hour24--, MIN_COUNT);
                lcd.setCursor(12,y);
                if (hour24<10) lcd.print("0");
                lcd.print(hour24);
              }
              else if ((x==15 || x==16) && y==3) {
                min60=max(min60--, MIN_COUNT);
                lcd.setCursor(15,y);
                if (min60<10) lcd.print("0");
                lcd.print(min60);
              }
              else if ((x==18 || x==19) && y==3) { 
                sec60=max(sec60--, MIN_COUNT);
                lcd.setCursor(18,y);
                if (sec60<10) lcd.print("0");
                lcd.print(sec60);
              }
              break;
              
            case SETTIME:
              if (!blinkON) {
                y = min(y++, MAX_ROW);      //but not more than the specified maximum
                lcd.setCursor(x,y);
              }
              else if ((x==0 || x==1) && y==3) {
                hour24=max(hour24--, MIN_COUNT);
                lcd.setCursor(0,y);
                if (hour24<10) lcd.print("0");
                lcd.print(hour24);
              }
              else if ((x==3 || x==4) && y==3) {
                min60=max(min60--, MIN_COUNT);
                lcd.setCursor(3,y);
                if (min60<10) lcd.print("0");
                lcd.print(min60);
              }
              else if ((x==6 || x==7) && y==3) {
                sec60=max(sec60--, MIN_COUNT);
                lcd.setCursor(6,y);
                if (sec60<10) lcd.print("0");
                lcd.print(sec60);
              }
              break;
          }
          STATE = WAIT;
          lcd.setCursor(x,y);
          break;

        case UP:                             //decrement the counter
          switch(MENU) {
            case MAIN:
              y = max(y--, MIN_COUNT);      //but not less than the specified minimum
              break;
              
            case CALIBRATION:
              if (!blinkON) {
                y = max(y--, MIN_COUNT);      //but not less than the specified minimum
                x = 18;
              }
              else if (blinkON && y==1) {
                //calibrationDistance=min(calibrationDistance++, 9);
                tempcalibdist=min(tempcalibdist++, 9);
                lcd.print(tempcalibdist);
              }
              else if (blinkON && y==3 && (x==18||x==19)) {
                calibdigit3 = min(calibdigit3++,99);
                lcd.setCursor(18,3);
                if (calibdigit3<10) lcd.print("0");
                lcd.print(calibdigit3);
              }
              else if (blinkON && y==3 && (x==16||x==17)) {
                calibdigit2 = min(calibdigit2++,99);
                lcd.setCursor(16,3);
                if (calibdigit2<10) lcd.print("0");
                lcd.print(calibdigit2);
              }
              else if (blinkON && y==3 && (x==14||x==15)) {
                calibdigit1 = min(calibdigit1++,50);
                lcd.setCursor(14,3);
                if (calibdigit1<10) lcd.print("0");
                lcd.print(calibdigit1);
              }
              break;
              
            case RALLY:
              if (blinkON) {
                if (y == 1) {
                  if (x == 0 || x == 1) {
                    dist1digit1 = min(dist1digit1++, 999);
                    lcd.setCursor(0,y);
                    if (dist1digit1 <10) lcd.print("0");
                    lcd.print(dist1digit1);
                  }
                  else if (x == 3 || x == 4 || x == 5) {
                    dist1digit2 = min(dist1digit2+=1, 999);
                    lcd.setCursor(3,y);
                    if (dist1digit2 <10) {
                      lcd.print("00");
                    }else if (dist1digit2 <100) lcd.print("0");
                    lcd.print(dist1digit2);
                  }
                }
                else if (y == 3) {
                  if (x == 0 || x == 1) {
                    dist2digit1 = min(dist2digit1++, 99);
                    lcd.setCursor(0,y);
                    if (dist2digit1 <10) lcd.print("0");
                    lcd.print(dist2digit1);
                  }
                  else if (x == 3 || x == 4 || x == 5) {
                    dist2digit2 = min(dist2digit2+=1, 999);
                    lcd.setCursor(3,y);
                    if (dist2digit2 <10) {
                      lcd.print("00");
                    }else if (dist2digit2 <100) lcd.print("0");
                    lcd.print(dist2digit2);
                  }                  
                }
                
              }
              else {
                CURSOR_POS=max(CURSOR_POS--,1);
                set_cursor_pos(CURSOR_POS);
              }
              break;
              
            case CONSTANTA:
              if (!blinkON) {
                y = max(y--, MIN_COUNT);      //but not less than the specified minimum
                if (y==0) x=16;
                else if (y==1) x=15;
              }
              else if (y==1) {
                prevTrip = max(prevTrip--, MIN_COUNT);
                //curTrip = max(curTrip--, MIN_COUNT);
                lcd.setCursor(14,y);
                lcd.print(Trip[prevTrip].sub);
              }
              else if (x==13 && y==2) {
                digit1=min(digit1++, 9);
                lcd.print(digit1);
              }
              else if (x==14 && y==2) {
                digit2=min(digit2++, 9);
                lcd.print(digit2);
              }
              else if (x==16 && y==2) {
                digit3=min(digit3++, 9);
                lcd.print(digit3);
              }
              else if (x==17 && y==2) {
                digit4=min(digit4++, 9);
                lcd.print(digit4);
              }
              else if (x==18 && y==2) {
                digit5=min(digit5++, 9);
                lcd.print(digit5);
              }
              else if (x==19 && y==2) {
                digit6=min(digit6++, 9);
                lcd.print(digit6);
              }
              else if ((x==12 || x==13) && y==3) { //******SET CONSTANTA TIME *******
                hour24=min(hour24++, 23);
                lcd.setCursor(12,y);
                if (hour24<10) lcd.print("0");
                lcd.print(hour24);
              }
              else if ((x==15 || x==16) && y==3) {
                min60=min(min60++, 59);
                lcd.setCursor(15,y);
                if (min60<10) lcd.print("0");
                lcd.print(min60);
              }
              else if ((x==18 || x==19) && y==3) {
                sec60=min(sec60++, 59);
                lcd.setCursor(18,y);
                if (sec60<10) lcd.print("0");
                lcd.print(sec60);
              }
              break;
              
            case SETTIME:
              if (!blinkON) {
                y = max(y--, MIN_COUNT);      //but not less than the specified minimum
                //Serial.print(x); Serial.print(","); Serial.println(y);
                //lcd.setCursor(x,y);
              }
              else if ((x==0 || x==1) && y==3) {
                hour24=min(hour24++, 23);
                lcd.setCursor(0,y);
                if (hour24<10) lcd.print("0");
                lcd.print(hour24);
              }
              else if ((x==3 || x==4) && y==3) {
                min60=min(min60++, 59);
                lcd.setCursor(3,y);
                if (min60<10) lcd.print("0");
                lcd.print(min60);
              }
              else if ((x==6 || x==7) && y==3) {
                sec60=min(sec60++, 59);
                lcd.setCursor(6,y);
                if (sec60<10) lcd.print("0");
                lcd.print(sec60);
              }
              break;
          }
          STATE = WAIT;
          lcd.setCursor(x,y);
          break;
            
        case RIGHT:                                //increment the counter
          switch (MENU) {
            case SETTIME:
              x = min(x++, MAX_COL); //MAX_COUNT);      //but not more than the specified maximum
              //STATE = WAIT;
            //Serial.print(x); Serial.print(","); Serial.println(y);
              //lcd.setCursor(x,y);
              break;
              
            case RALLY:
              if (blinkON) {
                
                x = min(x++, 5);
                //Serial.println(x);
              }
              else if (CURSOR_POS % 2 ==0) CURSOR_POS=min(CURSOR_POS+2,10);
              else CURSOR_POS=min(CURSOR_POS+2,9);
              if (!blinkON) set_cursor_pos(CURSOR_POS);
              break; 
                    
            case CONSTANTA:
              if (y==2 || y==3) {
                x = min(x++, MAX_COL); //MAX_COUNT);
              }
              break;
              
            case CALIBRATION:
              if (y==3) {
                x = min(x++,MAX_COL);
              }
              else x = 18;
              break;
            
          }
          STATE = WAIT;
          lcd.setCursor(x,y);
          break;

        case LEFT:                                //decrement the counter
          switch (MENU)  {
            case SETTIME:
              x = max(x--, MIN_COUNT);      //but not less than the specified minimum
              //STATE = WAIT;
              //Serial.print(x); Serial.print(","); Serial.println(y);
              //lcd.setCursor(x,y);
              break;
              
            case RALLY:
              if (blinkON) {
                x = max(x--, MIN_COUNT);
              }
              else if (CURSOR_POS % 2 == 0) CURSOR_POS=max(CURSOR_POS-2,2);
              else CURSOR_POS=max(CURSOR_POS-2,1);
              if (!blinkON) set_cursor_pos(CURSOR_POS);
              break;
                               
            case CONSTANTA:
              if (y==2 || y==3){
                x = max(x--, 12);
              }
              break;
              
            case CALIBRATION:
              if (y==3) {
                x = max(x--, 14);
              }
              else x = 18;
              break;
          }
          STATE = WAIT;
          lcd.setCursor(x,y);
          break;
          
        case OK:
          switch (MENU) {
            case MAIN:
              if (y==1) {
                MENU = SETTIME;
                //prevsettime += millis();
                //prevtempint+=millis();
              }
              else if (y==0) {
                MENU = RALLY;
                //prevtemprally=millis();
              }
              else if (y==2) MENU = CONSTANTA;
              else if (y==3) {
                MENU = CALIBRATION;
                //prevcalibrationtime+=millis();
              }
              break;
              
            case CONSTANTA:
              if (y==0) MENU = MAIN;
              //else if (!blinkON && y==1) {
                
             // }
              else if (!blinkON && y==2) {
                constantatodigit();
                lcd.blink();
                blinkON=!blinkON;
              }
              else if (!blinkON && y==3) {
                sec60 = starttractsec;
                min60 = starttractmin;
                hour24 = starttracthour;
                lcd.blink();
                blinkON=!blinkON;
              }
              else if (!blinkON && y==1) {
                prevTrip = curTrip;
                lcd.blink();
                blinkON = !blinkON;
              }
              else if (blinkON && y==1) {
                curTrip = prevTrip;
                constanta = Trip[curTrip].speed;
                starttractsec = Trip[curTrip].startSec;
                starttractmin = Trip[curTrip].startMin;
                starttracthour = Trip[curTrip].startHour;
                lcd.noBlink();
                blinkON=!blinkON;
                constantaMenu();
              }
              else if (blinkON && y==2) {
                lcd.noBlink();
                blinkON=!blinkON;
                constanta = (digit1*10)+digit2+(float)digit3/10+(float)digit4/100+(float)digit5/1000+(float)digit6/10000;  //+(float)digit6/100000;
                //mem.writeFloat(800, constanta);
                int pointeradd = ((TRIPADDRESS+curTrip)*32);
                mem.writeFloat(pointeradd, constanta);
                Trip[curTrip].speed = constanta;
#ifdef DEBUG
                Serial.println(pointeradd); Serial.print(";");Serial.println(constanta,5);
#endif
              }
              else if (blinkON && y==3) {
                starttractsec=sec60;
                starttractmin=min60;
                starttracthour=hour24;
                unsigned long valuesettime = starttracthour;
                valuesettime = valuesettime * 256 + starttractmin;  // effectively shift the first byte 8 bit positions
                valuesettime = valuesettime * 256 + starttractsec;
                //mem.writeLong(768,valuesettime);
                int pointeradd = ((TRIPADDRESS+curTrip)*32+4);
                mem.writeLong(pointeradd, valuesettime);
                Trip[curTrip].startHour = starttracthour;
                Trip[curTrip].startMin = starttractmin;
                Trip[curTrip].startSec = starttractsec;
                lcd.noBlink();
                blinkON=!blinkON;
#ifdef DEBUG
                Serial.println(pointeradd); Serial.println(valuesettime);
#endif
              }
              else {// if (y==3) {
                lcd.blink();
                blinkON=!blinkON;
              }
              break;

            case RALLY:
#ifdef DEBUG
              Serial.println("OKRALLY");
#endif
              switch(CURSOR_POS)
              {
                case 1:
                  count1 = 0;
                  backcount1 = 0;
                  time1 = VSSCount;
                  //Serial.println("case 1");
                  break;
                case 2:
                  count2 = 0;
                  backcount2 = 0;
                  time2 = VSSCount;
                  //Serial.println("case 2");
                  break;
                case 3:
                  count2 = count1;  //no yet UNDO next update can UNDO
                  backcount2 = 0;
                  //time2 = VSSCount;
                  //Serial.println("case 3");
                  break;
                case 4:
                  count1 = count2;  //no yet UNDO next update can UNDO
                  backcount1 = 0;
                  //time1 = VSSCount;
                  //Serial.println("case 4");
                  break;
                case 5:
                  //dirfwdcount1 = !dirfwdcount1;
                  switch (dircount1)
                  {
                    case FORWARD:
                      dircount1 = FREEZE;
                      break;
                    case FREEZE:
                      dircount1 = BACKWARD;
                      break;
                    case BACKWARD:
                      dircount1 = FORWARD;
                      break;
                    default:
                      dircount1 = FORWARD;
                      break;
                  }
                  secondrallyMenu();
                  //Serial.println("case 5");
                  break;
                case 6:
                  //dirfwdcount2 = !dirfwdcount2;
                  switch (dircount2)
                  {
                    case FORWARD:
                      dircount2 = FREEZE;
                      break;
                    case FREEZE:
                      dircount2 = BACKWARD;
                      break;
                    case BACKWARD:
                      dircount2 = FORWARD;
                      break;
                    default:
                      dircount2 = FORWARD;
                      break;
                  }                  
                  secondrallyMenu();
                  //Serial.println("case 6");
                  break;
                case 7:
                  showTIME = !showTIME;
                  rallyMenu();
                  break;
                case 8:
                  showTIME = !showTIME;
                  rallyMenu();
                  break;
                case 9:
                  //Serial.println("case 9");
                  MENU = MAIN;
                  lcd.noBlink();
                  break;
                case 10:
                  //Serial.println("case 10");
                  MENU = MAIN;
                  lcd.noBlink();
                  break;
              
              }
              break;
              
            case CALIBRATION:
              if (blinkON && y==1) {
                lcd.noBlink();
                blinkON=!blinkON;
                calibrationDistance = tempcalibdist;
              }
              else if (!blinkON && y==1) {
                lcd.blink();
                blinkON=!blinkON;
                tempcalibdist = calibrationDistance;
              }
              else if (!blinkON && y==2) {
                //startCalibration = millis();
                Calibrationcount = 0;
                startCalibration = VSSCount;
                Calibrationtime = startCalibration;
                blinkON=!blinkON;
                lcd.blink();
                calibrationON=true;
                lcd.setCursor(0,2);
                lcd.print("Calibrating 00.000Km");
              }
              else if (blinkON && y==2) {
                Calibration = (VSSCount-startCalibration)/(float)calibrationDistance;
                mem.writeLong(896, Calibration); //Save Calibration to memory
                oneMeter=(millis()-startCalibration)/(1000*(float)calibrationDistance);//*calibrationDistance);
                oneMeter=((float)VSSCount-startCalibration)/(1000*(float)calibrationDistance);//*calibrationDistance);
                oneMeter=(float)(Calibration)/1000*(float)calibrationDistance;
                blinkON=!blinkON;
                lcd.noBlink();
                calibrationON=false;
                lcd.setCursor(0,2);
                lcd.print("Start      ");
                lcd.setCursor(0,3);
                lcd.print("1m=");
                //Serial.print(startCalibration);Serial.print(";");Serial.print((millis()-startCalibration)/1000);Serial.print(";");
#ifdef DEBUG
                Serial.println(oneMeter,5);
#endif
                lcd.print(oneMeter,3);
                lcd.print(";");
              }
              else if (!blinkON && y==3) {   //not yet implemented
                lcd.blink();
                blinkON=!blinkON;
                calibdigit1 = (byte)(Calibration/10000);
                calibdigit2 =  (byte)((Calibration % 10000)/100);
                calibdigit3 = Calibration % 100;
                //Serial.println(calibdigit1);Serial.println(calibdigit2);Serial.println(calibdigit3);
              }
              else if (blinkON && y==3) {
                lcd.noBlink();
                blinkON=!blinkON;
                //Serial.println(calibdigit1);Serial.println(calibdigit2);Serial.println(calibdigit3);
                Calibration = ((long)calibdigit1*10000) + ((long)calibdigit2*100) + (calibdigit3);
                //Serial.println(Calibration);
                mem.writeLong(896, Calibration);
              }
              else if (y==0) MENU = MAIN;
              break;
              
            case SETTIME:
              if (blinkON && y==3) {
                t.sec=sec60;
                t.min=min60;
                t.hour=hour24;
                DS3231_set(t);
                lcd.noBlink();
                blinkON=!blinkON;
                //Serial.println("OK");
              }
              else if (y==3) {
                lcd.blink();
                blinkON=!blinkON;
              }
              else  {//(blinkON) {
                MENU = MAIN;
                lcd.noBlink();
              }
              break;
          }
          STATE = WAIT;
          lcd.setCursor(x,y);
          break;
          
        case RALLYEDIT:
          if ((CURSOR_POS == 1 || CURSOR_POS == 2) && !blinkON) {
            rallyEdit = true;
            blinkON = true;
            lcd.blink();
            //if (distance<100) {
            dist1digit1=((int)distance1%1000);
            dist1digit2=(int)((distance1 - (int)distance1)*1000);
            dist2digit1=((int)distance2%1000);
            dist2digit2=(int)((distance2 - (int)distance2)*1000);
            //}
            /*
            Serial.print(dist1digit1);Serial.println(dist1digit2); //Serial.print(dist1digit3); Serial.print(dist1digit4);Serial.println(dist1digit5);//Serial.println(digit6);
            Serial.println(distance1,3);
            Serial.print(dist2digit1);Serial.println(dist2digit2); //Serial.print(dist1digit3); Serial.print(dist1digit4);Serial.println(dist1digit5);//Serial.println(digit6);
            Serial.println(distance2,3);
            */
          }
          else if (blinkON) { //set the new distance to count
            blinkON = false;
            rallyEdit = false;
            /*
            Serial.println(count1);
            Serial.println(distance1,5);
            Serial.println("");
            */
            if (CURSOR_POS == 1) {
              distance1 = dist1digit1 + (float)dist1digit2/1000;
              count1 = (unsigned long)(distance1 * Calibration) + 5;
              //distance1 = (float)count1/((float)Calibration);
            }else if (CURSOR_POS == 2) {
              distance2 = dist2digit1 + (float)dist2digit2/1000;
              count2 = (unsigned long)(distance2 * Calibration) + 5;
              //distance2 = (float)count2/((float)Calibration);
            }
            /*
            Serial.println(count1);
            Serial.println(distance1,3);
            */
            set_cursor_pos(CURSOR_POS);
          }
          STATE = WAIT;
          lcd.setCursor(x,y);
          break;

        case ESC:
          switch (MENU)
          {
            case RALLY:
              lcd.noBlink();
              MENU=MAIN;
              if (rallyEdit) {
                rallyEdit = false;
                lcd.blink();
                MENU = RALLY;
                set_cursor_pos(CURSOR_POS);
              }
              blinkON=false;
              
              break;
            case SETTIME:
              lcd.noBlink();
              if (!blinkON) MENU = MAIN;
              blinkON=false;
              //MENU=MAIN;
              break;
            case CONSTANTA:
              //MENU=MAIN;
              if (!blinkON) MENU = MAIN;
              else constantaMenu();
              lcd.noBlink();
              blinkON=false;
              break;
            case CALIBRATION:
              lcd.noBlink();
              if (!blinkON) MENU = MAIN;
              blinkON=false;
              calibrationON=false;
              //MENU=MAIN;
              break;
            
          }
          STATE = WAIT;
          lcd.setCursor(x,y);
          break;
    }

//*****************************************************************
//*************************************** M E N U *****************
//
//
//
//*************************************** M E N U *****************
//*****************************************************************

    if (PREVMENU!=MENU)
    {
      switch (MENU)
      {
        case (MAIN):
           mainMenu();
           break;
           
        case (RALLY):
           lcd.clear();
           rallyMenu();
           rallyON=true;
           //lcd.blink();
           CURSOR_POS=10;
           set_cursor_pos(CURSOR_POS);
           break;
           
        case (SETTIME):
           lcd.clear();
           settimeMenu();
           sec60=t.sec;
           min60=t.min;
           hour24=t.hour;
           lcd.setCursor(0,3);
           lcdprint2digit(hour24);
           lcd.print(":");
           lcdprint2digit(min60);
           lcd.print(":");
           lcdprint2digit(sec60);
           x=0;
           y=2;
           lcd.setCursor(x,y);
           break;
           
        case (CALIBRATION):
           calibrationMenu();
           break;
           
        case CONSTANTA:
           sec60=starttractsec;
           min60=starttractmin;
           hour24=starttracthour;
           x = 16;
           y = 0;
           constantaMenu();
           break;
           
        case (INPUTSUBTRAYEK):
           break;
      }
    }
    
    if (MENU==RALLY) {
      if ((now1 - prevtemprally) >= tempintervalrally) {
      //if ((long)(now1 - prevtemprally) >= 0) {
        //rallyMenu();
        if (!rallyEdit) redrawrallyMenu(); //Distance is edited?
        prevtemprally=now1;
        //prevtemprally+=tempintervalrally;
      }
    }
    
    if (MENU==SETTIME) {
      //if ((long)(now1 - prevtempint) >=0 ){
      if ((now1 - prevtempint) >=tempinterval ){
        temperature = DS3231_get_treg(); //Get temperature
        dtostrf(temperature, 5, 1, tempF);
        lcd.setCursor(9,1);
        lcd.print(tempF);
        lcd.print((char)223);
        lcd.print("C ");
        //prevtempint+=tempinterval;
        prevtempint=now1;
        lcd.setCursor(x,y);
        //lcd.setCursor(15,0);
        //lcd.print(now);
      }
      //if ((long)(now1- prevsettime) >=0 ){ //show SET TIME MENU
      if ((now1 - prevsettime) >=intervalsettime ){ //show SET TIME MENU
        DS3231_get(&t); //Get time
        if (prevsec!=t.sec) { //will update the LCD every sec(as long as the t.sec is different) eventhough intervalseetime is 250ms
           settimeMenu();
           prevsettime = now1;
           //prevsettime += intervalsettime;
        }
      }
    }
    
    if (MENU==CALIBRATION) {
      if (calibrationON) {
        if ((now1 - prevcalibrationtime) >= calibrationtimeinterval ) {
        //if ((long)(now1 - prevcalibrationtime) >=0 ) {
          redrawcalibrationMenu();
          char Calibrationstr[6];
          unsigned long calibtemp = VSSCount - startCalibration;
          sprintf(Calibrationstr, "%06lu", calibtemp);
          lcd.setCursor(14,3);
          lcd.print(Calibrationstr);
          lcd.setCursor(x,y);
          prevcalibrationtime=now1;
          //prevcalibrationtime+=calibrationtimeinterval;
        }
      }
    }

    //if (((long)(millis()-startmillis)>=0) && MENU!=CALIBRATION) {
    if ((now1-startmillis)>=500) {  //&& MENU!=CALIBRATION && MENU!=MAIN) {
      unsigned long VSSCountDiff = VSSCount - startVSSCount;
      startVSSCount=VSSCount;
      if ((VSSCountDiff == 0) && savedCount) {
        intervalsavemillis = 4000000;
      }
      else if ((VSSCountDiff == 0) && !savedCount) {
        saveCount();
        savedCount = !savedCount;
        intervalsavemillis = 2000000;
      }
      else if (VSSCountDiff < 15) intervalsavemillis = 3000;
      else if (VSSCountDiff < 70) intervalsavemillis = 60000;
      else intervalsavemillis = 4000000;
      if (VSSCountDiff > 0) savedCount = false;
      if (MENU!=CALIBRATION && MENU!=MAIN && MENU!=CONSTANTA) {
        lcd.setCursor(0,0);
        if (VSSCountDiff==0) lcd.print("0");
        else if (VSSCountDiff <= 10) lcd.print("1");
        else if (VSSCountDiff <= 50) lcd.print("2");
        else if (VSSCountDiff <= 150) lcd.print("3");
        else if (VSSCountDiff <= 250) lcd.print("4");
        else if (VSSCountDiff <= 400) lcd.print("5");
        else if (VSSCountDiff <= 600) lcd.print("6");
        else if (VSSCountDiff <= 900) lcd.print("7");
        else if (VSSCountDiff <= 1200) lcd.print("8");
        else if (VSSCountDiff > 1200) lcd.print("9");
        
        //lcd.print(VSSCountDiff);
        //lcd.print("  ");
        lcd.setCursor(x,y);
      }
      //startmillis+=400;
      startmillis=now1;
      
    }

/*
    if (((millis() % 100)==0) && MENU!=CALIBRATION) {
      lcd.setCursor(0,2);
      lcd.print(VSSCount);
      lcd.print(" ");
      lcd.setCursor(x,y);
    }
*/
    if ((millis()-updatecounttime)>=250)  {
      updateCount();
      updatecounttime = millis();
    }

    if ((millis()-startsavemillis) > intervalsavemillis) {
      saveCount();
      //startsavemillis += 3000;
      startsavemillis = millis;
    }
#ifdef DEBUG
  Serial.print(micros()-prevtimemicros); Serial.println("**");
#endif
}
void printUPchar()
{
  byte thumb1[8] = {B00100,B01110,B11111,B00100,B00100,B00100,B00100,B00100};
  lcd.createChar(1, thumb1);
  //lcd.write(1);
}
void printDOWNchar()
{
  byte thumb1[8] = {B00100,B00100,B00100,B00100,B00100,B11111,B01110,B00100};
  lcd.createChar(2, thumb1);
  //lcd.write(2);
}
void printMOVEchar()
{
  byte Movechar3[8] = {B00000,B00000,B00000,B00000,B00000,B00000,B00000,B11111};
  lcd.createChar(3, Movechar3);
  byte Movechar4[8] = {B00000,B00000,B00000,B00000,B00000,B00000,B11111,B11111};
  lcd.createChar(4, Movechar4);
  byte Movechar5[8] = {B00000,B00000,B00000,B00000,B00000,B11111,B11111,B11111};
  lcd.createChar(5, Movechar5);
}


void lcdprint2digit(byte val)
{
  if (val<10) lcd.print("0");
  lcd.print(val);
}
void setTheTime(char *cmd)
{
    // ssmmhhWDDMMYYYY  set time

    t.sec = inp2toi(cmd, 0);
    t.min = inp2toi(cmd, 2);
    t.hour = inp2toi(cmd, 4);
    t.wday = inp2toi(cmd, 6);
    t.mday = inp2toi(cmd, 7);
    t.mon = inp2toi(cmd, 9);
    t.year = inp2toi(cmd, 11) * 100 + inp2toi(cmd, 13);
    DS3231_set(t);
#ifdef DEBUG
    Serial.println("OK setTheTime()");
#endif
}

void printMonth(int month)
{
  switch(month)
  {
    case 1: lcd.print(" January ");break;
    case 2: lcd.print(" February ");break;
    case 3: lcd.print(" March ");break;
    case 4: lcd.print(" April ");break;
    case 5: lcd.print(" May ");break;
    case 6: lcd.print(" June ");break;
    case 7: lcd.print(" July ");break;
    case 8: lcd.print(" August ");break;
    case 9: lcd.print(" September ");break;
    case 10: lcd.print(" October ");break;
    case 11: lcd.print(" November ");break;
    case 12: lcd.print(" December ");break;
    default: lcd.print(" Error ");break;
  } 
}

void mainMenu()
{
  MENU = MAIN;
  lcd.clear();
  lcd.print("1. Rally TSD");
  lcd.setCursor(0,1);
  lcd.print("2. Set Main Time");
  lcd.setCursor(0,2);
  lcd.print("3. Input Constanta");
  lcd.setCursor(0,3);
  lcd.print("4. Calibration ODO");
  //lcd.home();
  x=2;
  y=0;
  lcd.cursor();
  lcd.setCursor(x,y);
  PREVMENU = MENU;
}

void settimeMenu()
{
    lcd.setCursor(0,0);
    lcd.print(t.mday);
    printMonth(t.mon);
    lcd.print(t.year);
    
    runningtime(0,1);
        
    lcd.setCursor(x,y);
    prevsec = t.sec;
    PREVMENU = MENU;
}

void runningtime(byte x1,byte y1)
{
    lcd.setCursor(x1,y1); //Go to second line of the LCD Screen
    if(t.hour<10)
    {
      lcd.print("0");
    }
    lcd.print(t.hour);
    lcd.print(":");
    if(t.min<10)
    {
      lcd.print("0");
    }
    lcd.print(t.min);
    lcd.print(":");
    if(t.sec<10)
    {
      lcd.print("0");
    }
    lcd.print(t.sec);
}
void calibrationMenu()
{
  MENU = CALIBRATION;
  lcd.clear();
  lcd.print("Calibration     Back");
  lcd.setCursor(0,1);
  lcd.print("Set Distance(Km): ");
  lcd.print(calibrationDistance);
  lcd.setCursor(0,2);
  lcd.print("Start       00.000Km");
  lcd.setCursor(0,3);
  lcd.print("1m=");
  lcd.print(((float)Calibration/1000),3);
  char Calibrationstr[6];
  sprintf(Calibrationstr, "%06lu", Calibration);
  lcd.setCursor(14,3);
  lcd.print(Calibrationstr);
  //lcd.setCursor(0,0);
  //lcd.print(Calibration);
  //lcd.home();
  x=18;
  y=0;
  lcd.setCursor(x,y);
  lcd.cursor();
  //lcd.blink();
  //blinkON=true;
  PREVMENU = MENU;
}

void constantaMenu()
{
  MENU = CONSTANTA;
  lcd.clear();
  lcd.print("Constanta       Back");
  lcd.setCursor(0,1);
  lcd.print("Sub Trip:");
  lcd.setCursor(14,1);
  lcd.print(Trip[curTrip].sub);
  lcd.setCursor(0,2);
  lcd.print("Set Speed:");
  lcd.setCursor(13,2);
  if (constanta<10) lcd.print("0");
  lcd.print(constanta,4);
  //lcd.print(
  lcd.setCursor(0,3);
  lcd.print("Start Time:");
  //DS3231_get(&t); //Get time
  lcd.setCursor(12,3); //Go to second line of the LCD Screen
        if(starttracthour<10)
        {
          lcd.print("0");
        }
        lcd.print(starttracthour);
        lcd.print(":");
        if(starttractmin<10)
        {
          lcd.print("0");
        }
        lcd.print(starttractmin);
        lcd.print(":");
        if(starttractsec<10)
        {
          lcd.print("0");
        }
        lcd.print(starttractsec);
  lcd.print("");
  //x=14;
  //y=0;
  lcd.setCursor(x,y);
  lcd.cursor();
  //lcd.blink();
  //blinkON=true;
  PREVMENU = MENU;

}
void updateCount()
{
  if (dircount1 == FORWARD) {
      count1 = count1 + (VSSCount-time1);
  }
  else if (dircount1 == BACKWARD) {
      count1 = count1 - (VSSCount-time1);
  }

  if (dircount2 == FORWARD) {
      count2 = count2 + (VSSCount-time2);
  }
  else if (dircount2 == BACKWARD) {
      count2 = count2 - (VSSCount-time2);
  }
  time1 = VSSCount;
  time2 = VSSCount;
}
void saveCount()
{
  if (count1 != prevcount1) {
    count1Seq++;
    memwritingLong(count1Address, RRcountSIZE, count1Seq, count1);//(int startaddress, byte RRsize, unsigned long seq, unsigned long Data)
    prevcount1 = count1;
  }
  
  if ((count1-count2) != count1count2diff) {
#ifdef DEBUG
    Serial.print("count1=");Serial.print(count1);Serial.print(";count2=");Serial.print(count2);Serial.print("countdiff=");Serial.println(count1count2diff);
#endif
    count1count2diff = count1-count2;
    int pointer = count1count2diffAddress*32;
    mem.writeLong(pointer,count1count2diff);
#ifdef DEBUG
    Serial.println("save count1count2diff");
#endif
  }
}
void rallyMenu()
{
  MENU = RALLY;

  //lcd.clear();
  lcd.setCursor(15,0);
  if (showTIME)  lcd.print("Time ");
  else lcd.print("TDiff");

  secondrallyMenu();
  PREVMENU = MENU;
#ifdef DEBUG
  Serial.println("rallyMenu");
#endif
}

void secondrallyMenu()
{
  char heading2[6];
  char heading4[6];
  lcd.setCursor(9,1);
  //sprintf(heading2, "%s",  (dirfwdcount1?"Fw":"Bw"));
  sprintf(heading2, "%s",  dircount[dircount1]);
  lcd.print(heading2);

  lcd.setCursor(9,3);
  //sprintf(heading4, "%s",  (dirfwdcount2?"Fw":"Bw"));
  sprintf(heading4, "%s",  dircount[dircount2]);
  lcd.print(heading4);
#ifdef DEBUG  
  Serial.println("secondrallyMenu");
#endif
}

unsigned long startspeed=0;
float startdistance=0;

void redrawcalibrationMenu()
{
  float distance;
  Calibrationcount = Calibrationcount + (VSSCount-Calibrationtime);
  distance = (float)Calibrationcount/((float)Calibration);
  char cur_distance_str[6];
  byte xtemp=13;
  if (distance<100) {
    sprintf(cur_distance_str, "%02d.%03dKm", (int)distance, (int)((distance - (int)distance)*1000));
    xtemp = 12;
  }
  else if (distance<1000) {
    sprintf(cur_distance_str, "%02d.%02dKm", (int)distance, (int)((distance - (int)distance)*100));
  }
  else {
    sprintf(cur_distance_str, "%03d.%01dKm", (int)distance, (int)((distance - (int)distance)*10));
  }
  Calibrationtime = VSSCount;
  lcd.setCursor(xtemp,2);
  lcd.print(cur_distance_str);
}
void redrawrallyMenu()
{
  DS3231_get(&t); //Get time
  //runningtime(6,0);
  //float distance, distance1, distance2;

  //Serial.print(tempcount1);Serial.print(";"); Serial.print(time1);Serial.print("; ");Serial.print(Calibration); Serial.print(";"); Serial.println(count1);
  updateCount();
  
  //distance = (float)count1/((float)Calibration);
  distance1 = (float)count1/((float)Calibration); // * 1);
  distance2 = (float)count2/((float)Calibration);
  float speed1=0;
  
  if ((distance1 - startdistance) > 0) {
    speed1 = (distance1-startdistance)/(millis()-startspeed)*3600000;
  }

  startdistance = distance1;
  startspeed = millis(); //startspeed is only one no startspeed2
  
  if ((millis()-startsavemillis)>0) {
  
    if (distance1 != prevdistance1) {
      mem.writeFloat(832,distance1);
      prevdistance1 = distance1;
    }
    if (distance2 != prevdistance2) {
      mem.writeFloat(864,distance2);
      prevdistance2 = distance2;
    }
    saveCount();
    startsavemillis=millis()+2500;
  }
  //Serial.print(tempcount1);Serial.print(";"); Serial.print(time1);Serial.print("; ");Serial.print(Calibration); Serial.print(";"); Serial.println(count1);
  
  //Serial.print(millis());Serial.print(";");Serial.println(distance1,3);
  //Serial.print(starttracthour); Serial.print(":"); Serial.print(starttractmin); Serial.print(":"); Serial.println(starttractsec);
  //Serial.println(distance2,3);
  lcd.setCursor(4,0);
  lcd.print(Trip[curTrip].sub);
  lcd.setCursor(0,2);
  //lcd.print(T
  lcd.print(Trip[curTrip].speed);
  lcd.print("Km/H");
  
  runningtime(6,0);
  lcd.setCursor(11,2);
  char cur_speed_str[8];
  sprintf(cur_speed_str, "%3d.%01dKm/h", (int)speed1, (int)((speed1 - (int)speed1)*10));
  lcd.print(cur_speed_str);
  char cur_distance1_str[6];
  char cur_distance2_str[6];
  lcd.setCursor(0,1);
  if (distance1<100) {
    sprintf(cur_distance1_str, "%02d.%03dKm", (int)distance1, (int)((distance1 - (int)distance1)*1000));
  }
  else if (distance1<1000) {
    sprintf(cur_distance1_str, "%02d.%02dKm", (int)distance1, (int)((distance1 - (int)distance1)*100));
  }
  else {
    sprintf(cur_distance1_str, "%03d.%01dKm", (int)distance1, (int)((distance1 - (int)distance1)*10));
  }
  //lcd.print(distance1,3);
  lcd.print(cur_distance1_str);
  lcd.setCursor(0,3);
  if (distance2<100) {
    sprintf(cur_distance2_str, "%02d.%03dKm", (int)distance2, (int)((distance2 - (int)distance2)*1000));
  }
  else if (distance2<1000) {
    sprintf(cur_distance2_str, "%02d.%02dKm", (int)distance2, (int)((distance2 - (int)distance2)*100));
  }
  else {
    sprintf(cur_distance2_str, "%03d.%01dKm", (int)distance2, (int)((distance2 - (int)distance2)*10));
  }
  //lcd.print(distance2,3);
  lcd.print(cur_distance2_str);
  
  float distancetime1 = distance1 * 60 / constanta;
  float distancetime2 = distance2 * 60 / constanta;

//Calculating all about TIME,TDiff(including passing 23:59:59)
  lcd.setCursor(9,1);
  int secs1, mins1, hours1;

  secs1 = (distancetime1-(int)distancetime1)*60;
  mins1 = (int)distancetime1 % 60;
  hours1 = (int)distancetime1 / 60;
  unsigned long totcurrentsecs = t.sec + (t.min*60) + ((long)t.hour*3600);
  unsigned long totsecs1 = secs1 + (mins1*60) + ((long)hours1*3600);
  unsigned long totstarttractsecs = starttractsec + (starttractmin*60) + ((long)starttracthour*3600);
  unsigned long tottime1 = totsecs1 + totstarttractsecs;
  boolean negatifsign = false;

//Serial.print((long)starttracthour*3600); Serial.print(";"); Serial.print(starttractmin*60); Serial.println(";");
//Serial.print(totsecs1); Serial.print(";"); Serial.print(totstarttractsecs); Serial.print(";");
//Serial.print(tottime1);Serial.print(";");Serial.println(totcurrentsecs);
  if (totcurrentsecs<totstarttractsecs) totcurrentsecs+=86400;
  
  if (!showTIME) {
    //if ((long)(tottime1 - totcurrentsecs)>=0) {  //positif means we are too fast, slow down please.
    if (tottime1 >= totcurrentsecs) {  //positif means we are too fast, slow down please.
    //  Serial.println("pos");
      negatifsign = false;
      tottime1 = tottime1 - totcurrentsecs;
    }
    else { //negatif means we need to hurry, its late
    //  Serial.println("neg");
      negatifsign = true;
      tottime1 = totcurrentsecs - tottime1;
    }
  }
  //Serial.print(negatifsign);Serial.print(";"); Serial.println(tottime1); 
  hours1 = ((int)(tottime1 / 3600))%24;
  mins1 = (int)((tottime1 % 3600)/60);
  secs1 = (tottime1 % 60); 

  char cur_time1_str[10];
  if (showTIME) {
    sprintf(cur_time1_str, "%s %02d:%02d:%02d", dircount[dircount1], hours1, mins1, secs1);
  }
  else {
    if (hours1 < 10) sprintf(cur_time1_str, "%s %s%01d:%02d:%02d", dircount[dircount1], (negatifsign?"-":" "), hours1, mins1, secs1);
    else sprintf(cur_time1_str, "%s%s%02d:%02d:%02d", dircount[dircount1], (negatifsign?"-":" "), hours1, mins1, secs1);    
      
  }
  lcd.print(cur_time1_str);

// Print Second TIME
  lcd.setCursor(9,3);
  int secs2, mins2, hours2;

  secs2 = (distancetime2-(int)distancetime2)*60;
  mins2 = (int)distancetime2 % 60;
  hours2 = (int)distancetime2 / 60;

  unsigned long totsecs2 = secs2 + (mins2*60) + ((long)hours2*3600);
  unsigned long tottime2 = totsecs2 + totstarttractsecs;
//Serial.println(tottime2);

  if (!showTIME) {
    if (tottime2 >= totcurrentsecs) {  //positif means we are too fast, slow down please.
      negatifsign = false;
      tottime2 = tottime2 - totcurrentsecs;
    }
    else { //negatif means we need to hurry, its late
      negatifsign = true;
      tottime2 = totcurrentsecs - tottime2 ;
    }
  }
  
  hours2 = ((int)(tottime2 / 3600))%24;
  mins2 = (int)((tottime2 % 3600)/60);
  secs2 = (tottime2 % 60);
  char cur_time2_str[10];
  if (showTIME) {
    sprintf(cur_time2_str, "%s %02d:%02d:%02d", dircount[dircount2], hours2, mins2, secs2);
  }
  else {
    if (hours2<10) sprintf(cur_time2_str, "%s %s%01d:%02d:%02d", dircount[dircount2], (negatifsign?"-":" "), hours2, mins2, secs2);
    else  sprintf(cur_time2_str, "%s%s%02d:%02d:%02d", dircount[dircount2], (negatifsign?"-":" "), hours2, mins2, secs2);
  }
  //sprintf(cur_time2_str, "%s%02d:%02d:%02d", (dirfwdcount2?"Fw":"Bw"), hours2, mins2, secs2);
  //sprintf(cur_time2_str, "%s %02d:%02d:%02d", dircount[dircount2], hours2, mins2, secs2);
  lcd.print(cur_time2_str);
/* //may be needed to refresh if Km more than 10000Km  
  lcd.setCursor(8,3);
  lcd.write(1); //UP Arrow
  
  lcd.setCursor(8,1);
  lcd.write(2); //DOWN Arrow
*/
  lcd.setCursor(x,y);
}

void set_cursor_pos(int new_pos)
{
  /*
  Cursor Positon map:
  ----------------------
  |Count1 TIME    AvgSp|
  |1       35  |7    9 |
  |Count2           KmH|
  |2       46  |8    10|
  ----------------------
  */

  //Need to clear the previous cursor position
  //int x, y;
  switch(CURSOR_POS)
  {
    case 1:
      x = 0;
      y = 1;
      break;
    case 2:
      x = 0;
      y = 3;
      break;
    case 3:
       x = 8; //Km is 7,1
       y = 1;
       break;
    case 4:
       x = 8; //Km is 7,3
       y = 3;
       break;
    case 5:
       x = 9;
       y = 1;
       break;
    case 6:
       x = 9;
       y = 3;
       break;
    case 7:
       x = 13;
       y = 1;
       break;
    case 8:
       x = 13;
       y = 3;
       break;
    case 9:
       x = 18;
       y = 1;
       break;
    case 10:
       x = 18;
       y = 3;
       break;  
  }
  lcd.print(" ");
  lcd.blink();
  lcd.setCursor(8,3);
  lcd.write(1); //UP Arrow
  
  lcd.setCursor(8,1);
  lcd.write(2); //DOWN Arrow
  
  lcd.setCursor(x,y); 

/*
  switch(new_pos)
  {
    case 1:
      lcd.setCursor(6,1);
      //lcd.print(LCD_ARROW_RIGHT);
      break;
    case 2:
      lcd.setCursor(6,1);
      //lcd.print(LCD_ARROW_RIGHT);
      break;
    case 3:
      lcd.setCursor(6,1);
      //lcd.print(LCD_ARROW_DOWN);
      break;
    case 4:
      lcd.setCursor(6,1);
      //lcd.print(LCD_ARROW_DOWN);
      break;
    case 5:
      lcd.setCursor(6,1);
      //lcd.print(LCD_ARROW_DOWN);
      break;
    case 6:
      lcd.setCursor(6,1);
      //lcd.print(LCD_ARROW_DOWN);
      break;
    case 7:
      lcd.setCursor(6,1);
      //lcd.print(LCD_ARROW_LEFT);
      break;
    case 8:
      lcd.setCursor(6,1);
      //lcd.print(LCD_ARROW_LEFT);
      break;
  }
*/  
  
  CURSOR_POS = new_pos;
#ifdef DEBUG
  Serial.print("SetCursorPosition:");
  Serial.println(CURSOR_POS);
#endif
}