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

// Motor configuration
#define STEPS_PER_REV 200 // Adjust based on your motor (e.g., 1.8°/step = 200 steps/rev)
const int STEP_PIN = 9;
const int DIR_PIN = 8;
const int SLEEP_PIN = 10;
AccelStepper stepperMotor(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// LCD configuration
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Button pins
const int buttonIncreaseSpeed = 7;
const int buttonDecreaseSpeed = 6;
const int buttonForward = 5;
const int buttonBackward = 4;

// Variables
float rps = 0.5; // Initial RPS
float minRps = 0.25;
float maxRps = 3;
int direction = 1;     // 1 for forward, -1 for backward

void setup() {
  Serial.begin(9600);
  // Initialize the LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("RPS:");

  // Set button pins as inputs with internal pull-up resistors
  pinMode(buttonIncreaseSpeed, INPUT_PULLUP);
  pinMode(buttonDecreaseSpeed, INPUT_PULLUP);
  pinMode(buttonForward, INPUT_PULLUP);
  pinMode(buttonBackward, INPUT_PULLUP);
  pinMode(SLEEP_PIN, OUTPUT);

  // Set initial motor speed and acceleration
  stepperMotor.setMaxSpeed(1000); // Max steps per second
  stepperMotor.setAcceleration(1000); // Steps per second^2
  updateMotorSpeed();

  // Display initial RPS
  updateLCD();
}

void loop() {
  // Check buttons
  if (digitalRead(buttonIncreaseSpeed) == LOW) {
    increaseSpeed();
  }
  if (digitalRead(buttonDecreaseSpeed) == LOW) {
    decreaseSpeed();
  }
  if (digitalRead(buttonForward) == LOW) {
    direction = 1;
    digitalWrite(SLEEP_PIN, LOW);
    updateMotorSpeed();
  }
  else if (digitalRead(buttonBackward) == LOW) {
    direction = -1;
    digitalWrite(SLEEP_PIN, LOW);
    updateMotorSpeed();
  }
  else {
    direction = 0;
    digitalWrite(SLEEP_PIN, HIGH);
    updateMotorSpeed();
  }

  // Run the motor
  stepperMotor.run();
}

void increaseSpeed() {
  if (rps < maxRps) {
    rps = round((rps + 0.05) * 1000) / 1000.0;  // Add 0.05 and round to 3 decimal places
    rps = constrain(rps, minRps, maxRps);        // Ensure it doesn't exceed maxRps
    updateMotorSpeed();
    updateLCD();
    delay(250);  // Debounce delay
  }
}

void decreaseSpeed() {
  if (rps > minRps) {
    rps = round((rps - 0.05) * 1000) / 1000.0;  // Subtract 0.05 and round to 3 decimal places
    rps = constrain(rps, minRps, maxRps);        // Ensure it doesn't go below minRps
    updateMotorSpeed();
    updateLCD();
    delay(250);  // Debounce delay
  }
}

void updateMotorSpeed() {
  float stepsPerSecond = rps * STEPS_PER_REV;

  stepperMotor.setMaxSpeed(stepsPerSecond);
  stepperMotor.setSpeed(direction * stepsPerSecond);
}

void updateLCD() {
  lcd.setCursor(5, 0);
  lcd.print("     "); // Clear previous RPS value
  lcd.setCursor(5, 0);
  lcd.print(String(rps*20, 0));
  lcd.print(" RPS");
}