#include <Servo.h>

const uint8_t mixer = 23;           //AC relay for mixing

//Bath soap
const uint8_t BS_low_level = 24;    //Float sensor for low level
const uint8_t BS_high_level = 22;   //Float sensor for high level
const uint8_t BS_water = 27;        //AC relay for dispensing water to bath soap container
const uint8_t BS_dispense = 33;     //AC relay for dispensing bath soap to container
const uint8_t BS_liquid_open = 39;  //DC relay for opening dispenser for liquid mixture to bath soap container
const uint8_t BS_liquid_close = 41; //DC relay for closing dispenser for liquid mixture to bath soap container
const uint8_t BS_dispense_PB = 40;  //Push button for dispensing bath soap
const uint8_t BS_dispense_LED = 34; //LED indicator if ok for dispensing 
Servo BS_powder;

//Dishwashing liquid
const uint8_t DL_low_level = 28;    //Float sensor for low level
const uint8_t DL_high_level = 26;   //Float sensor for high level
const uint8_t DL_water = 29;        //AC relay for dispensing water to dishwashing liquid container
const uint8_t DL_dispense = 31;     //AC relay for dispensing dishwashing liquid to container
const uint8_t DL_liquid_open = 43;  //DC relay for opening dispenser for liquid mixture to dishwashing liquid container
const uint8_t DL_liquid_close = 45; //DC relay for opening dispenser for liquid mixture to dishwashing liquid container
const uint8_t DL_dispense_PB = 42;  //Push button for dispensing dishwashing liquid
const uint8_t DL_dispense_LED = 36; //LED indicator if ok for dispensing 
Servo DL_powder;

//Car-wash soap
const uint8_t CS_low_level = 32;    //Float sensor for low level
const uint8_t CS_high_level = 30;   //Float sensor for high level
const uint8_t CS_water = 25;        //AC relay for dispensing water to car-wash soap container
const uint8_t CS_dispense = 35;     //AC relay for dispensing car-wash soap to container
const uint8_t CS_liquid_open = 47;  //DC relay for opening dispenser for liquid mixture to car-wash soap container
const uint8_t CS_liquid_close = 49; //DC relay for opening dispenser for liquid mixture to car-wash soap container
const uint8_t CS_dispense_PB = 44;  //Push button for dispensing car-wash soap
const uint8_t CS_dispense_LED = 38; //LED indicator if ok for dispensing 
Servo CS_powder;

/* process latch */
bool BS_low_level_status = false;
bool DL_low_level_status = false;
bool CS_low_level_status = false;
bool all_low_level_status = false;
bool BSDL_low_level_status = false;
bool DLCS_low_level_status = false;
bool BSCS_low_level_status = false;
bool BS_on_process = false;
bool DL_on_process = false;
bool CS_on_process = false;

/* process previous time handler */
unsigned long BS_prev_time = 0;
unsigned long DL_prev_time = 0;
unsigned long CS_prev_time = 0;

/* servo open and close angles */
uint8_t BSopen_angle = 65;
uint8_t BSclose_angle = 180;
uint8_t DLopen_angle = 90;
uint8_t DLclose_angle = 0;
uint8_t CSopen_angle = 90;
uint8_t CSclose_angle = 15;

/* unused relays */
//const uint8_t relay_AC8 = 37;
//const uint8_t relay_DC7 = 51;
//const uint8_t relay_DC8 = 53;

