#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

//  OLED Display
#define SCREEN_WIDTH 128                         
#define SCREEN_HEIGHT 64                          
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

//  Proximity sensor
const byte proximityPin = 2;
volatile bool sensorTriggered = false;
void onSensorTriggered(){
  sensorTriggered = true;
}

//  Wheel dimensions
const float wheelDiameter = 0.496;                // mm
const float wheelCircumference = wheelDiameter * PI;

//  Time intervals for acceleration and velocity calculation
const float accelInterval = 0.1;
const float velocityInterval = 1.0;

//  Variable for calculation
unsigned long previousMillis = 0;
unsigned long previousAccelMillis = 0;
unsigned long previousVelocityMillis = 0;
float wheelRPM;
float acceleration = 0.0;
float velocity = 0.0;

/* speedCalc Variable Declare */
/* const float diameterWheel = 496;                       //in mm
double velocity;                                  //store speed in km/h value
double timeWheel_sec;                             //store second value of the interval in which the magnet does a full roation`
double timeWheel_min;                             //store minute value of the interval in which the magnet does a full roation
double rpmWheel;                                  //store rpm wheel value
int speedoValue;                                  //store rounded value of speedo

double prevHallBan = 0;                           //proxy pin
float pi = 3.14159;                               //pi value
int hour = 3600;                                  //store hour value */



/* speedo Variable Declare */
unsigned long Laps[20];                           //store lap array capacity
int i = 0;

int upButt = 5;                                   //up button pin
bool upPressed;
bool up = false;

int downButt = 7;                                  //down button pin
bool downPressed;
bool down = false;

int currentLap = 0;
int totalLap = 0;

int startButt = 6;                                 //start button pin
bool startPressed;
bool started = false;

bool printed = false;

unsigned long currentTime = 0;                    //store current time value
uint32_t timer = 0;                          //store timer
unsigned long diffTime;                           //store difference between prev and current lap time
unsigned long oledTime_real = 0;
int intMinutes_real = 0;
int intSeconds_real = 0;

float floatMinutes_real;
float floatSeconds_real;

unsigned long milliSecondsInSecond = 1000;        //store millisecond value of seconds
unsigned long milliSecondsInMinute = 60000;       //store millisecond value of minutes

String nowTime;

void setup(){
  Serial.begin(9600);
  //  Initialize proximity sensor interrupt
  pinMode(proximityPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(proximityPin), onSensorTriggered, RISING);
  // attachInterrupt(0, speedCalc, RISING);         //digital pin 2, void name "speedCalc", change voide during rising impulse

  //  Set initial time values
  previousMillis = millis();
  previousAccelMillis = millis();
  previousVelocityMillis = millis();

  pinMode(upButt, INPUT_PULLUP);
  pinMode(startButt, INPUT_PULLUP);
  pinMode(downButt, INPUT_PULLUP);

  // Initialize OLED display with address 0x3C for 128x64
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (true);
  }

  delay(2000);                                    // wait for initializing
  oled.clearDisplay();                            // clear display
}

