#include <ESP32Servo.h>

const int trigpin1 = 26;
const int echopin1 = 25;
const int trigpin2 = 23;
const int echopin2 = 22;
const int servopin = 13;
const int resetbutton = 5;

Servo servomotor;

long duration1;
double distance1;
long duration2;
double distance2;
double maxDistance = 12;

void task1(void * parameters){
  for(;;){
  digitalWrite(trigpin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigpin1, LOW);
  int buttonstate = digitalRead(resetbutton);
  
  duration1 = pulseIn(echopin1,HIGH);
  distance1 = duration1 /58;  

  Serial.println("UDS1 Distance: ");
  Serial.println(distance1);
  vTaskDelay(1000 / portTICK_PERIOD_MS);

  if(buttonstate == LOW){
  servomotor.write(90);
  }
  else if(distance1 <= 3 ){
  servomotor.write(25);
}
  else if(distance1 <= 6 && distance1 >= 4){
  servomotor.write(50);
}
  else if(distance1 <= 11 && distance1 >= 7){
  servomotor.write(75);
}
  }
}

void task2(void * parameters){
  for(;;){
  digitalWrite(trigpin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigpin2, LOW);

  duration2 = pulseIn(echopin2,HIGH);
  distance2 = duration2 /58;
  Serial.println("UDS2 distance: ");
  Serial.println(distance2);  
  vTaskDelay(1000 / portTICK_PERIOD_MS);

if(distance2 <= 3){
  servomotor.write(115);
}
else if(distance2 <= 6 && distance2 >= 4){
  servomotor.write(140);
}
else if(distance2 <= 11 && distance2 >= 7){
  servomotor.write(165);
}
  }
}


void setup() {
  Serial.begin(115200);
  servomotor.attach(servopin);

  pinMode(trigpin1, OUTPUT); 
  pinMode(echopin1, INPUT);
  pinMode(trigpin2, OUTPUT); 
  pinMode(echopin2, INPUT); 
  pinMode(resetbutton, INPUT_PULLUP);
  servomotor.write(90); 

  xTaskCreate(
  task1,
  "Task 1",
  1000,
  NULL,
  1,
  NULL
  );

  xTaskCreate(
  task2,
  "Task 2",
  1000,
  NULL,
  1,
  NULL
  );

  
}

void loop() {

}