// Ultrasonic Sensor Variables
#define TRIG_D 1
#define ECHO_D 3
#define TRIG_CAL 5
#define ECHO_CAL 18

bool scanMode = 0;
float sound_speed = 343;
long duration_d = 0;
long duration_cal = 0;

//Ambient sensor parameters
float d_cal_normal = 2;
float d_cal_current = 2;
float d_cal_limit = 5;
float d_cal_diff = 0;

//Ice build-up sensor parameters
float d_normal = 10;
float d_current = 10;
float d_diff = 0;
float d_threshold = 5;
float d_limit = 10;

// Delay variables
unsigned long currentMillis = millis();
unsigned long previousMillis_ON_5 = 0;
unsigned long previousMillis_OFF_5 = 0;
unsigned long interval_5 = 5000UL;

unsigned long previousMillis_1 = 0;
unsigned long interval_1 = 1000;

unsigned long previousMillis_2u = 0;
unsigned long interval_2u = 0.002;

unsigned long previousMillis_10u = 0;
unsigned long interval_10u = 0.01;

//MQTT data transmission parameters
bool error_1_val = 0;
bool error_2_val = 0;
bool current_shower_status = 0;
bool current_shower_timeout_status = 0;
bool current_pipe_status = 0;

void d_cal_diff_fun(){
  // Calculate error due to variations in
  //ambient conditions for readings of a constant distance
  d_cal_current = ((sound_speed * duration_cal)/2) * 100;
  d_cal_diff = d_cal_current - d_cal_normal;
}
void d_diff_fun(){
  //Remove errors due to variations in ambient conditions
  d_current = ((sound_speed * duration_d)/2) * 100;
  d_current = d_current - d_cal_diff;
  
  //Determine extent of ice build-up
  d_diff = d_current - d_normal;
} 

void pipeStatus(){
  //Start or Stop Deicing and inform of pipe status change
  //Okay = 0; Frozen = 1
  if (!current_pipe_status){
    current_pipe_status = 1;
    Serial.println("Pipe Frozen...");
  }
  else{
    current_pipe_status = 0;
    Serial.println("Pipe OK...");
  }
}
                    
void showerStatus(){
  //Turn shower ON or OFF and inform of status change
  if (!current_shower_status){
    current_shower_status = 1;
    Serial.println("system running...");
  }
  else{
    current_shower_status = 0;
    Serial.println("System idle...");
  }
}

void error_1_reset(){    
  error_1_val = 0;
}

void error_1(){
  if (!error_1_val){
    error_1_val = 1;
    Serial.println("E01: Reading Out of Range...");
  }
}

void setup(){
  Serial.begin(115200);
  Serial.println("Welcome Message");

  // Set up the ultrasonic sensor pins
  pinMode(TRIG_D, OUTPUT);
  pinMode(TRIG_CAL, OUTPUT);
  pinMode(ECHO_D, INPUT);
  pinMode(ECHO_CAL, INPUT);
}
void loop(){
  currentMillis = millis();
  
  //Check ice build-up level
  d_cal_diff_fun();
  d_diff_fun();
  
  if(currentMillis - previousMillis_2u > interval_2u && !scanMode){
    digitalWrite(TRIG_D, HIGH);
    digitalWrite(TRIG_CAL, HIGH);
    scanMode = 1;

    previousMillis_10u = currentMillis;
  }

  if (currentMillis - previousMillis_10u > interval_10u && scanMode){
    digitalWrite(TRIG_D, LOW);
    digitalWrite(TRIG_CAL, LOW);
    scanMode = 0;
    duration_d = pulseIn(ECHO_D, HIGH);
    duration_cal = pulseIn(ECHO_CAL, HIGH);

    previousMillis_2u = currentMillis;
  }

  if (d_diff >= d_threshold && d_diff < d_limit){
      
    //reset error values after errors are cleared
    //allow sending of them again through MQTT
    error_1_reset();
    
    //Ice build-up beyond limit; Start deicing process
    if (d_current > d_normal){
      
      //Turn on shower and inform of status change
      if (!current_shower_status){
        showerStatus();
        // Start timer for ON Time
        previousMillis_ON_5 = currentMillis;
      } 
      //Start Deicing and inform of pipe status change
      if (!current_pipe_status){
        pipeStatus();
      }
      
      Serial.print("Build_up: ");
      Serial.println(d_diff);               
                      
      //Update current readings
      d_cal_diff_fun();
      d_diff_fun();
      
      
      //To save on water; Turn off supply then turn on after a while
      if(currentMillis - previousMillis_ON_5 > interval_5 && !current_shower_timeout_status){
        Serial.println("Shower Timeout...");
        // Turn OFF shower
        current_shower_timeout_status = 1;
        previousMillis_OFF_5 = currentMillis;
      }
      if(currentMillis - previousMillis_OFF_5 > interval_5 && current_shower_timeout_status){
        Serial.println("Shower Resume...");
        // Turn ON shower
        current_shower_timeout_status = 0;
        previousMillis_ON_5 = currentMillis;
      }          
      //Update current readings
        d_cal_diff_fun();
        d_diff_fun();
    }
  }                    
                      
  //Display error if ice build-up is beyond the set limit 
  else if(d_diff >= d_limit){
    error_1();
  }                
              
  //Normal condition; Low to no ice build-up
  else{
    //Turn off shower and inform of status change
    if (current_shower_status){           
      showerStatus();
    }
    
    //Stop Deicing and inform of pipe status change
    if (current_pipe_status){
      pipeStatus();
    }
                
    //Simulation of ice build-up
    d_current +=1;
      
    if(currentMillis - previousMillis_1 > interval_1){
      Serial.print("Build_up: ");
      Serial.println(d_diff);
      previousMillis_1 = currentMillis;        
    }
  }
}