#include <AccelStepper.h>

// Define the stepper motor interface type and the pins for the motor driver
#define motorInterfaceType 1
#define dirPin 2
#define stepPin 3
#define homeSwitchPin 13  // Pin connected to the home switch

AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);

void setup() {
  // Set initial maximum speed for the stepper motor
  stepper.setMaxSpeed(1000);  // Default speed

  // Set the home switch pin as input with an internal pull-up resistor
  pinMode(homeSwitchPin, INPUT_PULLUP);

  // Start serial communication
  Serial.begin(9600);
  Serial.println("Stepper Motor Control Ready");
}

void loop() {
  // Check if data is available on the serial port
  if (Serial.available() > 0) {
    // Read the incoming command as a string
    String command = Serial.readStringUntil('\n');
    command.trim(); // Remove any newline characters

    // Parse the command
    if (command.startsWith("M ")) {
      // Extract the steps and direction from the command
      int stepsEndIndex = command.indexOf(' ', 2);
      int steps = command.substring(2, stepsEndIndex).toInt();
      String direction = command.substring(stepsEndIndex + 1);

      // Set the direction based on the command
      if (direction.equalsIgnoreCase("F")) {
        stepper.move(steps);  // Positive steps
      } else if (direction.equalsIgnoreCase("B")) {
        stepper.move(-steps); // Negative steps for reverse
      } else {
        Serial.println("Invalid direction");
      }
    } else if (command.startsWith("A ")) {
      // Example command: A 500
      int accel = command.substring(2).toInt();
      stepper.setAcceleration(accel);
      Serial.println("Acceleration set to: " + String(accel));
    } else if (command.startsWith("D ")) {
      // Example command: D 500
      int decel = command.substring(2).toInt();
      stepper.setAcceleration(decel);  // Same as acceleration
      Serial.println("Deceleration set to: " + String(decel));
    } else if (command.startsWith("S ")) {
      // Example command: S 800
      int speed = command.substring(2).toInt();
      stepper.setMaxSpeed(speed);
      Serial.println("Speed set to: " + String(speed));
    } else if (command.equals("H")) {
      // Home the stepper motor
      homeStepperMotor();
    } else if (command.equals("ST")) {
      // Check the status of the stepper motor
      checkMotorStatus();
    } else if (command.equals("X")) {
      // Immediately stop the motor
      emergencyStop();
    } else if (command.equals("P")) {
      // Report the current position of the stepper motor
      reportPosition();
    } else {
      Serial.println("Invalid command");
    }

    // Optionally, send feedback to the PC that the command was received
    Serial.println("Command received: " + command);
  }

  // Run the stepper motor to the target position
  stepper.run();
}

void homeStepperMotor() {
  // Move the motor backward until the home switch is triggered
  stepper.setMaxSpeed(500);  // Slow down for homing
  stepper.setAcceleration(500);

  while (digitalRead(homeSwitchPin) == HIGH) {
    stepper.moveTo(stepper.currentPosition() - 1); // Move one step backward
    stepper.run();
  }

  // Stop the motor and set the current position as 0 (home)
  stepper.stop();
  stepper.setCurrentPosition(0);

  // Send a confirmation to the PC
  Serial.println("Homing complete, position set to 0");
}

void checkMotorStatus() {
  if (stepper.distanceToGo() != 0) {
    Serial.println("Moving");
  } else {
    Serial.println("Stopped");
  }
}

void emergencyStop() {
  // Stop the motor immediately
  stepper.stop();
  Serial.println("Emergency stop executed");
}

void reportPosition() {
  // Report the current position of the stepper motor
  long currentPosition = stepper.currentPosition();
  Serial.println("Current position: " + String(currentPosition));
}