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

const float wheelDiameter = 31.585;  // in millimeters
const int encoderResolution = 100;    // pulses per revolution
const int encoderPinA = 2;           // replace with your actual pin
const int encoderPinB = 3;           // replace with your actual pin
const int resetButtonPin = 4;        // replace with your actual pin for the reset button
const int encoderButtonPin = 5;      // replace with your actual pin for the encoder button
const int encoderButtonLedPin = 10;  // replace with your actual pin for the encoder button LED
const int ledPin = 13;               // replace with your actual pin for the primary LED

float primaryDistance = 0.0;
float circumference;
volatile long encoderPulses = 20;
volatile long prevEncoderPulses = 20;
volatile bool resetRequested = false;

Encoder encoderDistance(6, 7);       // replace with your actual pin for encoder A and B for distance
Encoder encoderSet(8, 9);            // replace with your actual pin for encoder A and B for set

bool inputMode = false;
float secondaryDistance = 0.0;
bool lockSecondaryDistance = false;

LiquidCrystal_I2C lcd(0x27, 16, 2);  // I2C address 0x27, 16 columns and 2 rows

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  lcd.begin(16, 2);
  // lcd.print("Distance:");  // Commented out this line
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  pinMode(resetButtonPin, INPUT_PULLUP);
  pinMode(encoderButtonPin, INPUT_PULLUP);
  pinMode(encoderButtonLedPin, OUTPUT);
  pinMode(ledPin, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(encoderPinA), handleEncoder, RISING);
}

void loop() {
  // put your main code here, to run repeatedly:
  // Check if reset button is pressed
  if (digitalRead(resetButtonPin) == LOW) {
    resetDistance();
  }

  // Check encoder button for input mode
  checkEncoderButton();

  // Calculate circumference
  circumference = PI * wheelDiameter;

  // Read and update primary distance based on the direction of rotation
  int encoderDistanceValue = encoderDistance.read();
  updateDistance(encoderDistanceValue);

  // Read and update secondary distance based on the direction of rotation
  int encoderSetValue = encoderSet.read();
  updateSecondaryDistance(encoderSetValue);

  // Print primary distance with two decimal places on Serial
  Serial.print("Primary Distance: ");
  Serial.print(primaryDistance, 2); // 2 decimal places
  Serial.print(" meters\t");

  // Print secondary distance with two decimal places on Serial
  Serial.print("Secondary Distance: ");
  Serial.print(secondaryDistance, 2); // 2 decimal places
  Serial.println(" meters");

  // Display primary distance on LCD
  lcd.setCursor(0, 1);
  lcd.print("Dist 2:");
  lcd.setCursor(8, 1);
  lcd.print("      ");  // Clear the previous distance
  lcd.setCursor(8, 1);
  lcd.print(primaryDistance, 2);    // Display primary distance with two decimal places
  lcd.setCursor(15, 1);
  lcd.print("m");

  // Display secondary distance on LCD
  lcd.setCursor(0, 0);
  lcd.print("Dist 1:");
  lcd.setCursor(8, 0);
  lcd.print("      ");  // Clear the previous distance
  lcd.setCursor(8, 0);
  lcd.print(secondaryDistance, 2);    // Display secondary distance with two decimal places
  lcd.setCursor(15, 0);
  lcd.print("m");

  // Turn on LED if primary distance is equal to or greater than secondary distance
  if ((primaryDistance >= secondaryDistance) && (primaryDistance != 0 || secondaryDistance != 0)) {
    digitalWrite(ledPin, HIGH);
  } else {
    digitalWrite(ledPin, LOW);
  }

  // Control encoder button LED
  digitalWrite(encoderButtonLedPin, inputMode);

  delay(100);  // Adjust delay as needed
}

void handleEncoder() {
  encoderPulses++;
}

void resetDistance() {
  encoderPulses = 0;
  prevEncoderPulses = 0;
  lcd.setCursor(0, 1);
  lcd.print("      ");  // Clear the previous primary distance
  lcd.setCursor(0, 1);
  lcd.print("0.00");         // Display zero primary distance on LCD

  if (inputMode) {
    secondaryDistance = 0.0;
    lcd.setCursor(0, 0);
    lcd.print("                  ");  // Clear the previous secondary distance
    lcd.setCursor(0, 0);
    lcd.print("0.00");         // Display zero secondary distance on LCD
  }

  digitalWrite(ledPin, LOW);  // Turn off primary LED
  delay(1000);  // Adjust delay as needed to debounce the button
}

void checkEncoderButton() {
  static unsigned long lastButtonPress = 0;
  static bool buttonStatePrev = HIGH;

  bool buttonState = digitalRead(encoderButtonPin);

  if (buttonState != buttonStatePrev) {
    if (buttonState == LOW) {
      // Encoder button pressed
      if (millis() - lastButtonPress > 50) {  // debounce
        lastButtonPress = millis();

        if (!inputMode) {
          // Enter input mode and reset secondary distance
          inputMode = true;
          secondaryDistance = 0.0;
        } else {
          // Lock secondary distance and exit input mode
          inputMode = false;
          lockSecondaryDistance = true;
        }
      }
    }
  }

  buttonStatePrev = buttonState;
}

void updateDistance(int value) {
  // Determine the direction of rotation
  int direction = (encoderPulses - prevEncoderPulses > 0) ? 1 : -1;

  if (direction == 1) {
    // Clockwise rotation, increase distance
    primaryDistance += value * 0.01;  // Assuming each click changes the distance by 0.01 meters
  } else if (direction == -1) {
    // Counterclockwise rotation, decrease distance
    primaryDistance -= value * 0.01;  // Assuming each click changes the distance by 0.01 meters
    primaryDistance = max(0.0, primaryDistance);  // Ensure distance is not less than zero
  }

  // Update the previous encoder pulses
  prevEncoderPulses = encoderPulses;
}

void updateSecondaryDistance(int value) {
  // Determine the direction of rotation
  int direction = (encoderPulses - prevEncoderPulses > 0) ? 1 : -1;

  if (direction == 1) {
    // Clockwise rotation, decrease secondary distance
    secondaryDistance += value * 0.01;  // Assuming each click changes the distance by 0.01 meters
  } else if (direction == -1) {
    // Counterclockwise rotation, increase secondary distance
    secondaryDistance -= value * 0.01;  // Assuming each click changes the distance by 0.01 meters
    secondaryDistance = max(0.0, secondaryDistance);  // Ensure distance is not less than zero
  }

  // Update the previous encoder pulses
  prevEncoderPulses = encoderPulses;
}