#include <AccelStepper.h>
#include <Keypad.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>  // Include the EEPROM  library

// Define stepper motor control pins
#define DIR_PIN A1
#define STEP_PIN A0
#define DIR_PIN1 A3
#define STEP_PIN1 A2
#define MOTOR_STEPS 200
#define HOME_SWITCH_PIN 2  // Pin connected to the homing switch
#define HOME_SWITCH_PIN1 3

// EEPROM address for StepPermm
#define EEPROM_SIGNATURE_ADDR 0  // Address for the signature byte
#define EEPROM_STEPPERMM_ADDR 1  // Address where StepPermm is stored
#define EEPROM_SPEED_ADDR 5      // Example address for storing speed
#define EEPROM_SOFTSTOP_ADDR 10  //Adress for storing soft stop
#define EEPROM_TotalStepsMM_ADDR 15 // Adress for storing TotalSteps
//const byte EEPROM_SIGNATURE = 0xA5; // Any non-zero value to indicate EEPROM has been initialized

// Create stepper motor driver instances
AccelStepper stepper(1, STEP_PIN, DIR_PIN);
AccelStepper stepper1(1, STEP_PIN1, DIR_PIN1);

// LCD and Keypad Setup
LiquidCrystal_I2C lcd(0x27, 20, 4);

int FlagTotalSteps = 0;
int FlagStepsPermm = 0;
int FlagSpeed = 0;
int FlagSoftStop = 0;
int FlagTotalStepsMM = 0;
int FlagTrimLoffset = 0;
String result = "";
int TotalSteps ;
int TrimLoffset = 0;
int TotalStepsMAX = 570;             // Total MAXIMUM distance measured on the moving axis
int SoftStop = 20;                   // SoftStop in the front of the ruler
long StepPermm = 10;                 // Default value for steps per millimeter
int currentPosition = 0;             // Track the current position of the stepper
int stepperSpeed = 1500;             // Default speed
const byte EEPROM_SIGNATURE = 0xA5;  // Signature byte to indicate EEPROM initialization

void loadStepPermmFromEEPROM() {
  byte signature = EEPROM.read(EEPROM_SIGNATURE_ADDR);

  if (signature == EEPROM_SIGNATURE) {
    EEPROM.get(EEPROM_STEPPERMM_ADDR, StepPermm);
  } else {
    // No valid data in EEPROM, use default and save it
    EEPROM.put(EEPROM_STEPPERMM_ADDR, StepPermm);
    EEPROM.write(EEPROM_SIGNATURE_ADDR, EEPROM_SIGNATURE);
  }
}

void loadSpeedFromEEPROM() {
  byte signature = EEPROM.read(EEPROM_SIGNATURE_ADDR);

  if (signature == EEPROM_SIGNATURE) {
    EEPROM.get(EEPROM_SPEED_ADDR, stepperSpeed);
  } else {
    // No valid data in EEPROM, use default and save it
    EEPROM.put(EEPROM_SPEED_ADDR, stepperSpeed);
    EEPROM.write(EEPROM_SIGNATURE_ADDR, EEPROM_SIGNATURE);
  }
}

void loadSoftStopFromEEPROM() {
  byte signature = EEPROM.read(EEPROM_SIGNATURE_ADDR);

  if (signature == EEPROM_SIGNATURE) {
    EEPROM.get(EEPROM_SOFTSTOP_ADDR, SoftStop);
  } else {
    // No valid data in EEPROM, use default and save it
    EEPROM.put(EEPROM_SOFTSTOP_ADDR, SoftStop);
    EEPROM.write(EEPROM_SIGNATURE_ADDR, EEPROM_SIGNATURE);
  }
}

