#if CONFIG_FREERTOS_UNICORE
  static const BaseType_t app_cpu = 0;
#else
  static const BaseType_t app_cpu = 1;
#endif

#define LED 2
#include <ESP32Servo.h>
Servo servo;

unsigned long startTimeRead, stopTimeRead, cpuTime1, startTimeOverall , stopTimeLed ,stopTimeOverall , cpuTimeOverall , cpuTime3,  cpuTime2, startTimeMove, startTimeLed, stopTimeMove ;

int const trigPin = 27;
int const echoPin = 26; 
const int servoPin = 13;
float distance ;

void setup() {
  Serial.begin(115200);
  pinMode(LED, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  servo.attach(servoPin, 500, 2400);
  startTimeOverall = micros();
  xTaskCreatePinnedToCore(ultraread,"ultraread",1024,NULL,2,NULL,app_cpu);
  xTaskCreatePinnedToCore(open,"open",1024,NULL,1,NULL,app_cpu);
  
}


void ultraread(void *pt){
  for(;;){
    int duration;
    startTimeLed = micros();
    digitalWrite(LED, HIGH);
    stopTimeLed = micros();
    
    startTimeRead = micros();
    digitalWrite(trigPin, HIGH);
    delay(1);
    digitalWrite(trigPin, LOW);
    
    duration = pulseIn(echoPin, HIGH);
    distance = (duration/2)/ 29.1;
    
    stopTimeRead = micros();
    cpuTime1 = stopTimeRead - startTimeRead;
    stopTimeOverall = micros();
  }
  
}

void open(void *pt){
  for(;;){
    if (distance <= 50 && distance >=0){
      startTimeMove = micros();
      servo.write(50);
      stopTimeMove = micros();
      
      cpuTime2 = stopTimeMove - startTimeMove;
      cpuTime3 = stopTimeLed - startTimeLed;
      cpuTimeOverall = cpuTime1 + cpuTime2 + cpuTime3;
      Serial.print("Distance (cm): ");
      Serial.println(distance);
      
      Serial.print("CPU Time 1 Ultrasonic (us): ");
      Serial.println (cpuTime1);
      Serial.print("CPU Time 2 Servo (us): ");
      Serial.println (cpuTime2);
      Serial.print("CPU Time 3 LED (us): ");
      Serial.println (cpuTime3);
      Serial.print("CPU Time Overall (us): ");
      Serial.println (cpuTimeOverall);
      Serial.println("---");
      delay(3000);
  }
  else{
    servo.write(160);
  }
  }
  
}


void loop() {

}