#include <AccelStepper.h>
#include <ESP32Servo.h>

// ULTRASONIC
#define trigPin 27
#define echoPin 26

// mendefinisikan kecepatan suara dalam satuan cm/uS
#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701

long duration;
float distanceCm;
float distanceInch;

// STEPPER
#define STEP1 14
#define DIR1 12

#define STEP2 19
#define DIR2 18

#define motorInterfaceType 1

AccelStepper stepper1(motorInterfaceType, STEP1, DIR1);
AccelStepper stepper2(motorInterfaceType, STEP2, DIR2);

float posStepper1 = 0;                 // Sudut awal motor stepper 1
float posStepper2 = 0;                 // Sudut awal motor stepper 2

int targetPosStepper1_degree = 180;    // Sudut akhir motor stepper 1
int targetPosStepper2_degree = 180;    // Sudut akhit motor stepper 2

int targetPosStepper1_step = 100;      // targetPosStepper1_degree * 200 / 360
int targetPosStepper2_step = 100;      // targetPosStepper2_degree * 200 / 360

int setPosStepper1;                    // Menyesuaikan step stepper 1 berdasarkan waktu
int setPosStepper2;                    // Menyesuaikan step stepper 2 berdasarkan waktu


// SERVO
#define pinServo 21

Servo servo;

int posServo = 0;                   // Sudut awal servo
int targetPosServo = 180;           // Sudut akhir servo 

int setPosServo;                    // Menyesuaikan sudut servo 1 berdasarkan waktu


// UMUM
int startTime;            // Waktu saat pertama kali program dijalankan
int errorTime;
int durationTime = 5000;            // Lama waktu stepper bergerak dalam milidetik (5 detik)


int state = 0;
int state2 = 0;

void setup() {
  Serial.begin(115200);

  pinMode(trigPin, OUTPUT);    
  pinMode(echoPin, INPUT);     

  stepper1.setMaxSpeed(1000);
  stepper2.setMaxSpeed(1000);

  stepper1.setAcceleration(1000); 
  stepper2.setAcceleration(1000);

  servo.attach(pinServo); 
  servo.write(posServo);

  startTime = millis();
  
}

void loop() {
  // Clear trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Mengatur trigPin pada HIGH state dalam waktu 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Membaca echoPin, mengembalikan waktu yang ditempuh oleh suara dalam satuan microsecond
  duration = pulseIn(echoPin, HIGH);
  
  // Menghitung jaraknya
  distanceCm = duration * SOUND_SPEED / 2;

  if(distanceCm <= 80 && state == 0) {
    errorTime = millis();
    state = 1;
  }
  
  delay(10);

  if(millis() - startTime <= durationTime + errorTime && distanceCm <= 80 && state == 1) { // mempertimbangkan faktor error
    setPosStepper1  = map(millis() - startTime, errorTime, durationTime + errorTime, posStepper1, targetPosStepper1_step);
    setPosStepper2  = map(millis() - startTime, errorTime, durationTime + errorTime, posStepper2, -targetPosStepper2_step);
    setPosServo     = map(millis() - startTime, errorTime, durationTime + errorTime, posServo   , targetPosServo);


    stepper1.moveTo(setPosStepper1);
    stepper2.moveTo(setPosStepper2);

    stepper1.run();
    stepper2.run();

    servo.write(setPosServo);
    

    Serial.print("Time taken : ");
    if (millis() - startTime - errorTime >= durationTime) { Serial.print(5000); }
    else { Serial.print(millis() - startTime - errorTime); }
    Serial.print(" | Stepper 1 angle : ");
    Serial.print(setPosStepper1 * 360 / 200);
    Serial.print(" | Stepper 2 angle : ");
    Serial.print(setPosStepper2 * 360 / 200);
    Serial.print(" | Servo angle : ");
    Serial.print(setPosServo);
    Serial.print(" | Disctance (cm) : ");
    Serial.println(distanceCm);

    if(setPosServo >= 180) {
      state2 = 1;
    }

  }
  
  if(state2 == 1) {
    stepper1.stop();
    stepper2.stop();

    servo.detach();
  }
}