void loadTotalStepsMMFromEEPROM() {
  byte signature = EEPROM.read(EEPROM_SIGNATURE_ADDR);

  if (signature == EEPROM_SIGNATURE) {
    EEPROM.get(EEPROM_TotalStepsMM_ADDR, TotalStepsMAX);
  } else {
    // No valid data in EEPROM, use default and save it
    EEPROM.put(EEPROM_TotalStepsMM_ADDR, TotalStepsMAX);
    EEPROM.write(EEPROM_SIGNATURE_ADDR, EEPROM_SIGNATURE);
  }
}

void updateStepPermm(long newValue) {
  StepPermm = newValue;
  EEPROM.put(EEPROM_STEPPERMM_ADDR, StepPermm);
}

void updateSpeedToEEPROM(long newValue) {
  stepperSpeed = newValue;
  EEPROM.put(EEPROM_SPEED_ADDR, stepperSpeed);
}

void updateSoftStop(long newValue) {
  SoftStop = newValue;
  EEPROM.put(EEPROM_SOFTSTOP_ADDR, SoftStop);
}

void updateTotalStepsMM(long newValue) {
  TotalStepsMAX = newValue;
  EEPROM.put(EEPROM_TotalStepsMM_ADDR, TotalStepsMAX);
}

// Keypad Configuration
unsigned long lastKeyPressTime = 0;
const unsigned long debounceDelay = 50;  // 50 milliseconds debounce delay
char lastKey = NO_KEY;
const byte ROWS = 4;
const byte COLS = 4;
//byte rowPins[ROWS] = { 12, 11, 10, 9 };
//byte colPins[COLS] = { 8, 7, 6, 5 };
byte rowPins[ROWS] = {8, 7, 6, 5}; // Actual on arduino
byte colPins[COLS] = {12, 11, 10, 9}; // Actual on arduino
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '.', '0', '#', 'D' }
};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Menu Variables
String menuItems[] = { "Auto", "Reset", "Tax", "Stp/mm", "Softstop", "MaxMM", "CurPos", "TrimLft" };
int currentItem = 0;
int selectedItem = 0;
int menuLength = sizeof(menuItems) / sizeof(menuItems[0]);
bool totalStepsUpdated = false;   // Flag to check if TotalSteps needs to be updated
bool displayNeedsUpdate = false;  // Flag to check if the display needs to be updated
bool motorsMoving = false;        // Flag to track motor movement

void setup() {
  Serial.begin(9600);
  pinMode(HOME_SWITCH_PIN, INPUT_PULLUP);  // Set homing switch pin as input with pull-up resistor
  pinMode(HOME_SWITCH_PIN1, INPUT_PULLUP);
  lcd.init();
  lcd.backlight();
  displayWelcomeMessage("Digital", 0, 0);
  displayWelcomeMessage("DRO", 8, 1);
  displayWelcomeMessage("Movement", 12, 2);
  //displayWelcomeMessage("Dimitris", 5, 3);
  delay(1500);
  lcd.clear();
  displayMenu();
  loadStepPermmFromEEPROM();
  loadSpeedFromEEPROM();
  loadSoftStopFromEEPROM();
  loadTotalStepsMMFromEEPROM();
  //saveStepPermmToEEPROM(StepPermm);
  // Load the StepPermm value from EEPROM
  //StepPermm = loadStepPermmFromEEPROM();

  // Set initial speed and acceleration for the stepper motor
  stepper.setMaxSpeed(stepperSpeed);  // Speed in steps per second
  stepper.setAcceleration(500);       // Acceleration in steps per second^2
  stepper1.setMaxSpeed(stepperSpeed);
  stepper1.setAcceleration(500);
  TotalSteps = TotalStepsMAX;
  // Move stepper to a known position (home position)
  if ((digitalRead(HOME_SWITCH_PIN) == LOW) && (digitalRead(HOME_SWITCH_PIN1) == LOW)) {
    //homeStepper();
  } else {
    //HomeStepperSlow();
  }
}

