#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <AccelStepper.h>
#include <Arduino.h> // Include Arduino.h for dtostrf function

// Define keypad matrix
const byte ROWS = 4; // four rows
const byte COLS = 4; // four columns
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {7, 6, 5, 4}; // connect to the row pinouts of the keypad
byte colPins[COLS] = {3, 2, 1, 0};   // connect to the column pinouts of the keypad
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

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

// Define Stepper Motor
AccelStepper stepper(AccelStepper::FULL4WIRE, 9, 8); // 2 and 3 are the initial stepper motor pins

// Constants
const float mmPerStep = 0.79; // One step moves 3.15 mm
const char clearKey = 'C';

// Variables
float targetLength = 0;     // Target length in mm
float currentLength = 0;    // Current length in mm
bool isFeeding = false;     // Flag to indicate if the motor is feeding
bool invertedDirection = false; // Flag to indicate if the direction is inverted
bool useAlternatePins = false; // Flag to indicate if alternate pins are used

void setup() {
  lcd.init();                      // initialize the lcd 
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Enter length (mm):");
  // Initialize the LCD with the mode information
  lcd.setCursor(0, 3);
  lcd.print("Normal Mode");
  stepper.setMaxSpeed(1000); // Adjust this to your desired speed
  stepper.setAcceleration(50); // Adjust this to your desired acceleration
}

void loop() {
  char key = keypad.getKey();
  
  if (key != NO_KEY) {
    if (key == clearKey) {
      targetLength = 0;
      currentLength = 0;
      lcd.setCursor(0, 1);
      lcd.print("                "); // Clear the display
      lcd.setCursor(0, 2);
      lcd.print("Current: ");
      lcd.print(currentLength);
      lcd.setCursor(0, 1);
      lcd.print("Cleared");
      delay(1000);
      lcd.setCursor(0, 1);
      lcd.print("                "); // Clear the display
      lcd.setCursor(0, 0);
      lcd.print("Enter length (mm):");
    } else if (isdigit(key)) {
      targetLength = targetLength * 10 + (key - '0');
      lcd.setCursor(0, 1);
      lcd.print("                "); // Clear the display
      lcd.setCursor(0, 1);
      lcd.print(targetLength);
    } else if (key == '#' && !isFeeding) { // Check if not already feeding
      isFeeding = true;
      feedSpool();
    } else if (key == 'D' && isFeeding) { // Check if 'D' key is pressed while feeding
      stepper.stop(); // Stop the motor
      isFeeding = false;
      lcd.setCursor(0, 1);
      lcd.print("                "); // Clear the display
      lcd.setCursor(0, 1);
      lcd.print("Feeding aborted");
      delay(1000);
      lcd.setCursor(0, 1);
      lcd.print("                "); // Clear the display
      lcd.setCursor(0, 0);
      lcd.print("Enter length (mm):");
    } else if (key == '*') { // Toggle the stepper motor pins with 'B' key
      useAlternatePins = !useAlternatePins;
      if (useAlternatePins) {
        stepper = AccelStepper(AccelStepper::FULL4WIRE, 8, 9); // Use alternate pins 2 and 3
      } else {
        stepper = AccelStepper(AccelStepper::FULL4WIRE, 9, 8); // Use default pins 3 and 2
      }
      stepper.setMaxSpeed(1000); // Reconfigure stepper parameters
      stepper.setAcceleration(50);
      lcd.setCursor(0, 3);
      lcd.print("                "); // Clear the display
      lcd.setCursor(0, 3);
      if (useAlternatePins) {
        lcd.print("Reverse Mode");
        invertedDirection = true; // Set direction to inverted
      } else {
        lcd.print("Normal Mode");
        invertedDirection = false; // Set direction to normal
      }
    } else if (key == 'A') { // Start feeding with 'A' key
      if (!isFeeding) {
        isFeeding = true;
        if (invertedDirection) {
          stepper.setSpeed(-1000); // Set a negative speed for inverted direction
        } else {
          stepper.setSpeed(1000); // Set a positive speed for normal direction
        }
      }
    }
  }
}

void feedSpool() {
  int stepsToMove = targetLength / mmPerStep;

  if (stepsToMove > 1) {
    if (invertedDirection) {
      stepsToMove = -stepsToMove; // Invert direction
    }
    stepper.moveTo(stepsToMove); // Use moveTo to set target position
    lcd.setCursor(0, 1);
    lcd.print("                "); // Clear the display
    lcd.setCursor(0, 1);
    lcd.print("Feeding..."); // Display "Feeding..." while the motor is running
    while (stepper.isRunning()) {
      stepper.run();
      if (keypad.getKey() == 'D') { // Check if 'D' key is pressed during feeding
        stepper.stop(); // Stop the motor
        isFeeding = false;
        lcd.setCursor(0, 1);
        lcd.print("                "); // Clear the display
        lcd.setCursor(0, 1);
        lcd.print("Feeding aborted");
        delay(1000);
        lcd.setCursor(0, 1);
        lcd.print("                "); // Clear the display
        lcd.setCursor(0, 0);
        lcd.print("Enter length (mm):");
        return;
      }
    }
    currentLength += abs(stepsToMove) * mmPerStep * (invertedDirection ? -1 : 1); // Use absolute value for positive length and consider direction
  }
  
  lcd.setCursor(0, 2);
  lcd.print("                "); // Clear the display
  lcd.setCursor(0, 2);
  lcd.print("Current: ");
char buffer[10];
dtostrf(currentLength, 4, 1, buffer); // Format currentLength with 1 decimal place
lcd.setCursor(0, 2);
lcd.print("                "); // Clear the display
lcd.setCursor(0, 2);
lcd.print("Current: ");
lcd.print(buffer);
lcd.print(" mm");

  isFeeding = false; // Reset the flag
  stepper.setCurrentPosition(0);
  targetLength = 0;
  lcd.setCursor(0, 1);
  lcd.print("                "); // Clear the display
  lcd.setCursor(0, 1);
    lcd.print(targetLength);
  }