#define MICROSECONDS_PER_TICK (portTICK_PERIOD_MS / 1000)
int threshold = 61;
int senseTime = 2;
int flushTime = 7;

const int relay[] = {4, 0, 2, 15};  // define relays
const int trig[] = {33, 26, 14, 13};  // define trigs
const int echo[] = {32, 25, 27, 12};  // define echo
const int valNum[] = {0, 1, 2, 3};  // define echo index
const int numPins = 4;

long duration[4];  // Array to store duration for each sensor
int distance[4];  // Array to store distance for each sensor
int count[4] = {0};  // Array to store count for each sensor, initialized to 0
int wait[4] = {0};  // Array to store wait for each sensor, initialized to 0
int tFlush[4] = {0};  // Array to store tFlush for each sensor, initialized to 0

void uriCore0(void *pvParameters) {
  while (1) {
    for (int i = 0; i < 2; i++) {
      // Clears the trigPin
      digitalWrite(trig[i], LOW);
      vTaskDelay(pdMS_TO_TICKS(2));

      // Sets the trigPin on HIGH state for 10 microseconds
      digitalWrite(trig[i], HIGH);
      vTaskDelay(pdMS_TO_TICKS(10));
      digitalWrite(trig[i], LOW);

      // Reads the echoPin, returns the sound wave travel time in microseconds
      duration[valNum[i]] = pulseIn(echo[i], HIGH);

      // Calculating the distance
      distance[valNum[i]] = duration[valNum[i]] * 0.034 / 2;

      if (distance[valNum[i]] <= threshold && wait[valNum[i]] != 1) {
        if (count[valNum[i]] <= senseTime) {
          count[valNum[i]]++;
          vTaskDelay(1000 / portTICK_PERIOD_MS);
        }

        if (count[valNum[i]] > senseTime) {
          count[valNum[i]] = 0;
          wait[valNum[i]] = 1;
        }
      }

      if (count[valNum[i]] <= senseTime && distance[valNum[i]] > threshold && wait[valNum[i]] != 1) {
        count[valNum[i]] = 0;
      }

      if (wait[valNum[i]] == 1) {
        if (distance[valNum[i]] > threshold) {
          digitalWrite(relay[i], LOW);
          tFlush[valNum[i]]++;
          vTaskDelay(1000 / portTICK_PERIOD_MS);
          if (tFlush[valNum[i]] > flushTime) {
            tFlush[valNum[i]] = 0;
            wait[valNum[i]] = 0;
          }
        }

        if (distance[valNum[i]] <= threshold) {
          tFlush[valNum[i]] = 0;
          digitalWrite(relay[i], HIGH);
        }
      }

      if (wait[valNum[i]] == 0) {
        digitalWrite(relay[i], HIGH);
      }
    }
  }
}

void uriCore1(void *pvParameters) {
  while (1) {
    for (int i = 2; i < 4; i++) {
      // Clears the trigPin
      digitalWrite(trig[i], LOW);
      vTaskDelay(pdMS_TO_TICKS(2));

      // Sets the trigPin on HIGH state for 10 microseconds
      digitalWrite(trig[i], HIGH);
      vTaskDelay(pdMS_TO_TICKS(10));
      digitalWrite(trig[i], LOW);

      // Reads the echoPin, returns the sound wave travel time in microseconds
      duration[valNum[i]] = pulseIn(echo[i], HIGH);

      // Calculating the distance
      distance[valNum[i]] = duration[valNum[i]] * 0.034 / 2;

      if (distance[valNum[i]] <= threshold && wait[valNum[i]] != 1) {
        if (count[valNum[i]] <= senseTime) {
          count[valNum[i]]++;
          vTaskDelay(1000 / portTICK_PERIOD_MS);
        }

        if (count[valNum[i]] > senseTime) {
          count[valNum[i]] = 0;
          wait[valNum[i]] = 1;
        }
      }

      if (count[valNum[i]] <= senseTime && distance[valNum[i]] > threshold && wait[valNum[i]] != 1) {
        count[valNum[i]] = 0;
      }

      if (wait[valNum[i]] == 1) {
        if (distance[valNum[i]] > threshold) {
          digitalWrite(relay[i], LOW);
          tFlush[valNum[i]]++;
          vTaskDelay(1000 / portTICK_PERIOD_MS);
          if (tFlush[valNum[i]] > flushTime) {
            tFlush[valNum[i]] = 0;
            wait[valNum[i]] = 0;
          }
        }

        if (distance[valNum[i]] <= threshold) {
          tFlush[valNum[i]] = 0;
          digitalWrite(relay[i], HIGH);
        }
      }

      if (wait[valNum[i]] == 0) {
        digitalWrite(relay[i], HIGH);
      }
    }
  }
}

void setup() {
  for (int i = 0; i < numPins; i++) {
    pinMode(relay[i], OUTPUT);
    pinMode(trig[i], OUTPUT);
    pinMode(echo[i], INPUT);
  }

  xTaskCreatePinnedToCore(uriCore0, "Urinal Core0", 5000, NULL, 1, NULL, 0);
  xTaskCreatePinnedToCore(uriCore1, "Urinal Core1", 5000, NULL, 1, NULL, 1);
}

void loop() {
  // The loop function is empty because FreeRTOS tasks are used.
}