void loop() {
  handleKeypadInput();  // Process keypad input with minimal blocking
  stepper.run();        // Keep the stepper running smoothly
  stepper1.run();

  // Any other non-blocking tasks can be added here
  if (selectedItem == 0 && displayNeedsUpdate) {
    displayTotalSteps();
    displayNeedsUpdate = false;  // Reset the flag after updating the display
  }
  //prnDebug();
  lcd.setCursor(17, 3);
  lcd.print(digitalRead(HOME_SWITCH_PIN));
  lcd.print(" ");
  lcd.print(digitalRead(HOME_SWITCH_PIN1));
}

void handleKeypadInput() {
  if (motorsMoving) {
    return;  // Ignore input if motors are moving
  }
  char key = keypad.getKey();
  if (key != NO_KEY) {
    // Debounce check: only proceed if enough time has passed since the last key press
    if (key != lastKey || (millis() - lastKeyPressTime) > debounceDelay) {
      lastKeyPressTime = millis();  // Update the last key press time
      lastKey = key;                // Update the last key pressed

      switch (key) {
        case 'A':  // Up
          currentItem = (currentItem - 1 + menuLength) % menuLength;
          displayMenu();
          break;
        case 'B':  // Down
          currentItem = (currentItem + 1) % menuLength;
          displayMenu();
          break;
        case 'C':  // Select
          selectItem();
          result = "";
          break;
        case 'D':  // Back
          // Handle back functionality if needed
          result = "";
          break;
      }

      if (selectedItem == 0 || selectedItem == 2 || selectedItem == 3 || selectedItem == 4 || selectedItem == 5|| selectedItem == 6|| selectedItem == 7) {
        handleNumberInput(key);
      }
    }
  }
}

void displayWelcomeMessage(String message, int col, int row) {
  lcd.setCursor(col, row);
  for (int i = 0; i < message.length(); i++) {
    lcd.print(message[i]);
    delay(100);  // Adjust the delay to control typing speed
  }
}

void displayMenu() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Menu:");
  lcd.setCursor(6, 0);
  lcd.print(menuItems[currentItem]);
  lcd.setCursor(0, 1);
  lcd.print("Sel:");
  lcd.setCursor(5, 1);
  lcd.print(menuItems[selectedItem]);
  lcd.print(" ");
  if (selectedItem == 0) {
    lcd.print(TotalSteps);
    lcd.print("mm");
  } else if (selectedItem == 2) {
    loadSpeedFromEEPROM();
    lcd.print(stepperSpeed);
    //lcd.print("sps"); // steps per second
  } else if (selectedItem == 3) {
    loadStepPermmFromEEPROM();
    lcd.print(StepPermm);
  } else if (selectedItem == 6) {
    lcd.print(stepper1.currentPosition());
  } else if (selectedItem == 4) {
    loadSoftStopFromEEPROM();
    lcd.print(SoftStop);
  }else if (selectedItem == 5) {
    loadTotalStepsMMFromEEPROM();
    lcd.print(TotalStepsMAX);
  }else if (selectedItem == 7) {
    //loadTotalStepsMMFromEEPROM();
    lcd.print(TrimLoffset);
  }
  displayNeedsUpdate = true;  // Set flag to update display if needed
}

void displayTotalSteps() {
  static String prevResult = "";
  if (result != prevResult) {
    lcd.setCursor(0, 2);
    lcd.print("Mikos:");
    lcd.setCursor(7, 2);
    lcd.print("    ");  // Clear previous value
    lcd.setCursor(7, 2);
    lcd.print(result);
    prevResult = result;
  }
}

