#include <AccelStepper.h>

// Stepper motor connections
#define DIR_M1 10
#define STEP_M1 9

#define DIR_M2 8
#define STEP_M2 7

#define DIR_M3 6
#define STEP_M3 5

#define ENABLE_PIN 4 // Enable pin for A4988

// Define stepper motors (1 = driver mode)
AccelStepper bender(1, STEP_M1, DIR_M1);
AccelStepper zAxis(1, STEP_M2, DIR_M2);
AccelStepper feeder(1, STEP_M3, DIR_M3);

bool last_benderRun;
bool last_zAxisRun;
bool last_feederRun;

void setup() 
{
  Serial.begin(115200);

  // Enable A4988 motor drivers
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, LOW); // Enable motor driver

  pinMode(A0,INPUT_PULLUP);
  pinMode(A1,INPUT_PULLUP);
  pinMode(A2,INPUT_PULLUP);  
  pinMode(A3,INPUT_PULLUP);
  pinMode(A4,INPUT_PULLUP);
  pinMode(A5,INPUT_PULLUP);

  // Stepper motor setup with safer speed and acceleration
  feeder.setMaxSpeed(200);
  feeder.setAcceleration(100);

  zAxis.setMaxSpeed(200);
  zAxis.setAcceleration(100);

  bender.setMaxSpeed(200);
  bender.setAcceleration(100);

  Serial.println("3D Wire Bender Ready...");
}

void loop() 
{
  if(digitalRead(A0) == LOW)
  {
    bender.moveTo(1000);
  }
  if(digitalRead(A1) == LOW)
  {
    zAxis.moveTo(1000);
  }
  if(digitalRead(A2) == LOW)
  {
    feeder.moveTo(1000);
  }
  if(digitalRead(A3) == LOW)
  {
    bender.moveTo(0);
  }
  if(digitalRead(A4) == LOW)
  {
    zAxis.moveTo(0);
  }
  if(digitalRead(A5) == LOW)
  {
    feeder.moveTo(0);
  }

  bool new_benderRun = bender.isRunning();
  bool new_zAxisRun = zAxis.isRunning();
  bool new_feederRun = feeder.isRunning();

  if(new_benderRun != last_benderRun)
  {
    if(!new_benderRun)
      Serial.println("Bender ready");
    last_benderRun = new_benderRun;
  }

  if(new_zAxisRun != last_zAxisRun)
  {
    if(!new_zAxisRun)
      Serial.println("zAxis ready");
    last_zAxisRun = new_zAxisRun;
  }

  if(new_feederRun != last_feederRun)
  {
    if(!new_feederRun)
      Serial.println("Feeder ready");
    last_feederRun = new_feederRun;
  }

  // Keep the library active.
  // The library decides if a step is required at this moment.
  feeder.run();
  zAxis.run();
  bender.run();
}