/*
  HC-SR04 Ultrasonic Sensor Example.

  Turn the LED on when an object is within 100cm range.

  Copyright (C) 2021, Uri Shaked
*/

#define PIN_SW_ECHO 2
#define PIN_SW_TRIG 3
#define PIN_RW_ECHO 4
#define PIN_RW_TRIG 5
#define PIN_RELAY1 8
#define PIN_RELAY2 9
#define PIN_SEL_AUTO 13
#define PIN_SEL_SW 12
#define PIN_SEL_RW 11
#define PIN_SEL_OVERRULE 10

void setup() {
  Serial.begin(115200);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(PIN_SW_TRIG, OUTPUT);
  pinMode(PIN_SW_ECHO, INPUT);
  pinMode(PIN_RW_TRIG, OUTPUT);
  pinMode(PIN_RW_ECHO, INPUT);
  pinMode(PIN_RELAY1, OUTPUT);
  pinMode(PIN_RELAY2, OUTPUT);
  pinMode(PIN_SEL_AUTO, INPUT_PULLUP);
  pinMode(PIN_SEL_SW, INPUT_PULLUP);
  pinMode(PIN_SEL_RW, INPUT_PULLUP);
  pinMode(PIN_SEL_OVERRULE, INPUT_PULLUP);
}

float readDistance1() {
  digitalWrite(PIN_SW_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_SW_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_SW_TRIG, LOW);
  int duration = pulseIn(PIN_SW_ECHO, HIGH);
  return duration * 0.034 / 2;
  //delay(1000);
}

float readDistance2() {
  digitalWrite(PIN_RW_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_RW_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_RW_TRIG, LOW);
  int duration = pulseIn(PIN_RW_ECHO, HIGH);
  return duration * 0.034 / 2;
  //delay(1000);
}

void TankLevels(){
  bool TANK_SEL_AUTO = digitalRead((PIN_SEL_AUTO));
  bool TANK_SEL_SW = digitalRead((PIN_SEL_SW));
  bool TANK_SEL_RW = digitalRead((PIN_SEL_RW));
  bool Tank_Level_Overrule = digitalRead((PIN_SEL_OVERRULE));
  int TankSelector = 0; //1=SW, 2=RW
  int SelectedTank = 0; //0=AUTO, 1=SW, 2=RW

  float SWTankLevel = readDistance1();
  float RWTankLevel = readDistance2();

  // *** TANK SELECTOR SET TO AUTO ***
  if(TANK_SEL_AUTO == 0){ //inverted because pullup
    Serial.println("AUTO Tank Selection Detected.");
    if(SWTankLevel >= 100 ){
      TankSelector = 1;
      digitalWrite(PIN_RELAY1, 1);
      digitalWrite(PIN_RELAY2, 0);
      Serial.println("1. Sludge Water Tank Selected.");
    }
    else if((SWTankLevel < 150) & (RWTankLevel >= 100)){
      TankSelector = 2;
      digitalWrite(PIN_RELAY2, 1);
      digitalWrite(PIN_RELAY1, 0);
      Serial.println("2. Rain Water Tank Selected");
    }
    else{
      TankSelector = 0;
      Serial.println("3. ABORTED: AUTO Watering Failed: The level of both tanks is to low.");
    }
  }
  // *** TANK SELECTOR SET TO SLUDGE WATER ***
  else if(TANK_SEL_SW == 0){
    Serial.println("Sludge Water Tank Selection Detected.");
    if(Tank_Level_Overrule == 1){
      if(SWTankLevel >= 100){
        TankSelector = 1;
        digitalWrite(PIN_RELAY1, 1);
        digitalWrite(PIN_RELAY2, 0);
        Serial.println("4. Sludge Water Tank was Selected Manally.");
      }
      else{
        Serial.println("5. ABORTED: The level of the Sludge Water Tank is to low.");
      }
    }
    else if(Tank_Level_Overrule == 0){
      TankSelector = 1;
      digitalWrite(PIN_RELAY1, 1);
      digitalWrite(PIN_RELAY2, 0);
      Serial.println("6. WARNING: Tank Level Overrule Activated. Sludge Water Tank was Selected Manally");
    }
  }
  // *** TANK SELECTOR SET TO RAIN WATER ***
  else if(TANK_SEL_RW == 0){
    Serial.println("Rain Water Tank Selection Detected.");
    if(Tank_Level_Overrule == 1){
      if(RWTankLevel >= 100){
        TankSelector = 2;
        digitalWrite(PIN_RELAY2, 1);
        digitalWrite(PIN_RELAY1, 0);
        Serial.println("7. Rain Water Tank was Selected Manally.");
      }
      else{
        Serial.println("8. ABORTED: The level of the Rain Water Tank is to low.");
      }
    }
    else if(Tank_Level_Overrule == 0){
      TankSelector = 2;
      digitalWrite(PIN_RELAY2, 1);
      digitalWrite(PIN_RELAY1, 0);
      Serial.println("9. WARNING: Tank Level Overrule Activated. Rain Water Tank was Selected Manally");
    }
  }
}

void loop() {
  bool debug = 0;

  if(debug == 1) {
    Serial.print("TANK_SEL_AUTO: ");
    Serial.println(digitalRead((PIN_SEL_AUTO)));
    Serial.print("TANK_SEL_SW: ");
    Serial.println(digitalRead((PIN_SEL_SW)));
    Serial.print("TANK_SEL_RW: ");
    Serial.println(digitalRead((PIN_SEL_RW)));
    Serial.print("BTN_OVERRULE: ");
    Serial.println(digitalRead((PIN_SEL_OVERRULE)));
    Serial.print("Sludge Water Tank Level: ");
    Serial.println(readDistance1());
    Serial.print("Rain Water Tank Level: ");
    Serial.println(readDistance2());
    Serial.println("---");
  }
  
  TankLevels();
  Serial.println("----- EOS -----");
  delay(5000);
}