#define CORE1 0
#define CORE2 1

#include <Arduino.h>
#include <ESP32Servo.h>

const int trigPin = 21; // Pin del trigger del sensor ultrasónico
const int echoPin = 22; // Pin del echo del sensor ultrasónico
const int servoPin = 5; // Pin al que está conectado el servo
const int minAngle = 0; // Ángulo mínimo del servo
const int maxAngle = 180; // Ángulo máximo del servo
const int minDistance = 5; // Distancia mínima para el servo (cm)
const int maxDistance = 50; // Distancia máxima para el servo (cm)

Servo myServo;

void TareaServo(void *pvParameters) {
  (void) pvParameters;
  
  for (;;) {
    int distance = lecturaSensorUltrasonico(); // Leer la distancia del sensor

    if (distance >= maxDistance) {
      moveServo(maxAngle); // Mover el servo a su posición máxima
    } else if (distance <= minDistance) {
      moveServo(minAngle); // Mover el servo a su posición mínima
    } else {
      int angle = map(distance, minDistance, maxDistance, minAngle, maxAngle); // Mapear la distancia al ángulo del servo
      angle = constrain(angle, minAngle, maxAngle); // Limitar el ángulo dentro del rango permitido
      moveServo(angle); // Mover el servo al ángulo calculado
    }
    vTaskDelay(pdMS_TO_TICKS(20)); // Pequeño retardo entre las lecturas del sensor
  }
}

void TareaSensorUltrasonico(void *pvParameters) {
  (void) pvParameters;

  for (;;) {
    long duration = pulseIn(echoPin, HIGH); // Mide la duración del pulso en el pin del echo
    long distance = (duration / 2) / 29.1; // Calcula la distancia en centímetros
    Serial.print("Distancia: ");
    Serial.print(distance);
    Serial.println(" cm");
    vTaskDelay(pdMS_TO_TICKS(2000)); // Espera dos segundos antes de tomar otra medición
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myServo.attach(servoPin);
  
  xTaskCreatePinnedToCore(TareaServo, "TareaServo", 2048, NULL, 1, NULL, CORE1);
  xTaskCreatePinnedToCore(TareaSensorUltrasonico, "TareaSensorUltrasonico", 2048, NULL, 3, NULL, CORE2);
}

void loop() {
  // No es necesario tener código en el bucle principal para tareas en RTOS
}

int lecturaSensorUltrasonico() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  long distance = (duration / 2) / 29.1;
  return distance;
}

void moveServo(int angle) {
  myServo.write(angle); // Mover el servo al ángulo especificado
  delay(20); // Pequeño retardo para asegurar la estabilidad del servo
}