void selectItem() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Use:");
  selectedItem = currentItem;
  lcd.print(menuItems[selectedItem]);

  switch (selectedItem) {
    case 1:  // Reset or Home
      FlagTotalSteps = 0;

      if ((digitalRead(HOME_SWITCH_PIN) == LOW) && (digitalRead(HOME_SWITCH_PIN1) == LOW)) {
        homeStepper();
      } else {
        HomeStepperSlow();
      }
      FlagTotalSteps = 0;
      FlagStepsPermm = 0;
      FlagSpeed = 0;
      FlagSoftStop = 0;
      FlagTotalStepsMM = 0;
      FlagTrimLoffset == 0;
      result = "";  // Clear the result after homing
      break;

    case 0:  // Handle Auto
      FlagStepsPermm = 0;
      FlagSpeed = 0;
      FlagSoftStop = 0;
      FlagTotalStepsMM = 0;
      FlagTrimLoffset == 0;
      if (FlagTotalSteps == 0) {
        lcd.print(" ");
        lcd.print(TotalSteps);
        lcd.print("mm");
        FlagTotalSteps = 1;
      } else {
        if (result.length() > 0) {  // Only update TotalSteps if a new value is entered
         if (result.toInt() < SoftStop) {
          result = SoftStop;  // Cap the value at 20mm
         }
          TotalSteps = result.toInt();
        }
        lcd.print(" ");
        lcd.print(TotalSteps);
        lcd.print("mm");
        moveStepper(TotalSteps);  // Move the stepper motor
        //FlagTotalSteps = 0;
        result = "";  // Clear the result after updating
      }

      break;

    case 2:  // Handle Speed
      FlagTotalSteps = 0;
      FlagStepsPermm = 0;
      FlagSoftStop = 0;
      FlagTotalStepsMM = 0;
      FlagTrimLoffset == 0;
      if (FlagSpeed == 0) {
        loadSpeedFromEEPROM();
        lcd.print(" ");
        lcd.print(stepperSpeed);
        FlagSpeed = 1;
      } else {
        if (result.length() > 0) {  // Only update stepperSpeed if a new value is entered
          stepperSpeed = result.toInt();
          updateSpeedToEEPROM(stepperSpeed);
          stepper.setMaxSpeed(stepperSpeed);
          stepper1.setMaxSpeed(stepperSpeed);
        }
        lcd.print(" ");
        lcd.print(stepperSpeed);
        FlagSpeed = 0;
        result = "";  // Clear the result after updating
      }
      break;

    case 3:  // Handle step/mm
      FlagTotalSteps = 0;
      FlagSpeed = 0;
      FlagSoftStop = 0;
      FlagTotalStepsMM = 0;
      FlagTrimLoffset == 0;
      if (FlagStepsPermm == 0) {
        loadStepPermmFromEEPROM();
        lcd.print(" ");
        lcd.print(StepPermm);
        FlagStepsPermm = 1;
      } else {
        if (result.length() > 0) {  // Only update StepPermm if a new value is entered
          StepPermm = result.toInt();
          updateStepPermm(StepPermm);  // Save StepPermm to EEPROM
        }
        lcd.print(" ");
        lcd.print(StepPermm);
        FlagStepsPermm = 0;
        result = "";  // Clear the result after updating
      }
      break;

    case 6:  // Set Current Position
      FlagStepsPermm = 0;
      FlagSpeed = 0;
      FlagSoftStop = 0;
      FlagTotalStepsMM = 0;
      FlagTrimLoffset == 0;
      if (result.length() > 0) {  // Only update currentPosition if a new value is entered
        long newPosition = result.toInt();
        stepper.setCurrentPosition(newPosition * StepPermm);
        stepper1.setCurrentPosition(newPosition * StepPermm);
        currentPosition = newPosition;
      }
      lcd.print(" ");
      lcd.print(currentPosition);
      lcd.print(" mm");
      result = "";  // Clear the result after updating
      break;

    case 4:  // Set SoftStop
      FlagTotalSteps = 0;
      FlagStepsPermm = 0;
      FlagSpeed = 0;
      FlagTotalStepsMM = 0;
      FlagTrimLoffset == 0;
      if (FlagSoftStop == 0) {
        loadSoftStopFromEEPROM();
        lcd.print(" ");
        lcd.print(SoftStop);
        FlagSoftStop = 1;
      } else {
        if (result.length() > 0) {
          SoftStop = result.toInt();
          updateSoftStop(SoftStop);
        }
        lcd.print(" ");
        lcd.print(SoftStop);
        lcd.print(" mm");
        FlagSoftStop = 0;
        result = "";  // Clear the result after updating
      }
      break;

    case 5:  // Set MaxMM
      FlagTotalSteps = 0;
      FlagStepsPermm = 0;
      FlagSpeed = 0;
      FlagSoftStop = 0;
      FlagTrimLoffset == 0;
      if (FlagTotalStepsMM == 0) {
        loadTotalStepsMMFromEEPROM();
        lcd.print(" ");
        lcd.print(TotalStepsMAX);
        FlagTotalStepsMM = 1;
      } else {
        if (result.length() > 0) {
          TotalStepsMAX = result.toInt();
          TotalSteps = TotalStepsMAX;
          updateTotalStepsMM(TotalStepsMAX);
        }
        lcd.print(" ");
        lcd.print(TotalStepsMAX);
        lcd.print(" mm");
        FlagTotalStepsMM = 0;
        result = "";  // Clear the result after updating
      }
      break;

    case 7:  // Set TrimL
      FlagTotalSteps = 0;
      FlagStepsPermm = 0;
      FlagSpeed = 0;
      FlagSoftStop = 0;
      FlagTotalStepsMM = 0;
      if (FlagTrimLoffset == 0) {
        //loadTotalStepsMMFromEEPROM();
        lcd.print(" ");
        lcd.print(TrimLoffset);
        FlagTrimLoffset = 1;
      } else {
        if (result.length() > 0) {
          TrimLoffset = result.toInt();
          //TotalSteps = TotalStepsMAX;
          //updateTotalStepsMM(TotalStepsMAX);
        }
        lcd.print(" ");
        lcd.print(TrimLoffset);
        lcd.print(" stp");
        FlagTrimLoffset = 0;
        result = "";  // Clear the result after updating
      }
      break;

    default:
      break;
  }

  delay(500);
  displayMenu();
  displayNeedsUpdate = true;  // Ensure the display is updated
}

