#include <AccelStepper.h>

#define ENCODER_CLK 15   
#define ENCODER_DT   27

// Motor configuration
#define stepsPerRevolution 5
#define MOTOR_STEP_PIN 2
#define MOTOR_DIR_PIN  4

#define MOTOR2_STEP_PIN 17
#define MOTOR2_DIR_PIN  16

#define MOTOR3_STEP_PIN 18
#define MOTOR3_DIR_PIN  5

#define MOTOR4_STEP_PIN 23
#define MOTOR4_DIR_PIN  22

AccelStepper motor(AccelStepper::DRIVER, MOTOR_STEP_PIN, MOTOR_DIR_PIN);
AccelStepper motor2(AccelStepper::DRIVER, MOTOR2_STEP_PIN, MOTOR2_DIR_PIN);
AccelStepper motor3(AccelStepper::DRIVER, MOTOR3_STEP_PIN, MOTOR3_DIR_PIN);
AccelStepper motor4(AccelStepper::DRIVER, MOTOR4_STEP_PIN, MOTOR4_DIR_PIN);

void setup() {
  Serial.begin(115200);
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);

  motor.setMaxSpeed(1000.0);
  motor.setAcceleration(500.0);
}

int lastClk = HIGH;
int encoderSteps = 0;

void loop() {
  int newClk = digitalRead(ENCODER_CLK);
  int dtValue = digitalRead(ENCODER_DT);

  if (newClk != lastClk) {
    lastClk = newClk;
    if (newClk == LOW && dtValue == HIGH) {
      // Clockwise rotation
      encoderSteps++;
      if (encoderSteps >= 20) {
        encoderSteps = 0; // Reset to 0 after 20 steps
      }
    }

    // Control the motor based on encoder steps
    if (encoderSteps >= 0 && encoderSteps <= 4) {
      digitalWrite(MOTOR_DIR_PIN, HIGH);
      digitalWrite(MOTOR2_DIR_PIN, HIGH);
      digitalWrite(MOTOR3_DIR_PIN, HIGH);
      digitalWrite(MOTOR4_DIR_PIN, HIGH);
      for (int i = 0; i < stepsPerRevolution; i++){ 
      digitalWrite(MOTOR_STEP_PIN, LOW);
      digitalWrite(MOTOR2_STEP_PIN, LOW);
      digitalWrite(MOTOR3_STEP_PIN, HIGH);
      digitalWrite(MOTOR4_STEP_PIN, HIGH);
      delayMicroseconds(2000);
      digitalWrite(MOTOR_STEP_PIN, LOW);
      digitalWrite(MOTOR2_STEP_PIN, LOW);
      digitalWrite(MOTOR3_STEP_PIN, LOW);
      digitalWrite(MOTOR4_STEP_PIN, LOW);
      delayMicroseconds(2000);
      }
    } else if(encoderSteps >= 5 && encoderSteps <= 9){
      digitalWrite(MOTOR_DIR_PIN, HIGH);
      digitalWrite(MOTOR2_DIR_PIN, HIGH);
      digitalWrite(MOTOR3_DIR_PIN, HIGH);
      digitalWrite(MOTOR4_DIR_PIN, HIGH);
      for (int i = 0; i < stepsPerRevolution; i++){ 
      digitalWrite(MOTOR_STEP_PIN, LOW);
      digitalWrite(MOTOR2_STEP_PIN, HIGH);
      digitalWrite(MOTOR3_STEP_PIN, HIGH);
      digitalWrite(MOTOR4_STEP_PIN, LOW);
      delayMicroseconds(2000);
      digitalWrite(MOTOR_STEP_PIN, LOW);
      digitalWrite(MOTOR2_STEP_PIN, LOW);
      digitalWrite(MOTOR3_STEP_PIN, LOW);
      digitalWrite(MOTOR4_STEP_PIN, LOW);
      delayMicroseconds(2000);
      }
    }if (encoderSteps >= 10 && encoderSteps <= 14) {
      digitalWrite(MOTOR_DIR_PIN, HIGH);
      digitalWrite(MOTOR2_DIR_PIN, HIGH);
      digitalWrite(MOTOR3_DIR_PIN, HIGH);
      digitalWrite(MOTOR4_DIR_PIN, HIGH);
      for (int i = 0; i < stepsPerRevolution; i++){ 
      digitalWrite(MOTOR_STEP_PIN, HIGH);
      digitalWrite(MOTOR2_STEP_PIN, HIGH);
      digitalWrite(MOTOR3_STEP_PIN, LOW);
      digitalWrite(MOTOR4_STEP_PIN, LOW);
      delayMicroseconds(2000);
      digitalWrite(MOTOR_STEP_PIN, LOW);
      digitalWrite(MOTOR2_STEP_PIN, LOW);
      digitalWrite(MOTOR3_STEP_PIN, LOW);
      digitalWrite(MOTOR4_STEP_PIN, LOW);
      delayMicroseconds(2000);
      }
    } if (encoderSteps >= 15 && encoderSteps <= 20) {
      digitalWrite(MOTOR_DIR_PIN, HIGH);
      digitalWrite(MOTOR2_DIR_PIN, HIGH);
      digitalWrite(MOTOR3_DIR_PIN, HIGH);
      digitalWrite(MOTOR4_DIR_PIN, HIGH);
      for (int i = 0; i < stepsPerRevolution; i++){ 
      digitalWrite(MOTOR_STEP_PIN, HIGH);
      digitalWrite(MOTOR2_STEP_PIN, LOW);
      digitalWrite(MOTOR3_STEP_PIN, LOW);
      digitalWrite(MOTOR4_STEP_PIN, HIGH);
      delayMicroseconds(2000);
      digitalWrite(MOTOR_STEP_PIN, LOW);
      digitalWrite(MOTOR2_STEP_PIN, LOW);
      digitalWrite(MOTOR3_STEP_PIN, LOW);
      digitalWrite(MOTOR4_STEP_PIN, LOW);
      delayMicroseconds(2000);
      }
    } else {
      // Motor stops
      motor.setSpeed(0);
      motor.runSpeed();
    }
  }
}