#include <Encoder.h>
#include <Wire.h> // Include the Wire library for I2C communication
#include <LiquidCrystal_I2C.h>

// Define the pins for the rotary encoder
#define encoderPinA 2
#define encoderPinB 3

Encoder encoder(encoderPinA, encoderPinB);

float angularDisplacement = 0.0;

// Define the I2C LCD address
int lcdAddress = 0x27; // Use your specific address if different

// Create an instance of the LiquidCrystal_I2C library
LiquidCrystal_I2C lcd(lcdAddress, 16, 2); // 16 columns and 2 rows

void setup() {
  Serial.begin(9600);
  lcd.init(); // Initialize the LCD
  lcd.backlight(); // Turn on the backlight
}

void loop() {
  // Variables to store the previous and current encoder positions
  static long prevPosition = 0; // Static to retain value between loop iterations
  long currentPosition = encoder.read();

  // Variables to calculate time and angular displacement
  static unsigned long prevTime = 0; // Static to retain value between loop iterations

  //Variables to calculate angular speed
  float angularSpeed = 0.0;

  // Calculate the time elapsed since the last measurement
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - prevTime) / 1000.0; // Convert milliseconds to seconds

  // Calculate angular displacement (in degrees)
  float angularDisplacement = (currentPosition - prevPosition) * 360.0 / 80.0; // 20 pulses per revolution

  // Calculate angular speed (in degrees per second)
  angularSpeed = angularDisplacement / deltaTime;

  // Update the previous position and time
  prevPosition = currentPosition;
  prevTime = currentTime;

  // Print the results to the Serial Monitor
  Serial.print("Angular Speed (degrees/s): ");
  Serial.print(angularSpeed, 2);
  Serial.print("\tAngular Displacement (degrees): ");
  Serial.println(angularDisplacement, 2);

  // Clear the LCD and display the results
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Speed: ");
  lcd.print(angularSpeed, 2);
  lcd.print(" deg/s");
  lcd.setCursor(0, 1);
  lcd.print("Disp.: ");
  lcd.print(angularDisplacement, 2);
  lcd.print(" deg");

  delay(5000); // Adjust the delay time as needed for your application
}