#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);
  pinMode(button1, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);
  pinMode(stopButton, INPUT_PULLUP);
  stepperA.setMaxSpeed(maxSpeed);
  stepperB.setMaxSpeed(maxSpeed);
}

void loop() {
  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; // Negative for counterclockwise
      stepperA.setSpeed(currentSpeedA);
      motorRunning = true;
      Serial.println("Motor A moving counterclockwise at speed 100");
    }
  }

  // Check if button2 is pressed to set standard speed
  if (button2State == LOW && previousButton2State == HIGH) {
    delay(50); // Simple debounce
    if (digitalRead(button2) == LOW) {
      currentSpeedA = -20;
      currentSpeedB = -50;
      stepperA.setSpeed(currentSpeedA);
      stepperB.setSpeed(currentSpeedB);
      motorRunning = true;
      Serial.print("Speed set to standard: A=");
      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;
      if (!motorRunning) {
        stepperA.setSpeed(0);
        stepperB.setSpeed(0);
        Serial.println("Motors stopped");
      } else {
        stepperA.setSpeed(currentSpeedA);
        stepperB.setSpeed(currentSpeedB);
        Serial.println("Motors running");
      }
    }
  }

  previousButton1State = button1State;
  previousButton2State = button2State;
  previousStopButtonState = stopButtonState;

  // Run the motors if they are supposed to be running
  if (motorRunning) {
    stepperA.runSpeed();
    stepperB.runSpeed();
  }
}