#define MICROSECONDS_PER_TICK (portTICK_PERIOD_MS / 1000)

//Uri 1 ========================================================================
const int trig1 = 33;
const int echo1 = 32;
const int relay1 = 4;

long duration1;
int distance1;
int threshold1 = 61;
int count1 = 0;
int wait1 = 0;

void uri1(void * parameters) {
  for(;;){
    // Clears the trigPin
    digitalWrite(trig1, LOW);
    vTaskDelay(pdMS_TO_TICKS(2));

    // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(trig1, HIGH);
    vTaskDelay(pdMS_TO_TICKS(10));
    digitalWrite(trig1, LOW);

    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration1 = pulseIn(echo1, HIGH);

    // Calculating the distance
    distance1 = duration1 * 0.034/2;

    if (distance1 <= threshold1){
      wait1 = 1;
      count1 = 0;
    }

    if (wait1 == 1){
      if (distance1 > threshold1){
        digitalWrite(relay1, LOW);
        count1 ++;
        vTaskDelay(1000 / portTICK_PERIOD_MS);
        if (count1 > 10){
          wait1 = 0;
          count1 = 0;
        }
      }
    }

    if (count1 == 0){
      digitalWrite(relay1, HIGH);
    }
  }
}

//Uri 2 ========================================================================
const int trig2 = 26;
const int echo2 = 25;
const int relay2 = 0;

long duration2;
int distance2;
int threshold2 = 61;
int count2 = 0;
int wait2 = 0;

void uri2(void * parameters) {
  for(;;){
    // Clears the trigPin
    digitalWrite(trig2, LOW);
    vTaskDelay(pdMS_TO_TICKS(2));

    // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(trig2, HIGH);
    vTaskDelay(pdMS_TO_TICKS(10));
    digitalWrite(trig2, LOW);

    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration2 = pulseIn(echo2, HIGH);

    // Calculating the distance
    distance2 = duration2 * 0.034/2;

    if (distance2 <= threshold2){
      wait2 = 1;
      count2 = 0;
    }

    if (wait2 == 1){
      if (distance2 > threshold2){
        digitalWrite(relay2, LOW);
        count2 ++;
        vTaskDelay(1000 / portTICK_PERIOD_MS);
        if (count2 > 10){
          wait2 = 0;
          count2 = 0;
        }
      }
    }

    if (count2 == 0){
      digitalWrite(relay2, HIGH);
    }
  }
}

//Uri 3 ========================================================================
const int trig3 = 14;
const int echo3 = 27;
const int relay3 = 2;

long duration3;
int distance3;
int threshold3 = 61;
int count3 = 0;
int wait3 = 0;

void uri3(void * parameters) {
  for(;;){
    // Clears the trigPin
    digitalWrite(trig3, LOW);
    vTaskDelay(pdMS_TO_TICKS(2));

    // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(trig3, HIGH);
    vTaskDelay(pdMS_TO_TICKS(10));
    digitalWrite(trig3, LOW);

    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration3 = pulseIn(echo3, HIGH);

    // Calculating the distance
    distance3 = duration3 * 0.034/2;

    if (distance3 <= threshold3){
      wait3 = 1;
      count3 = 0;
    }

    if (wait3 == 1){
      if (distance3 > threshold3){
        digitalWrite(relay3, LOW);
        count3 ++;
        vTaskDelay(1000 / portTICK_PERIOD_MS);
        if (count3 > 10){
          wait3 = 0;
          count3 = 0;
        }
      }
    }

    if (count3 == 0){
      digitalWrite(relay3, HIGH);
    }
  }
}

//Uri 4 ========================================================================
const int trig4 = 13;
const int echo4 = 12;
const int relay4 = 15;

long duration4;
int distance4;
int threshold4 = 61;
int count4 = 0;
int wait4 = 0;

void uri4(void * parameters) {
  for(;;){
    // Clears the trigPin
    digitalWrite(trig4, LOW);
    vTaskDelay(pdMS_TO_TICKS(2));

    // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(trig4, HIGH);
    vTaskDelay(pdMS_TO_TICKS(10));
    digitalWrite(trig4, LOW);

    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration4 = pulseIn(echo4, HIGH);

    // Calculating the distance
    distance4 = duration4 * 0.034/2;

    if (distance4 <= threshold4){
      wait4 = 1;
      count4 = 0;
    }

    if (wait4 == 1){
      if (distance4 > threshold4){
        digitalWrite(relay4, LOW);
        count4 ++;
        vTaskDelay(1000 / portTICK_PERIOD_MS);
        if (count4 > 10){
          wait4 = 0;
          count4 = 0;
        }
      }
    }

    if (count4 == 0){
      digitalWrite(relay4, HIGH);
    }
  }
}

void setup() {
  Serial.begin(115200);

//Uri 1 ========================================================================
  pinMode(trig1, OUTPUT); 
  pinMode(echo1, INPUT); 
  pinMode(relay1, OUTPUT);

  xTaskCreatePinnedToCore(
  uri1,
  "Urinal One",
  5000,
  NULL,
  1,
  NULL,
  1
  );

//Uri 2 ========================================================================
  pinMode(trig2, OUTPUT); 
  pinMode(echo2, INPUT); 
  pinMode(relay2, OUTPUT);

  xTaskCreatePinnedToCore(
  uri2,
  "Urinal Two",
  5000,
  NULL,
  1,
  NULL,
  1
  );

//Uri 3 ========================================================================
  pinMode(trig3, OUTPUT); 
  pinMode(echo3, INPUT); 
  pinMode(relay3, OUTPUT);

  xTaskCreatePinnedToCore(
  uri3,
  "Urinal Three",
  5000,
  NULL,
  1,
  NULL,
  0
  );

//Uri 4 ========================================================================
  pinMode(trig4, OUTPUT); 
  pinMode(echo4, INPUT); 
  pinMode(relay4, OUTPUT);

  xTaskCreatePinnedToCore(
  uri4,
  "Urinal Four",
  5000,
  NULL,
  1,
  NULL,
  0
  );
}



void loop() {

}