void handleNumberInput(char key) {
  if (result.length() < 5) {  // Adjust length as needed
    if (key >= '0' && key <= '9') {
      result += key;
      totalStepsUpdated = true;  // Set the flag to indicate the value has changed
    }
  }
  if (selectedItem == 0) {
    int inputValue = result.toInt();
    if (inputValue > TotalStepsMAX) {
      inputValue = TotalStepsMAX;             // Cap the value at 570mm
      result = String(inputValue);  // Update the result to reflect the capped value
    }
    displayTotalSteps();
  } else if (selectedItem == 2) { // Stepper Speed
    displayStepperSpeed();
  } else if (selectedItem == 3) {  // Steps/mm
    displayStepPermm();
  } else if (selectedItem == 6) {  // Current Position
    displayCurPos();
  } else if (selectedItem == 4) { // Soft Stop
    displaySoftStop();
  }else if (selectedItem == 5) {  //Max MM
    displayMaxMM();
  }else if (selectedItem == 7) {  //Max MM
    TrimL();
  }
}

void homeStepper() {
  motorsMoving = true;  // Motors are moving
  // Set initial speed and acceleration for both steppers
  stepper.setCurrentPosition(0);  // Reset the current position to zero
  stepper.setMaxSpeed(1000);      // Set speed for homing
  stepper.setAcceleration(500);   // Set acceleration for homing

  stepper1.setCurrentPosition(0);  // Reset the current position to zero
  stepper1.setMaxSpeed(1000);      // Set speed for homing
  stepper1.setAcceleration(500);   // Set acceleration for homing

  // Move both steppers in the direction to trigger their respective homing switches
  stepper.moveTo(TotalStepsMAX * StepPermm);   // Arbitrarily large positive value to ensure movement
  stepper1.moveTo(TotalStepsMAX * StepPermm);  // Arbitrarily large positive value to ensure movement

  // Keep track of whether each stepper has homed
  bool stepperHomed = false;
  bool stepper1Homed = false;

  // Move both steppers simultaneously until both are homed
  while (!stepperHomed || !stepper1Homed) {
    if (!stepperHomed) {
      if (digitalRead(HOME_SWITCH_PIN) == HIGH) {  // Assume LOW means the switch is triggered
        stepper.stop();                            // Stop the first stepper if the switch is triggered
        stepper.setCurrentPosition(0);             // Set the home position to 0
        stepperHomed = true;                       // Mark as homed
      } else {
        stepper.run();  // Continue moving the first stepper
      }
    }

    if (!stepper1Homed) {
      if (digitalRead(HOME_SWITCH_PIN1) == HIGH) {  // Assume LOW means the switch is triggered
        stepper1.stop();                            // Stop the second stepper if the switch is triggered
        stepper1.setCurrentPosition(0);             // Set the home position to 0
        stepper1Homed = true;                       // Mark as homed
      } else {
        stepper1.run();  // Continue moving the second stepper
      }
    }
  }

  // Final speed and acceleration settings after homing
  stepper.setMaxSpeed(stepperSpeed);
  stepper.setAcceleration(500);

  stepper1.setMaxSpeed(stepperSpeed);
  stepper1.setAcceleration(500);
  motorsMoving = false;  // Motors have stopped
  delay(200);
  HomeStepperSlow();
}

