// Pin Sleep A4988 Pulled Up (Active Low)
// Pin Reset A4988 Floating (Active Low)

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

// Servo Object
Servo servo;

// Ultrasonic Sensor
#define PIN_TRIG 13
#define PIN_ECHO 12
float duration;
float distance;

// Stepper Motor Object(motorInterfaceType, stepPin, dirPin)
AccelStepper stepper1(1,27,26);  // motorInterfaceType == 1 (using driver)
AccelStepper stepper2(1,25,33);  // motorInterfaceType == 1 (using driver)
#define stepsPerRevolution 400  // Half-Stepping: 0.9 degrees per step
#define acceleration 1000  // 1000 steps per second per second
float steps1 = (300.0/360)*stepsPerRevolution;
float steps2 = (-150.0/360)*stepsPerRevolution;
float simulationTime = 5.0;  // 5 detik
int angle = 10;

void moveServo(void *param);
void moveStepper(void *param);
void UltrasonicInput(void *param);

void moveServo(void *param){
  for (; angle <= 150; angle++) {
    servo.write(angle);
    vTaskDelay((5000 / 140) / portTICK_PERIOD_MS);  // 5 detik / (150-10) derajat
  }
  vTaskDelete(NULL);
}

void moveStepper(void *param){
  while(1){
    stepper1.run();
    stepper2.run();
    vTaskDelay(20 / portTICK_PERIOD_MS);
  }
}

void ultrasonicInput(void *param){
  while(1){
    digitalWrite(PIN_TRIG, HIGH);
    vTaskDelay(10 / portTICK_PERIOD_MS);
    digitalWrite(PIN_TRIG, LOW);
    // duration in us
    duration = pulseIn(PIN_ECHO, HIGH);  
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("Start!");

  servo.attach(23);

  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  // Set the maximum speed in steps per second
  stepper1.setMaxSpeed(steps1/simulationTime);
  stepper2.setMaxSpeed(steps2/simulationTime);

  stepper1.setAcceleration(acceleration);
  stepper2.setAcceleration(acceleration);

  stepper1.moveTo(steps1);
  stepper2.moveTo(steps2);

  xTaskCreate(moveServo, "ServoTask", 2048, NULL, 1, NULL);
  xTaskCreate(moveStepper, "StepperTask", 2048, NULL, 1, NULL);
  xTaskCreate(ultrasonicInput, "UltrasonicTask", 2048, NULL, 1, NULL);
}

void loop() {
  distance = duration * 0.017;
  Serial.print("Distance in cm: ");
  Serial.println(distance);
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}