void setup() {
/* DEBUG */
  Serial.begin(9600);

/* Assign pins for Servos */
  BS_powder.attach(7);
  DL_powder.attach(6);
  CS_powder.attach(5);
  BS_powder.write(BSclose_angle);
  delay(500);
  DL_powder.write(DLclose_angle);
  delay(500);
  CS_powder.write(CSclose_angle);
  delay(500);
  
/* Assign pins as INPUT or OUTPUT */
  pinMode(mixer, OUTPUT);

  pinMode(BS_low_level, INPUT_PULLUP);
  pinMode(BS_high_level, INPUT_PULLUP);
  pinMode(BS_dispense_PB, INPUT_PULLUP);
  pinMode(BS_water, OUTPUT);
  pinMode(BS_dispense, OUTPUT);
  pinMode(BS_liquid_open, OUTPUT);
  pinMode(BS_liquid_close, OUTPUT);
  pinMode(BS_dispense_LED, OUTPUT);

  pinMode(DL_low_level, INPUT_PULLUP);
  pinMode(DL_high_level, INPUT_PULLUP);
  pinMode(DL_dispense_PB, INPUT_PULLUP);
  pinMode(DL_water, OUTPUT);
  pinMode(DL_dispense, OUTPUT);
  pinMode(DL_liquid_open, OUTPUT);
  pinMode(DL_liquid_close, OUTPUT);
  pinMode(DL_dispense_LED, OUTPUT);

  pinMode(CS_low_level, INPUT_PULLUP);
  pinMode(CS_high_level, INPUT_PULLUP);
  pinMode(CS_dispense_PB, INPUT_PULLUP);
  pinMode(CS_water, OUTPUT);
  pinMode(CS_dispense, OUTPUT);
  pinMode(CS_liquid_open, OUTPUT);
  pinMode(CS_liquid_close, OUTPUT);
  pinMode(CS_dispense_LED, OUTPUT);

/* Turn off outputs first */
 digitalWrite(mixer,HIGH);
 
 digitalWrite(BS_water,HIGH);
 digitalWrite(BS_dispense,HIGH);
 digitalWrite(BS_liquid_open,HIGH);
 digitalWrite(BS_liquid_close,HIGH);

 digitalWrite(DL_water,HIGH);
 digitalWrite(DL_dispense,HIGH);
 digitalWrite(DL_liquid_open,HIGH);
 digitalWrite(DL_liquid_close,HIGH);

 digitalWrite(CS_water,HIGH);
 digitalWrite(CS_dispense,HIGH);
 digitalWrite(CS_liquid_open,HIGH);
 digitalWrite(CS_liquid_close,HIGH);

/* Initial checks for containers level */
 if(!digitalRead(BS_low_level)) digitalWrite(BS_dispense_LED, HIGH);
 if(!digitalRead(DL_low_level)) digitalWrite(DL_dispense_LED, HIGH);
 if(!digitalRead(CS_low_level)) digitalWrite(CS_dispense_LED, HIGH);
}

void loop() {
/* Check level for each container */
  if(digitalRead(BS_low_level) && !digitalRead(BS_high_level)){
    BS_low_level_status = true;
    digitalWrite(BS_dispense_LED, LOW);
  }
//  if(digitalRead(DL_low_level) && !digitalRead(DL_high_level)){
//    DL_low_level_status = true;
//    digitalWrite(DL_dispense_LED, LOW);
//  }
//  if(digitalRead(CS_low_level) && !digitalRead(CS_high_level)){
//    CS_low_level_status = true;
//    digitalWrite(CS_dispense_LED, LOW);
//  }

//  if(!BS_on_process && !DL_on_process && !CS_on_process && !all_low_level_status && BS_low_level_status && DL_low_level_status && CS_low_level_status)
//  {
//    all_low_level_status = true;
//  }
//  else if (!BS_on_process && !DL_on_process && !CS_on_process && !all_low_level_status && BS_low_level_status && DL_low_level_status && !CS_low_level_status)
//  {
//    BSDL_low_level_status = true;
//  }
//  else if (!BS_on_process && !DL_on_process && !CS_on_process && !all_low_level_status && !BS_low_level_status && DL_low_level_status && CS_low_level_status)
//  {
//    DLCS_low_level_status = true;
//  }
//  else if (!BS_on_process && !DL_on_process && !CS_on_process && !all_low_level_status && BS_low_level_status && !DL_low_level_status && CS_low_level_status)
//  {
//    BSCS_low_level_status = true;
//  }

//  if(all_low_level_status)
//  {
//    if (BS_low_level_status && DL_low_level_status && CS_low_level_status)
//    {
//      BS_process (2000, 5000);
//    }
//    else if (!BS_low_level_status && DL_low_level_status && CS_low_level_status)
//    {
//      DL_process (2000, 5000);
//    }
//    else if(!BS_low_level_status && !DL_low_level_status && CS_low_level_status)
//    {
//      CS_process (2000, 5000);
//    }
//    else if(!BS_low_level_status && !DL_low_level_status && !CS_low_level_status)
//    {
//      all_low_level_status = false;
//    }
//  }
//  else if (!CS_on_process && BSDL_low_level_status)
//  {
//    if (BS_low_level_status && DL_low_level_status)
//    {
//      BS_process (2000, 5000);
//    }
//    else if (!BS_low_level_status && DL_low_level_status)
//    {
//      DL_process (2000, 5000);
//    }
//    else if(!BS_low_level_status && !DL_low_level_status)
//    {
//      BSDL_low_level_status = false;
//    }
//  }
//  else if (!BS_on_process && DLCS_low_level_status)
//  {
//    if (DL_low_level_status && CS_low_level_status)
//    {
//      DL_process (2000, 5000);
//    }
//    else if (!DL_low_level_status && CS_low_level_status)
//    {
//      CS_process (2000, 5000);
//    }
//    else if(!DL_low_level_status && !CS_low_level_status)
//    {
//      DLCS_low_level_status = false;
//    }
//  }
//  else if (!DL_on_process && BSCS_low_level_status)
//  {
//    if (BS_low_level_status && CS_low_level_status)
//    {
//      BS_process (2000, 5000);
//    }
//    else if (!BS_low_level_status && CS_low_level_status)
//    {
//      CS_process (2000, 5000);
//    }
//    else if(!BS_low_level_status && !CS_low_level_status)
//    {
//      BSCS_low_level_status = false;
//    }
//  }

  if(BS_low_level_status && !DL_on_process && !CS_on_process)
  {
    BS_on_process = true;
    BS_process (2000, 5000);
  }
  else
  {
    BS_on_process = false;
    if(!digitalRead(BS_dispense_PB)) digitalWrite(BS_dispense, LOW);
    else digitalWrite(BS_dispense, HIGH);
  }
//  if(DL_low_level_status && !BS_on_process && !CS_on_process)
//  {
//    DL_on_process = true;
//    DL_process (2000, 5000);
//  }
//  else
//  {
//    DL_on_process = false;
//    if(!digitalRead(DL_dispense_PB)) digitalWrite(DL_dispense, LOW);
//    else digitalWrite(DL_dispense, HIGH);
//  }
//  if(CS_low_level_status && !BS_on_process && !DL_on_process)
//  {
//    CS_on_process = true;
//    CS_process (2000, 3000);
//  }
//  else
//  {
//    CS_on_process = false;
//    if(!digitalRead(CS_dispense_PB)) digitalWrite(CS_dispense, LOW);
//    else digitalWrite(CS_dispense, HIGH);
//  }

/*=================================================================================================*/

}

