#include <AccelStepper.h>
#include <Keypad.h>

// Define the number of steps per revolution for your stepper motor
const int STEPS = 2000;

// Motor driver pins
#define motorStep_1 2   // Step pin of motor 1
#define motorDir_1 3    // Direction pin of motor 1
#define ENABLE_1 4
#define motorStep_2 5   // Step pin of motor 2
#define motorDir_2 6    // Direction pin of motor 2
#define ENABLE_2 7
#define motorStep_3 8
#define motorDir_3 9
#define ENABLE_3 10
#define motorStep_4 11
#define motorDir_4 12
#define ENABLE_4 13
#define motorStep_5 14
#define motorDir_5 15
#define ENABLE_5 16
#define motorStep_6 17
#define motorDir_6 18
#define ENABLE_6 19

// Keypad setup
const byte ROWS = 4; // Number of rows
const byte COLS = 4; // Number of columns
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
byte rowPins[ROWS] = {A2, A3, A4, A5}; // Connect to the row pinouts of the keypad
byte colPins[COLS] = {A6, A7, A8, A9}; // Connect to the column pinouts of the keypad
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Create an instance of the AccelStepper class for each motor
AccelStepper stepper_1(AccelStepper::DRIVER, motorStep_1, motorDir_1);
AccelStepper stepper_2(AccelStepper::DRIVER, motorStep_2, motorDir_2);
AccelStepper stepper_3(AccelStepper::DRIVER, motorStep_3, motorDir_3);
AccelStepper stepper_4(AccelStepper::DRIVER, motorStep_4, motorDir_4);
AccelStepper stepper_5(AccelStepper::DRIVER, motorStep_5, motorDir_5);
AccelStepper stepper_6(AccelStepper::DRIVER, motorStep_6, motorDir_6);

void setup() {
  pinMode(ENABLE_1 , OUTPUT);
  pinMode(ENABLE_2 , OUTPUT);
  pinMode(ENABLE_3 , OUTPUT);
  pinMode(ENABLE_4 , OUTPUT);
  pinMode(ENABLE_5 , OUTPUT);
  pinMode(ENABLE_6 , OUTPUT);

  digitalWrite(ENABLE_1 , LOW);
  digitalWrite(ENABLE_2 , LOW);
  digitalWrite(ENABLE_3 , LOW);
  digitalWrite(ENABLE_4 , LOW);
  digitalWrite(ENABLE_5 , LOW);
  digitalWrite(ENABLE_6 , LOW);

  // Set the maximum speed and acceleration of each stepper motor
  stepper_1.setMaxSpeed(1000);
  stepper_1.setAcceleration(500);
  stepper_2.setMaxSpeed(1000);
  stepper_2.setAcceleration(500);
  stepper_3.setMaxSpeed(1000);
  stepper_3.setAcceleration(500);
  stepper_4.setMaxSpeed(1000);
  stepper_4.setAcceleration(500);
  stepper_5.setMaxSpeed(1000);
  stepper_5.setAcceleration(500);
  stepper_6.setMaxSpeed(1000);
  stepper_6.setAcceleration(500);
}

void loop() {
  // Handle keypad input
  char key = keypad.getKey();
  if (key != NO_KEY) {
    // Check if the key corresponds to a motor number
    int motorIndex = key - '1';  // Convert from ASCII to motor index
    if (motorIndex >= 0 && motorIndex < 6) {
      // Stop all motors
      stepper_1.stop();
      stepper_2.stop();
      stepper_3.stop();
      stepper_4.stop();
      stepper_5.stop();
      stepper_6.stop();

      // Start the selected motor
      switch (motorIndex) {
        case 0:
          stepper_1.move(STEPS);
          stepper_1.run();
          break;

        case 1:
          stepper_2.move(STEPS);
          stepper_2.run();
          break;
        case 2:
          stepper_3.move(STEPS);
          stepper_3.run();
          break;
        case 3:
          stepper_4.move(STEPS);
          stepper_4.run();
          break;
        case 4:
          stepper_5.move(STEPS);
          stepper_5.run();
          break;
        case 5:
          stepper_6.move(STEPS);
          stepper_6.run();
          break;
      }
    }
  }

  // Update the motor positions
  stepper_1.run();
  stepper_2.run();
  stepper_3.run();
  stepper_4.run();
  stepper_5.run();
  stepper_6.run();
}