#include <AccelStepper.h>
#include <Servo.h>

#define FULLSTEP 8
#define A_PLUS 6
#define A_MINUS 5
#define B_PLUS 4
#define B_MINUS 3

#define A_PLUS_2 10
#define A_MINUS_2 11
#define B_PLUS_2 9
#define B_MINUS_2 8

#define A_PLUS_3 A2
#define A_MINUS_3 A3
#define B_PLUS_3 A1
#define B_MINUS_3 A0

AccelStepper stepper1(FULLSTEP, A_PLUS, A_MINUS, B_PLUS, B_MINUS);
AccelStepper stepper2(FULLSTEP, A_PLUS_2, A_MINUS_2, B_PLUS_2, B_MINUS_2);
AccelStepper stepper3(FULLSTEP, A_PLUS_3, A_MINUS_3, B_PLUS_3, B_MINUS_3);
Servo servo1;
Servo servo2;

int direction = 1;
int stepper1Position = 0;
bool arahKeKanan = true;

int maxStepperTengah = 2500;

void setup()
{
  servo1.attach(12); 
  servo2.attach(13);

  stepper1.setMaxSpeed(1000.0);
  stepper1.setAcceleration(1000);
  stepper1.setSpeed(1000);
  stepper1.moveTo(0);

  stepper2.setMaxSpeed(1000.0);
  stepper2.setAcceleration(1000);
  stepper2.setSpeed(1000);
  stepper2.moveTo(0);

  stepper3.setMaxSpeed(1000.0);
  stepper3.setAcceleration(1000);
  stepper3.setSpeed(1000);
  stepper3.moveTo(0);


  servo1.write(90); // Set posisi awal servo
  servo2.write(90);
  

  delay(1000);



}

void loop()
{
  if (stepper1.distanceToGo() == 0)
  {
    direction *= -1;
    stepper1.moveTo(direction * maxStepperTengah);
    stepper1Position = stepper1.currentPosition();
    Serial.println(stepper1Position);

    if (stepper1Position == -1*maxStepperTengah)
    {
      int stepsToAdd = 360 / 1.8 * 2; // 360 derajat pada mode 1/16 mikrostep
      int stepper2Position = stepper2.currentPosition();
      int stepper2MoveTo = stepper2.currentPosition() + stepsToAdd;
      stepper2.moveTo(stepper2MoveTo);
      stepper3.moveTo(-1*stepper2MoveTo);
      // stepper2.moveTo(-1*(stepper3.currentPosition() + stepsToAdd));

      
    }else{
      putarArah();
    }
  }

  stepper1.run();
  stepper2.run();
  stepper3.run();
}

void putarArah(){
if(arahKeKanan){
servo1.write(180);
  servo2.write(0);
  arahKeKanan = false;
}else{
  servo1.write(0);
servo2.write(180);
arahKeKanan = true;

}

}