#include "AccelStepper.h"

// Define stepper motor connections and motor interface type
#define AdirPin D10
#define AstepPin D9
#define BdirPin D8
#define BstepPin D7
#define button1 D0
#define button2 D1
#define stopButton D2
#define motorInterfaceType 1

// Create instances of the AccelStepper class
AccelStepper stepperA = AccelStepper(motorInterfaceType, AstepPin, AdirPin);
AccelStepper stepperB = AccelStepper(motorInterfaceType, BstepPin, BdirPin);

// Define variables
int button1State;
int button2State;
int stopButtonState;
int previousButton1State = HIGH;
int previousButton2State = HIGH;
int previousStopButtonState = HIGH;
int currentSpeedA = 0; // Starting speed for motor A
int currentSpeedB = 0; // Starting speed for motor B
const int maxSpeed = 600;
bool motorRunning = false;

void setup() {
  Serial.begin(9600); // Initialize serial communication
  pinMode(button1, INPUT_PULLUP); // Set button1 pin as input with internal pull-up resistor
  pinMode(button2, INPUT_PULLUP); // Set button2 pin as input with internal pull-up resistor
  pinMode(stopButton, INPUT_PULLUP); // Set stopButton pin as input with internal pull-up resistor
  stepperA.setMaxSpeed(maxSpeed); // Set maximum speed for stepper A
  stepperB.setMaxSpeed(maxSpeed); // Set maximum speed for stepper B
}

void loop() {
  // Read the state of each button
  button1State = digitalRead(button1);
  button2State = digitalRead(button2);
  stopButtonState = digitalRead(stopButton);

  // Check if button1 is pressed to move motor A counterclockwise at speed 100
  if (button1State == LOW && previousButton1State == HIGH) {
    delay(50); // Simple debounce
    if (digitalRead(button1) == LOW) {
      currentSpeedA = 100; // Set speed for counterclockwise rotation
      stepperA.setSpeed(currentSpeedA); // Set speed for stepper A
      motorRunning = true; // Indicate that motors should be running
      Serial.println("Motor A moving counterclockwise at speed 100"); // Debug message
    }
  }

  // Check if button2 is pressed to set standard speed
  if (button2State == LOW && previousButton2State == HIGH) {
    delay(50); // Simple debounce
    if (digitalRead(button2) == LOW) {
      currentSpeedA = -20; // Set speed for motor A
      currentSpeedB = -50; // Set speed for motor B
      stepperA.setSpeed(currentSpeedA); // Set speed for stepper A
      stepperB.setSpeed(currentSpeedB); // Set speed for stepper B
      motorRunning = true; // Indicate that motors should be running
      Serial.print("Speed set to standard: A="); // Debug message
      Serial.print(currentSpeedA);
      Serial.print(", B=");
      Serial.println(currentSpeedB);
    }
  }

  // Check if stopButton is pressed to stop/resume the motors
  if (stopButtonState == LOW && previousStopButtonState == HIGH) {
    delay(50); // Simple debounce
    if (digitalRead(stopButton) == LOW) {
      motorRunning = !motorRunning; // Toggle motor running state
      if (!motorRunning) {
        stepperA.setSpeed(0); // Stop stepper A
        stepperB.setSpeed(0); // Stop stepper B
        Serial.println("Motors stopped"); // Debug message
      } else {
        stepperA.setSpeed(currentSpeedA); // Resume stepper A at current speed
        stepperB.setSpeed(currentSpeedB); // Resume stepper B at current speed
        Serial.println("Motors running"); // Debug message
      }
    }
  }

  // Update previous button states
  previousButton1State = button1State;
  previousButton2State = button2State;
  previousStopButtonState = stopButtonState;

  // Run the motors if they are supposed to be running
  if (motorRunning) {
    stepperA.runSpeed(); // Run stepper A at set speed
    stepperB.runSpeed(); // Run stepper B at set speed
  }
}