void loop(){  
  if(sensorTriggered){
    //  Calculate wheel RPM
    unsigned long nowMillis = millis();
    float elapsedSeconds = (nowMillis - previousMillis) / 1000.0;
    wheelRPM = 60.0 / (elapsedSeconds * wheelCircumference);
    previousMillis = nowMillis;

    //  Calculate acceleration
    float elapsedAccelSeconds = (nowMillis - previousAccelMillis) / 1000.0;
    if(elapsedAccelSeconds >= accelInterval){
      acceleration = wheelRPM / elapsedAccelSeconds;
      previousAccelMillis = nowMillis;
    }

    float elapsedVelocitySeconds = (nowMillis - previousVelocityMillis) / 1000.0;
    if(elapsedVelocitySeconds >= velocityInterval){
      velocity += (acceleration * elapsedVelocitySeconds);
      previousVelocityMillis = nowMillis;
    }

    //  Reset sensor trigger flag
    sensorTriggered = false;
  }

  upPressed = digitalRead(upButt);
  downPressed = digitalRead(downButt);
  startPressed = digitalRead(startButt);

  if(!started && !printed){
    lapSelect();
  }

  
  if(!startPressed && !printed){
    if(!started){
      started = true;
      Serial.println("\n3");
      oled.setTextSize(6);
      oled.setTextColor(WHITE);
      oled.setCursor(55, 10);
      oled.println("3");
      oled.display();
      oled.clearDisplay();
      delay(1000);
      Serial.println("2");
      oled.setTextSize(6);
      oled.setTextColor(WHITE);
      oled.setCursor(55, 10);
      oled.println("2");
      oled.display();
      oled.clearDisplay();
      delay(1000);
      Serial.println("1");
      oled.setTextSize(6);
      oled.setTextColor(WHITE);
      oled.setCursor(55, 10);
      oled.println("1");
      oled.display();
      oled.clearDisplay();
      delay(1000);
      Serial.println("Start!");
      oled.setTextSize(3);
      oled.setTextColor(WHITE);
      oled.setCursor(20, 8);
      oled.println("Start!");
      oled.display();
      oled.clearDisplay();
      
      currentTime = millis();
      delay(50);
      startPressed = true;
    }else if(started){
      upPressed = true;
      downPressed = true;
      if(totalLap == i && !startPressed){
        started = false;
        startPressed = true;
        printed = true;
        i = 0;
      }else if(!startPressed){
        timer = millis() - currentTime;
        Laps[i] = timer;
        diffTime = Laps [i] - Laps[i-1];
        i++;   
        delay(50);
        startPressed = true;
      }
      delay(50);
    }
  }

  if(printed){
    oled.clearDisplay();
    for (int i = 1; i <= totalLap; i++) {
      oled.setCursor(0, 0 + (i * 20));
      oled.setTextSize(0.5);
      oled.print("Lap ");
      oled.print(i);
      oled.print(": ");
      oled.print((Laps[i - 1]) / 1000);
      oled.print(".");
      oled.print((Laps[i - 1] % 1000) / 10);
      oled.print("s");
      oled.display();
    }
    if(!startPressed){
      printed = false;
      delay(50);
      startPressed = true;
    }
  }

  if(!printed && started == true){
    /* Print in Serial Monitor */
    Serial.print("\t");Serial.print(i);

    // Display mech results on OLED display
    oled.setTextSize(1);
    oled.setCursor(0, 0);
    //  oled.print("Wheel RPM: ");
    oled.print(wheelRPM);
    oled.setCursor(50, 0);
    //  oled.print("Acceleration: ");
    oled.print(acceleration);
    oled.setCursor(100, 0);
    //  oled.print("Velocity: ");
    oled.print(velocity);
    oled.display();

    oledTime_real = millis() - currentTime;
    intSeconds_real = oledTime_real / 1000;
    intMinutes_real = intSeconds_real / 60;
    unsigned long elapsedHours = intMinutes_real / 60;
    intSeconds_real = intSeconds_real % 60;
    intMinutes_real = intMinutes_real % 60;
    elapsedHours = elapsedHours % 24;
    oled.setCursor(0, 20);
    oled.setTextSize(2);
    oled.print(elapsedHours);
    oled.print(":");
    if (intMinutes_real < 10) {
      oled.print("0");
    }
    oled.print(intMinutes_real);
    oled.print(":");
    if (intSeconds_real < 10) {
      oled.print("0");
    }
    oled.print(intSeconds_real);
    oled.setCursor(60, 40);
    oled.print(i);
    oled.display();
    oled.clearDisplay();

    // if(oledTime_real >= 1000){
    //   floatSeconds_real = oledTime_real / milliSecondsInSecond % 60;
    //   intSeconds_real = floatSeconds_real;

      
    //   floatMinutes_real = oledTime_real / milliSecondsInMinute % 60;
    //   intMinutes_real = floatMinutes_real;
    // }
    // if (intMinutes_real < 10){
    //   oled.setTextSize(3);
    //   oled.setTextColor(WHITE);
    //   oled.setCursor(45, 0);
    //   oled.println("\t0");
    // }
    // oled.print(intMinutes_real);
    // oled.print(":");

    // if (intSeconds_real < 10) {
    //   oled.print("0");
    // }
    // oled.print(intMinutes_real);
    // oled.print(":");  
    // oled.print(intSeconds_real);
    // oled.display();
    // oled.clearDisplay();
    computeHMS(millis() - currentTime);

    // nowTime = String(computeHMS(millis() - currentTime));
    // oled.setTextSize(1);
    // oled.setTextColor(WHITE);
    // oled.setCursor(32, 0);
    // oled.println(nowTime);
    // oled.display();
    computeHMS(diffTime);
    
    Serial.println("\n------");
  }

  // if(printed == true && startPressed == false){
  //   printed == false;
  //   totalLap = 0;
  //   oled.clearDisplay();
  //   oled.display();
  //   lapSelect();
  //   delay(25);
  // }


  // if(currentTime - prevHallBan < 3000){           //if hall sensor recieves impulse for less than 3 seconds, the display will show the corresponding speed value
  //     Serial.print("\t");Serial.print(speedoValue);Serial.print(" ");  
  //   }else if(currentTime - prevHallBan>= 3000){     //if hall sensor does not receive impulse for more than 3 seconds, the display will show 0
  //     Serial.print(" ");Serial.print("\t0");Serial.print(" ");
  //   }
  
}

