#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);

// Variables to store the previous and current encoder positions
long prevPosition = 0;
long currentPosition = 0;

// Variables to calculate time and angular displacement
unsigned long prevTime = 0;
float angularDisplacement = 0.0;

// Variables to calculate angular speed
float angularSpeed = 0.0;
long prevDisplacement = 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() {
  // Read the current encoder position
  currentPosition = encoder.read();

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

  float time = deltaTime + 1.77;

  // Calculate angular displacement
  angularDisplacement += (currentPosition - prevPosition) * 360.0 / 80.0;

  // Calculate angular speed (in degrees per second)
  angularSpeed = (angularDisplacement - prevDisplacement) / time;

  // Update the previous position, time, and displacement
  prevPosition = currentPosition;
  prevTime = currentTime;
  prevDisplacement = angularDisplacement;

  // Print the results to the Serial Monitor
  Serial.print("Angular Speed (degrees/s): ");
  Serial.print(angularSpeed, 2);
  Serial.print("\tAngular Displ. (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(500); // Adjust the delay time as needed for your application

}