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

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

// Joystick input pins
#define joystickXPin A0  // Joystick X-axis input
#define joystickYPin A1  // Joystick Y-axis input

// Motor driver pins
const int enablePins[] = {4, 5, 6, 7, 8, 9};  // Enable pins of the motor drivers
const int motorStepPins[] = {10, 11, 12, 13, A2, A3};  // Step pins of the motor drivers
const int motorDirPins[] = {14, 15, 16, 17, A4, A5};  // Direction pins of the motor drivers

// Keypad pins
const byte ROWS = 4; // Define the number of rows and columns of the keypad
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {18, 19, 20, 21}; // Connect to the row pinouts of the keypad
byte colPins[COLS] = {22, 23, 24, 25}; // Connect to the column pinouts of the keypad
Keypad keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Number of motors
const int numMotors = 6;

// Create an array of AccelStepper instances for each motor
AccelStepper motors[numMotors];

// Array to store the states of each motor (activated or deactivated)
bool motorStates[numMotors] = {false};

void setup() {
  // Set up motor driver pins
  for (int i = 0; i < numMotors; i++) {
    pinMode(enablePins[i], OUTPUT);
    digitalWrite(enablePins[i], LOW);
    motors[i] = AccelStepper(AccelStepper::DRIVER, motorStepPins[i], motorDirPins[i]);
    motors[i].setMaxSpeed(1000);
    motors[i].setAcceleration(500);
  }
}

void loop() {
  // Check for keypad input
  char key = keypad.getKey();

  // If "0" is pressed, stop all motors
  if (key == '0') {
    for (int i = 0; i < numMotors; i++) {
      motors[i].stop();
      motorStates[i] = false;
    }
  }

  // Map the keypad input to the motor index
  int motorIndex = -1;
  switch (key) {
    case '1':
      motorIndex = 0;
      break;
    case '2':
      motorIndex = 1;
      break;
    case '3':
      motorIndex = 2;
      break;
    case '4':
      motorIndex = 3;
      break;
    case '5':
      motorIndex = 4;
      break;
    case '6':
      motorIndex = 5;
      break;
    default:
      break;
  }

  // If a valid motor index is selected, toggle the motor state
  if (motorIndex >= 0 && motorIndex < numMotors) {
    motorStates[motorIndex] = !motorStates[motorIndex];
  }

  // Control the motors based on their states
  for (int i = 0; i < numMotors; i++) {
    if (motorStates[i]) {
      int joystickX = analogRead(joystickXPin);
      int joystickY = analogRead(joystickYPin);

      int motorSpeed = map(joystickY, 0, 1023, 0, 1000);
      int motorDirection = (joystickX > 512) ? -1 : 1;

      motors[i].setSpeed(motorSpeed * motorDirection);
      motors[i].move(motorDirection * STEPS);
      motors[i].run();
    } else {
      motors[i].stop();
    }
  }
}