void lapSelect(){
  /* Selecting lap */
  
  if(upPressed == false){
    totalLap++;
    Serial.print("\tTotal Lap: ");Serial.println(totalLap);
    delay(50);
  }

  
  if(downPressed == false){
    totalLap--;
    Serial.print("\tTotal Lap: ");Serial.println(totalLap);
    delay(50);
  }

  oled.setTextSize(6);
  oled.setTextColor(WHITE);
  oled.setCursor(49, 15);
  oled.println(totalLap);
  oled.setTextSize(1);
  oled.setTextColor(WHITE);
  oled.setCursor(32, 0);
  oled.println("CHOOSE LAP:");
  oled.display();
  delay(25);
  oled.clearDisplay();
  delay(25);
}

/* // Speedometer function
void speedCalc() {
  timeWheel_sec = (millis() - prevHallBan) / 4000;              // one period rev in second 
  prevHallBan = millis();                                       // declare new prevTime
  timeWheel_min = timeWheel_sec / 60;                           // one period rev in minute
  rpmWheel = 1/timeWheel_min;                                   // rpm = 1/T
  velocity = pi * diameterWheel * rpmWheel * 0.06;              // velocity in km/h = pi * d * rpm * 0.06
  speedoValue = round(velocity);


  oled.setTextSize(6);
  oled.setTextColor(WHITE);
  oled.setCursor(00, 15);
  oled.println(speedoValue);
  oled.display();
  delay(25);
  oled.clearDisplay();
  delay(25);
} */

// Convert to HH::MM::SS format
void computeHMS(unsigned long duration) {
  int intMinutes;
  int intSeconds;

  float floatMinutes;
  float floatSeconds;

  intMinutes = 0;
  intSeconds = 0;

  if (duration >= 1000) {
      floatSeconds = duration / milliSecondsInSecond % 60;
      intSeconds = floatSeconds;

      
      floatMinutes = duration / milliSecondsInMinute % 60;
      intMinutes = floatMinutes;

  }
  if (intMinutes < 10) {
    Serial.print("\t0");
  }
  Serial.print(intMinutes);
  Serial.print(":");
  if (intSeconds < 10) {
    Serial.print("0");
  }
  Serial.print(intMinutes);
  Serial.print(":");  
  Serial.print(intSeconds);
} 

void totalDisplay(){          //protocol for displaying total elapsed time per lap and total time
  Serial.print("-------------------------------------\n");
      for(int j = 0; j < i; j++){
        Serial.print("Lap"); Serial.print(j+1);Serial.print(":");
        computeHMS(Laps[j]);
        Serial.println(" ");
        printed = true;
        started = false;
      }

      Serial.print("-------------------------------------\n");
      for(int j = 0; j < i; j++){
        totalLap += Laps[j];
        currentLap = i;
      }
      
}