void HomeStepperSlow() {
  motorsMoving = true;  // Motors are moving
    // Check if the motors are already homed
  bool stepperHomed = (digitalRead(HOME_SWITCH_PIN) == HIGH);
  bool stepper1Homed = (digitalRead(HOME_SWITCH_PIN1) == HIGH);

  // If both motors are homed, perform the slow homing procedure
  if (stepperHomed && stepper1Homed) {
    // Move the steppers away from the homing switches to ensure they are not triggered
    stepper.setCurrentPosition(0);
    stepper.setMaxSpeed(800);         // Set a moderate speed to move away from the switch
    stepper.moveTo(-10 * StepPermm);  // Move 50 steps away from the switch

    stepper1.setCurrentPosition(0);
    stepper1.setMaxSpeed(800);         // Set a moderate speed to move away from the switch
    stepper1.moveTo(-10 * StepPermm);  // Move 50 steps away from the switch

    // Run both steppers until they have moved away from the switches
    while (stepper.distanceToGo() != 0 || stepper1.distanceToGo() != 0) {
      stepper.run();
      stepper1.run();
    }

    // Now that the steppers are away from the switches, perform slow homing
    stepper.setMaxSpeed(500);        // Set slower speed for precise homing
    stepper.setAcceleration(500);    // Set slower acceleration for precise homing
    stepper.moveTo(15 * StepPermm);  // Move towards the switch at slower speed

    stepper1.setMaxSpeed(500);        // Set slower speed for precise homing
    stepper1.setAcceleration(500);    // Set slower acceleration for precise homing
    stepper1.moveTo(15 * StepPermm);  // Move towards the switch at slower speed

    // Reset homing flags for the slow homing process
    bool stepperHomed1 = false;
    bool stepper1Homed1 = false;

    // Keep moving both steppers towards the switches until they are triggered again
    while (!stepperHomed1 || !stepper1Homed1) {
      if (!stepperHomed1) {
        if (digitalRead(HOME_SWITCH_PIN) == HIGH) {
          stepper.stop();
          //stepper.setCurrentPosition(600 * StepPermm); // Set accurate home position
          stepperHomed1 = true;
        } else {
          stepper.run();
        }
      }
      if (!stepper1Homed1) {
        if (digitalRead(HOME_SWITCH_PIN1) == HIGH) {
          stepper1.stop();
          //stepper1.setCurrentPosition(600 * StepPermm); // Set accurate home position
          stepper1Homed1 = true;
        } else {
          stepper1.run();
        }
      }
    }
    // Final speed and acceleration settings after precise homing
      stepper.setCurrentPosition(0);
      stepper1.setCurrentPosition(0);
      stepper.moveTo(1 * StepPermm / 2 );
      stepper1.moveTo(1 * StepPermm / 2);
      while (stepper.distanceToGo() != 0 || stepper1.distanceToGo() != 0) {
      stepper.run();
      stepper1.run();
    }
  }
  stepper.setMaxSpeed(stepperSpeed);
  stepper.setAcceleration(500);
  stepper1.setMaxSpeed(stepperSpeed);
  stepper1.setAcceleration(500);
  stepper.setCurrentPosition(TotalStepsMAX * StepPermm);
  stepper1.setCurrentPosition(TotalStepsMAX * StepPermm);
  TotalSteps = TotalStepsMAX;
  motorsMoving = false;  // Motors have stopped
}