/*=================================================================================================*/
void BS_process(unsigned long mix_time_liquid, unsigned long mix_time_powder){
  unsigned long current_time = millis();
  int dispense_delay = 5000;
  
  // process latch
  static bool A = false;
  static bool B = false;
  static bool C = false;
  static bool D = false;

  if(!A && BS_low_level_status){
    Serial.println("BS 1");
    digitalWrite(mixer, HIGH);
    digitalWrite(BS_water, LOW);
  }
  if(!A && (!digitalRead(BS_low_level) && digitalRead(BS_high_level))){
    digitalWrite(BS_water, HIGH);
    digitalWrite(BS_liquid_open, HIGH);
    digitalWrite(BS_liquid_close, LOW);
    digitalWrite(mixer, LOW);
    BS_prev_time = current_time;
    Serial.println("BS 2");
    A = true;
  }
  if(A && !B){
    if ((current_time - BS_prev_time) >= mix_time_liquid){
      digitalWrite(BS_liquid_open, LOW);
      digitalWrite(BS_liquid_close, HIGH);    // Turn off liquid mixture dispenser
      digitalWrite(mixer, HIGH);
      BS_prev_time = current_time;
      Serial.println("BS 3");
      B = true;
    }
  }
  if(B && !C){
    if ((current_time - BS_prev_time) >= dispense_delay){
      BS_powder.write(BSopen_angle);
      digitalWrite(mixer, LOW);
      BS_prev_time = current_time;
      Serial.println("BS 4");
      C = true;
    }
  }
  if(C && !D){
    if ((current_time - BS_prev_time) >= mix_time_powder){
      BS_powder.write(BSclose_angle);         // Turn off powder mixture dispenser after 20 secs
      digitalWrite(mixer, HIGH);
      BS_prev_time = current_time;
      Serial.println("BS 5");
      D = true;
    }
  }
  if(D){
    if ((current_time - BS_prev_time) >= dispense_delay){
      digitalWrite(BS_dispense_LED, HIGH); // Turn on LED indicating dispensing is now available
      digitalWrite(BS_liquid_open, HIGH);  // Turn off all unnecessary relays after process
      BS_prev_time = current_time;
      Serial.println("BS 6");
      BS_low_level_status = false;
      A = false;
      B = false;
      C = false;
      D = false;
    }
  }
}

