#include <AccelStepper.h>

// Pin definitions for Motor 1 (Penutup kanan-kiri)
const int stepPin1 = 28;
const int dirPin1 = 27;
const int enablePin1 = 26;

// Pin definitions for Motor 2 (Penutup atas-bawah)
const int stepPin2 = 24;
const int dirPin2 = 23;
const int enablePin2 = 22;

// Pin definitions for Motor 3 (Penyedia Tutup botol)
const int stepPin3 = 32;
const int dirPin3 = 31;
const int enablePin3 = 30;

// Pin definitions for Motor 4 (Meja PUTAR 8 slot)
const int stepPin4 = 36;
const int dirPin4 = 35;
const int enablePin4 = 34;

// Pin definitions for limit switches and relays
const int sw1 = 25; // Motor2KANAN1 and MOTOR2KANAN2
const int sw2 = 29; // MOTOR2KIRI
const int sw3 = 41; // VACUMON_MOTOR1ATAS and VACUMOFF_MOTOR1ATAS
const int sw4 = 45; // MOTOR1BAWAH1, MOTOR1BAWAH2, and MOTOR1BAWAH3
const int relayPin = 52;
const int vacumRelayPin = 51;
const int irSensorPin = 4;

// Enum for different states
enum State {
  START,
  MOTOR2KANAN1,
  MOTOR1BAWAH1,
  VACUMON_MOTOR1ATAS,
  MOTOR2KIRI,
  MOTOR1BAWAH2,
  VACUMOFF_MOTOR1ATAS,
  MOTOR2KANAN2,
  MOTOR1BAWAH3
};

State currentState = START;

// Create stepper objects
AccelStepper stepper1(AccelStepper::DRIVER, stepPin1, dirPin1);
AccelStepper stepper2(AccelStepper::DRIVER, stepPin2, dirPin2);
AccelStepper stepper3(AccelStepper::DRIVER, stepPin3, dirPin3);
AccelStepper stepper4(AccelStepper::DRIVER, stepPin4, dirPin4);

void setup() {
  // Initialize pins
  pinMode(enablePin1, OUTPUT);
  pinMode(enablePin2, OUTPUT);
  pinMode(enablePin3, OUTPUT);
  pinMode(enablePin4, OUTPUT);
  pinMode(sw1, INPUT_PULLUP);
  pinMode(sw2, INPUT_PULLUP);
  pinMode(sw3, INPUT_PULLUP);
  pinMode(sw4, INPUT_PULLUP);
  pinMode(relayPin, OUTPUT);
  pinMode(vacumRelayPin, OUTPUT);
  pinMode(irSensorPin, INPUT);

  // Enable motors
  digitalWrite(enablePin1, LOW);
  digitalWrite(enablePin2, LOW);
  digitalWrite(enablePin3, LOW);
  digitalWrite(enablePin4, LOW);

  // Set initial state
  currentState = START;
}

void loop() {
  switch (currentState) {
    case START:
      digitalWrite(relayPin, LOW); 
      stepper4.move(10000); 
      if (digitalRead(irSensorPin) == LOW) {
        stepper4.stop(); 
      }
      break;

    case MOTOR2KANAN1:
      stepper2.setSpeed(1000);
      stepper2.runSpeed();
      if (digitalRead(sw1) == LOW) {
        stepper2.stop(); 
        currentState = MOTOR1BAWAH1;
      }
      break;

    case MOTOR1BAWAH1:
      stepper1.setSpeed(-1000);
      stepper1.runSpeed();
      if (digitalRead(sw4) == LOW) {
        stepper1.stop(); 
        currentState = VACUMON_MOTOR1ATAS;
      }
      break;

    case VACUMON_MOTOR1ATAS:
      digitalWrite(vacumRelayPin, LOW); 
      stepper1.setSpeed(1000);
      stepper1.runSpeed();
      if (digitalRead(sw3) == LOW) {
        stepper1.stop(); 
        currentState = MOTOR2KIRI;
      }
      break;

    case MOTOR2KIRI:
      stepper2.setSpeed(-1000);
      stepper2.runSpeed();
      if (digitalRead(sw2) == LOW) {
        stepper2.stop(); 
        currentState = MOTOR1BAWAH2;
      }
      break;

    case MOTOR1BAWAH2:
      stepper1.setSpeed(-1000);
      stepper1.runSpeed();
      if (digitalRead(sw4) == LOW) {
        stepper1.stop(); 
        currentState = VACUMOFF_MOTOR1ATAS;
      }
      break;

    case VACUMOFF_MOTOR1ATAS:
      digitalWrite(vacumRelayPin, HIGH); 
      stepper1.setSpeed(1000);
      stepper1.runSpeed();
      if (digitalRead(sw3) == LOW) {
        stepper1.stop(); 
        currentState = MOTOR2KANAN2;
      }
      break;

    case MOTOR2KANAN2:
      stepper2.setSpeed(1000);
      stepper2.runSpeed();
      if (digitalRead(sw1) == LOW) {
        stepper2.stop(); 
        currentState = MOTOR1BAWAH3;
      }
      break;

    case MOTOR1BAWAH3:
      stepper1.setSpeed(-1000);
      stepper1.runSpeed();
      if (digitalRead(sw4) == LOW) {
        stepper1.stop(); 
        currentState = START; 
      }
      break;
  }

  // Run stepper motors
  stepper1.run();
  stepper2.run();
  stepper3.run();
  stepper4.run();
}