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

// Define stepper motor connections and motor interface type
#define motorInterfaceType 1
#define dirPin1 33
#define stepPin1 25
#define dirPin2 26
#define stepPin2 27

// Create stepper motor objects
AccelStepper stepper1(motorInterfaceType, stepPin1, dirPin1);
AccelStepper stepper2(motorInterfaceType, stepPin2, dirPin2);

// Define keypad pins and layout
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] = {14, 12, 13, 15}; // Connect to the row pinouts of the keypad
byte colPins[COLS] = {16, 17, 18, 19}; // Connect to the column pinouts of the keypad

// Create keypad object
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

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

// Variables to store user input
int wireLength = 12;
int wireQuantity = 5;
int currentPos = 0; // Variable to store current position of the feeding motor

// Create Preferences object for non-volatile storage
Preferences preferences;

enum State {
  ENTER_LENGTH,
  ENTER_QUANTITY,
  CONFIRMATION,
  ARE_YOU_SURE,
  CUTTING,
  RESTART,
  ERROR_LENGTH,
  ERROR_QUANTITY
};

State state = ENTER_LENGTH;

void setup() {
  // Initialize the LCD
  lcd.init();
  lcd.backlight();

  // Set the maximum speed and acceleration for the stepper motors
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(500);
  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(500);

  // Initialize Preferences and retrieve last position
  preferences.begin("wirecutter", false);
  currentPos = preferences.getInt("lastPos", 0);
  stepper1.setCurrentPosition(currentPos); // Set the initial position of the feeding motor

  // Display initial messages
  lcd.setCursor(0, 0);
  lcd.print("Enter Length:");
  lcd.setCursor(0, 1);
  lcd.print("0");

  Serial.begin(115200);
}

void loop() {
  char key = keypad.getKey();
  handleKeypad(key);
}

void handleKeypad(char key) {
  if (key) {
    switch (state) {
      case ENTER_LENGTH:
        if (key >= '0' && key <= '9') {
          wireLength = wireLength * 10 + (key - '0');
          lcd.setCursor(0, 1);
          lcd.print(wireLength);
        } else if (key == '#') {
          if (wireLength == 0) {
            state = ERROR_LENGTH;
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Error: Length 0");
            delay(2000); // Display error message for 2 seconds
            state = ENTER_LENGTH;
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Enter Length:");
            lcd.setCursor(0, 1);
            lcd.print(wireLength);
          } else {
            state = ENTER_QUANTITY;
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Enter Quantity:");
            lcd.setCursor(0, 1);
            lcd.print("0");
          }
        } else if (key == 'A') {
          wireLength /= 10;
          lcd.setCursor(0, 1);
          lcd.print("                "); // Clear the line
          lcd.setCursor(0, 1);
          lcd.print(wireLength);
        }
        break;

      case ENTER_QUANTITY:
        if (key >= '0' && key <= '9') {
          wireQuantity = wireQuantity * 10 + (key - '0');
          lcd.setCursor(0, 1);
          lcd.print(wireQuantity);
        } else if (key == '#') {
          if (wireQuantity == 0) {
            state = ERROR_QUANTITY;
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Error: Quantity 0");
            delay(2000); // Display error message for 2 seconds
            state = ENTER_QUANTITY;
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Enter Quantity:");
            lcd.setCursor(0, 1);
            lcd.print(wireQuantity);
          } else {
            state = CONFIRMATION;
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Length:");
            lcd.print(wireLength);
            lcd.setCursor(0, 1);
            lcd.print("Quantity:");
            lcd.print(wireQuantity);
            delay(2000); // Wait 2 seconds before showing the confirmation step
            state = ARE_YOU_SURE;
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Are you sure?");
            lcd.setCursor(0, 1);
            lcd.print("A: Yes  *: No");
          }
        } else if (key == '*') {
          state = ENTER_LENGTH;
          wireQuantity = 0; // Reset wire quantity
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Enter Length:");
          lcd.setCursor(0, 1);
          lcd.print(wireLength);
        } else if (key == 'A') {
          wireQuantity /= 10;
          lcd.setCursor(0, 1);
          lcd.print("                "); // Clear the line
          lcd.setCursor(0, 1);
          lcd.print(wireQuantity);
        }
        break;

      case ARE_YOU_SURE:
        if (key == 'A') {
          state = CUTTING;
          startCutting();
        } else if (key == '*') {
          state = ENTER_QUANTITY;
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Enter Quantity:");
          lcd.setCursor(0, 1);
          lcd.print(wireQuantity);
        }
        break;

      case RESTART:
        if (key == '#') {
          state = ENTER_LENGTH;
          wireLength = 0;
          wireQuantity = 0;
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Enter Length:");
          lcd.setCursor(0, 1);
          lcd.print("0");
        } else if (key == '*') {
          state = ENTER_QUANTITY;
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Enter Quantity:");
          lcd.setCursor(0, 1);
          lcd.print(wireQuantity);
        }
        break;
    }
  }
}

void startCutting() {
  for (int i = 0; i < wireQuantity; i++) {
    // Display current cutting progress
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Cutting:");
    lcd.print(i + 1);
    lcd.print("/");
    lcd.print(wireQuantity);

    // Move the wire forward (stepper1 controls the feeding mechanism)
    currentPos += wireLength * 10; // Increment position for each piece
    stepper1.moveTo(currentPos);
    while (stepper1.distanceToGo() != 0) {
      stepper1.run();
    }
    delay(500); // Wait for the wire to stabilize

    // Cut the wire (stepper2 controls the cutting mechanism)
    stepper2.moveTo(200); // Adjust the position based on your cutter
    while (stepper2.distanceToGo() != 0) {
      stepper2.run();
    }
    delay(500); // Wait for the cut to complete
    stepper2.moveTo(0); // Return to the original position
    while (stepper2.distanceToGo() != 0) {
      stepper2.run();
    }
    delay(500); // Wait before the next feed
  }

  // Store the last position in Preferences
  preferences.putInt("lastPos", currentPos);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Done!");
  delay(2000); // Wait 2 seconds before allowing new input
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Restart: #");
  lcd.setCursor(0, 1);
  lcd.print("Back: *");

  state = RESTART;
}