/*=================================================================================================*/
void DL_process(unsigned long dispense_time, unsigned long mix_time){
  unsigned long current_time = millis();
  
  // process latch
  static bool A = false;
  static bool B = false;
  static bool C = false;
  static bool D = false;

  if(!A && DL_low_level_status){
    Serial.println("DL 1");
    digitalWrite(mixer, HIGH);
    digitalWrite(DL_water, LOW);
  }
  if(!A && (!digitalRead(DL_low_level) && digitalRead(DL_high_level))){
    digitalWrite(DL_water, HIGH);
    digitalWrite(DL_liquid_open, HIGH);
    digitalWrite(DL_liquid_close, LOW);
    DL_prev_time = current_time;
    Serial.println("DL 2");
    A = true;
  }
  if(A && !B){
    if ((current_time - DL_prev_time) >= dispense_time){
      digitalWrite(DL_liquid_open, LOW);
      digitalWrite(DL_liquid_close, HIGH);    // Turn off liquid mixture dispenser after 20 secs
      digitalWrite(mixer, LOW);
      DL_prev_time = current_time;
      Serial.println("DL 3");
      B = true;
    }
  }
  if(B && !C){
    if ((current_time - DL_prev_time) >= mix_time){
      digitalWrite(mixer, HIGH);    // Turn off mixer after 5 mins
      DL_powder.write(DLopen_angle);
      DL_prev_time = current_time;
      Serial.println("DL 4");
      C = true;
    }
  }
  if(C && !D){
    if ((current_time - DL_prev_time) >= dispense_time){
      DL_powder.write(DLclose_angle);         // Turn off powder mixture dispenser after 20 secs
      digitalWrite(mixer, LOW);
      DL_prev_time = current_time;
      Serial.println("DL 5");
      D = true;
    }
  }
  if(D){
    if ((current_time - DL_prev_time) >= mix_time){
      digitalWrite(DL_dispense_LED, HIGH); // Turn on LED indicating dispensing is now available
      digitalWrite(DL_liquid_open, HIGH);  // Turn off all unnecessary relays after process
      digitalWrite(mixer, HIGH);
      DL_prev_time = current_time;
      Serial.println("DL 6");
      DL_low_level_status = false;
      A = false;
      B = false;
      C = false;
      D = false;
    }
  }
}

/*=================================================================================================*/
void CS_process(unsigned long dispense_time, unsigned long mix_time){
  unsigned long current_time = millis();
  
  // process latch
  static bool A = false;
  static bool B = false;
  static bool C = false;
  static bool D = false;

  if(!A && CS_low_level_status){
    Serial.println("CS 1");
    digitalWrite(mixer, HIGH);
    digitalWrite(CS_water, LOW);
  }
  if(!A && (!digitalRead(CS_low_level) && digitalRead(CS_high_level))){
    digitalWrite(CS_water, HIGH);
    digitalWrite(CS_liquid_open, HIGH);
    digitalWrite(CS_liquid_close, LOW);
    CS_prev_time = current_time;
    Serial.println("CS 2");
    A = true;
  }
  if(A && !B){
    if ((current_time - CS_prev_time) >= dispense_time){
      digitalWrite(CS_liquid_open, LOW);
      digitalWrite(CS_liquid_close, HIGH);    // Turn off liquid mixture dispenser after 20 secs
      digitalWrite(mixer, LOW);
      CS_prev_time = current_time;
      Serial.println("CS 3");
      B = true;
    }
  }
  if(B && !C){
    if ((current_time - CS_prev_time) >= mix_time){
      digitalWrite(mixer, HIGH);    // Turn off mixer after 5 mins
      CS_powder.write(CSopen_angle);
      CS_prev_time = current_time;
      Serial.println("CS 4");
      C = true;
    }
  }
  if(C && !D){
    if ((current_time - CS_prev_time) >= dispense_time){
      CS_powder.write(CSclose_angle);         // Turn off powder mixture dispenser after 20 secs
      digitalWrite(mixer, LOW);
      CS_prev_time = current_time;
      Serial.println("CS 5");
      D = true;
    }
  }
  if(D){
    if ((current_time - CS_prev_time) >= mix_time){
      digitalWrite(CS_dispense_LED, HIGH); // Turn on LED indicating dispensing is now available
      digitalWrite(CS_liquid_open, HIGH);  // Turn off all unnecessary relays after process
      digitalWrite(mixer, HIGH);
      CS_prev_time = current_time;
      Serial.println("CS 6");
      CS_low_level_status = false;
      A = false;
      B = false;
      C = false;
      D = false;
    }
  }
}