void displayStepperSpeed() {
  static String prevResult = "";
  if (result != prevResult) {
    lcd.setCursor(0, 2);
    lcd.print("Stp/sec:");
    lcd.setCursor(10, 2);
    lcd.print("    ");  // Clear previous value
    lcd.setCursor(10, 2);
    lcd.print(result);
    prevResult = result;
  }
}

void displayCurPos() {
  static String prevResult = "";
  if (result != prevResult) {
    lcd.setCursor(0, 2);
    lcd.print("mm:");
    lcd.setCursor(7, 2);
    lcd.print("    ");  // Clear previous value
    lcd.setCursor(7, 2);
    lcd.print(result);
    prevResult = result;
  }
}

void displaySoftStop() {
  static String prevResult = "";
  if (result != prevResult) {
    lcd.setCursor(0, 2);
    lcd.print("mm:");
    lcd.setCursor(10, 2);
    lcd.print("    ");  // Clear previous value
    lcd.setCursor(7, 2);
    lcd.print(result);
    prevResult = result;
  }
}

void displayTotalStepsMM() {
  static String prevResult = "";
  if (result != prevResult) {
    lcd.setCursor(0, 2);
    lcd.print("mm:");
    lcd.setCursor(10, 2);
    lcd.print("    ");  // Clear previous value
    lcd.setCursor(7, 2);
    lcd.print(result);
    prevResult = result;
  }
}

void displayStepPermm() {
  static String prevResult = "";
  if (result != prevResult) {
    lcd.setCursor(0, 2);
    lcd.print("Stp/mm:");
    lcd.setCursor(10, 2);
    lcd.print("    ");  // Clear previous value
    lcd.setCursor(10, 2);
    lcd.print(result);
    prevResult = result;
  }
}

void displayMaxMM() {
  static String prevResult = "";
  if (result != prevResult) {
    lcd.setCursor(0, 2);
    lcd.print("mm:");
    lcd.setCursor(10, 2);
    lcd.print("    ");  // Clear previous value
    lcd.setCursor(7, 2);
    lcd.print(result);
    prevResult = result;
  }
}

void TrimL() {
  static String prevResult = "";
  if (result != prevResult) {
    lcd.setCursor(0, 2);
    lcd.print("Stp:");
    lcd.setCursor(10, 2);
    lcd.print("    ");  // Clear previous value
    lcd.setCursor(7, 2);
    lcd.print(result);
    prevResult = result;
  }
}

void moveStepper(long steps) {
  steps = steps * StepPermm;
  motorsMoving = true;    // Motors are moving
  stepper.moveTo(steps);  // Move the stepper motor to the specified steps
  stepper1.moveTo(steps);
  while (stepper.distanceToGo() != 0 || stepper1.distanceToGo() != 0) {
    stepper.run();
    stepper1.run();
  }
  motorsMoving = false;     // Motors have stopped
  currentPosition = steps;  // Update the current position
}

void prnDebug() {
    lcd.setCursor(0, 3);
  lcd.print(FlagTotalSteps);
  lcd.print(" ");
  lcd.print(FlagStepsPermm);
  lcd.print(" ");
  lcd.print(FlagSpeed);
  lcd.print(" ");
  lcd.print(FlagSoftStop);
}
A4988
A4988