#include <LiquidCrystal_I2C.h>

#define ECHO_PIN 2
#define TRIG_PIN 3

#define I2C_ADDR    0x27
#define LCD_COLUMNS 16
#define LCD_LINES   2
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

#define INTERVAL_DISPLAY 20000UL
#define INTERVAL_MEASURE 500000UL

#define DEBOUNCE_PERIOD 10UL  //  Debounce Delay 10 milliseconds
typedef struct {
  private:
    bool input;
    unsigned long startTime;

  public:
    bool state;

    bool debounce(bool bounce) {
      unsigned long currentTime = millis();
      bool prevState = state;
      if (bounce) {
        state = true;
      } else {
        if (state) {
          if (input) {
            startTime = currentTime;
          }
          unsigned long elapsedTime = currentTime - startTime;
          if (elapsedTime >= DEBOUNCE_PERIOD) {
            state = false;
          }
        }
      }
      input = bounce;
      return state != prevState & state == true;
    }
} debounce_t;

float distanceA;
float distanceB;
float speed;
unsigned long deltaTime;

debounce_t button;

unsigned long readStartTime;
unsigned long inteStartTime;

void setup() {
  Serial.begin(115200);
  lcd.init();
  lcd.backlight();

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  TitleMessage();
  SpeedMessage();
}

void loop() {
  SpeedDetector();
  // 3. 6 = (60 minutes * 60 seconds / 1000 meters)
  speed = abs(distanceB - distanceA) * 3.6f / (deltaTime * 0.000001f);
  SpeedMessage();

}

void SpeedDetector() {
  static uint8_t index;
  static unsigned long startTime;
  unsigned long currTime = micros();
  if (currTime - startTime > INTERVAL_MEASURE) {
    deltaTime = currTime - startTime;
    startTime = currTime;
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(5);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    unsigned long duration = pulseIn(ECHO_PIN, HIGH);
    distanceB = distanceA;
    distanceA = (duration / 58.0f);
    // distanceB = 0;
    // distanceA = 50;
  }
}

void TitleMessage() {
  lcd.setCursor(0, 0);
  lcd.print("D1:");
  lcd.setCursor(8, 0);
  lcd.print("D2:");
  lcd.setCursor(0, 1);
  lcd.print("SPEED:");
}
void SpeedMessage() {
  static unsigned long startTime;
  unsigned long currTime = micros();
  if (currTime - startTime >= INTERVAL_DISPLAY) {
    startTime = currTime;
    lcd.setCursor(3, 0);
    LT(distanceA, 10.0f);
    lcd.print(String(distanceA, 0));
    lcd.setCursor(11, 0);
    lcd.print(String(distanceB, 0));
    lcd.setCursor(7, 1);
    LT(speed, 10.0f);
    lcd.print(String(speed, 0));
  }
}

void LT(float a, float b) {
  if ( a < b) {
    lcd.print(' ');
  }
}