#include <Ultrasonic.h>
#include <Relay.h>
#include <ESP32Servo.h>

const int pin_trig = 14;
const int pin_echo = 27;  // Change this to the appropriate pin for your solenoid valve relay
const int servo_pin_1 = 26;
const int servo_pin_2 = 25;
const int relaypin = 4;

Ultrasonic ultrasonic(pin_trig, pin_echo);
Relay solenoidRelay(4, true);  // Adjust the second argument based on relay characteristics
Servo servo1;
Servo servo2;

void UDS_Task(void *parameters) {
  (void)parameters;
  for (;;) {
    int distance = ultrasonic.read();

    if (distance < 30) {
      openTrashcan();
      vTaskDelay(pdMS_TO_TICKS(500));  // Wait for a short duration before activating the solenoid valve
    } else {
      closeTrashcan();
      vTaskDelay(pdMS_TO_TICKS(1000));
    }
  }
}

void relayTask (void * parameters){
  (void) parameters;
  for(;;){
    int detect = ultrasonic.read();

    if (detect < 30){
      digitalWrite(relaypin, HIGH);
    }
    else {
    digitalWrite(relaypin, LOW);
  }

}
}


void setup() {
  pinMode(servo_pin_1, OUTPUT);
  pinMode(servo_pin_2, OUTPUT);
  pinMode(relaypin, OUTPUT);
  servo1.attach(servo_pin_1);
  servo2.attach(servo_pin_2);

  servo1.write(0);
  servo2.write(0);

  solenoidRelay.begin();  // Initialize the solenoid valve relay

  xTaskCreate(UDS_Task, "UDS task", 1000, NULL, 1, NULL);
  xTaskCreate(relayTask, "Relay task", 1000, NULL, 2, NULL);
}

void loop() {
}

void openTrashcan() {
  servo1.write(90);
  servo2.write(90);
}

void closeTrashcan() {
  servo1.write(0);
  servo2.write(0);
}