#include <AccelStepper.h>

#define ENCODER_CLK 15   // Change this to the desired CLK pin on ESP32
#define ENCODER_DT   27   // Change this to the desired DT pin on ESP32

// Motor configuration
#define MOTOR1_STEP_PIN 2
#define MOTOR1_DIR_PIN  4
#define MOTOR2_STEP_PIN 14
#define MOTOR2_DIR_PIN  16

AccelStepper motor1(AccelStepper::DRIVER, MOTOR1_STEP_PIN, MOTOR1_DIR_PIN);
AccelStepper motor2(AccelStepper::DRIVER, MOTOR2_STEP_PIN, MOTOR2_DIR_PIN);

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

  motor1.setMaxSpeed(1000.0);
  motor1.setAcceleration(500.0);
  
  motor2.setMaxSpeed(1000.0);
  motor2.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
      }
    }
    if (newClk == LOW && dtValue == LOW) {
      // Counterclockwise rotation
      encoderSteps--;
      if (encoderSteps < 0) {
        encoderSteps = 19; // Wrap to 19 after -1 step
      }
    }
    
    Serial.print("Encoder Steps: ");
    Serial.println(encoderSteps);

    // Control the motors based on encoder steps
    if (encoderSteps >= 0 && encoderSteps <= 4) {
      // Motor 1 moves clockwise, Motor 2 is stopped
      motor1.setSpeed(10);
      motor1.runSpeed();
      motor2.setSpeed(0);
      motor2.runSpeed();
      Serial.println("Motor 1: Clockwise");
      Serial.println("Motor 2: Stopped");
    } else if (encoderSteps >= 5 && encoderSteps <= 9) {
      // Motor 1 stops, Motor 2 moves clockwise
      motor1.setSpeed(-10);
      motor1.runSpeed();
      motor2.setSpeed(0);
      motor2.runSpeed();
      Serial.println("Motor 1: Counterclockwise");
      Serial.println("Motor 2: Stopped");
    } else if (encoderSteps >= 10 && encoderSteps <= 14) {
      // Motor 1 moves counterclockwise, Motor 2 is stopped
      motor1.setSpeed(0);
      motor1.runSpeed();
      motor2.setSpeed(-10);
      motor2.runSpeed();
      Serial.println("Motor 1: stopped");
      Serial.println("Motor 2: Clockwise");
    } else if (encoderSteps >= 15 && encoderSteps <= 19) {
      // Motor 1 moves counterclockwise, Motor 2 is stopped
      motor1.setSpeed(0);
      motor1.runSpeed();
      motor2.setSpeed(10);
      motor2.runSpeed();
      Serial.println("Motor 1: stopped");
      Serial.println("Motor 2: CounterClockwise");
    } else {
      // Both motors stop
      motor1.setSpeed(0);
      motor1.runSpeed();
      motor2.setSpeed(0);
      motor2.runSpeed();
      Serial.println("Motor 1: Stopped");
      Serial.println("Motor 2: Stopped");
    }
  }
}