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

int DT = 2;   // GPIO #4-DT on encoder (Output B)
int CLK = 3;  // GPIO #5-CLK on encoder (Output A)

volatile float distanceTraveled = 0;
volatile bool stateChanged = false;
unsigned long previousMillis = 0;

// Define LCD dimensions and address
LiquidCrystal_I2C lcd(0x27, 20, 4);  // Set the LCD I2C address and the LCD dimensions (20x4)

void setup() {
  Serial.begin(9600);
  pinMode(CLK, INPUT_PULLUP);
  pinMode(DT, INPUT_PULLUP);
  
  // Initialize LCD
  lcd.init();
  lcd.backlight();

  // Display static text on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("mm/min:");

  lcd.setCursor(0, 1);
  lcd.print("mm:");

  lcd.setCursor(0, 2);
  lcd.print("m^3/hr:");

  lcd.setCursor(0, 3);
  lcd.print("L/min:");

  attachInterrupt(digitalPinToInterrupt(CLK), encoderInterrupt, FALLING);
}

void loop() {
  if (stateChanged) {
    unsigned long currentMillis = millis();
    unsigned long timeDiff = currentMillis - previousMillis;

    float distancePerPulse = 0.8168; // mm
    float speed_mm_min = (distancePerPulse / timeDiff) * 60000;
    float speed_m_hr = speed_mm_min * 60 / 1000.0;

    float deltaFlow = speed_m_hr * PI * 4.041 * 4.041 / 4.0;
    float bentFlow = speed_m_hr * PI * (4.041 * 4.041 - 3.996 * 3.996) / 4.0;
    float bentFlow_L_min = bentFlow * 1000 / 60;

    // // Print to serial
    Serial.print("Speed (mm/min): ");
    Serial.println(speed_mm_min);
    Serial.print("Dist (mm): ");
    Serial.println(distanceTraveled);
    Serial.print("DeltaF (m³/hr): ");
    Serial.println(deltaFlow);
    Serial.print("BentF (L/min): ");
    Serial.println(bentFlow_L_min);

    // Update LCD with new values
    lcd.setCursor(7, 0);
    lcd.print("     ");
    lcd.setCursor(7, 0);
    lcd.print(round(speed_mm_min));

    lcd.setCursor(3, 1);
    lcd.print("     ");
    lcd.setCursor(3, 1);
    lcd.print(round(distanceTraveled));

    lcd.setCursor(7, 2);
    lcd.print("     ");
    lcd.setCursor(7, 2);
    lcd.print(round(deltaFlow));

    lcd.setCursor(6, 3);
    lcd.print("     ");
    lcd.setCursor(6, 3);
    lcd.print(round(bentFlow_L_min));

    previousMillis = currentMillis;
    stateChanged = false;
  }
}

void encoderInterrupt() {
  if (digitalRead(DT) != digitalRead(CLK)) {
    distanceTraveled += 0.8168; // mm half the actual value to fix the doublePulse Bug
  } else {
    distanceTraveled -= 0.8168;
  }
  
  // Reset distance if it exceeds 2500 mm
  if (distanceTraveled >= 2500.0) {
    distanceTraveled = 0;
  }
  
  stateChanged = true;
}