#include <LiquidCrystal_I2C.h>
#define SW1 13   //float sensor
#define PB1 10   //to system ready 
#define PB2  9   //to open SolFill
#define PB3  8   //to pause the process

#define SolFill 7
#define SolDrain 6
#define Pump 5
#define R1 4      //f³or turn left
#define R2 3      //for turn right
LiquidCrystal_I2C lcd(0x27, 16, 2);

int ProcessStatus = 0;  //to indicate currenct process
int MotorStatus = 0;  //to indicate currenct process
int ProcessStatus_buff = 0;
bool ReadyStatus = 0;   //Status Ready
bool pb1_pressed, pb2_pressed, pb3_pressed;

int sensorPin = A0;

unsigned long T_nowP2, T_nowP3, T_nowP4, T_nowP5, T_nowP6, T_nowP7, T_nowP8;


unsigned long DurationR1 = 3000;   //durasi R1
unsigned long DurationR2 = 4000;   //durasi R2
unsigned long DurationStop = 2000;   //delay off pumpa bergatin in ms

unsigned long DurationP2 = 6000;  //durasi proses 2 in ms pump running
unsigned long DurationP4 = 5000;   //durasi proses 4 in ms SolDrain
unsigned long DurationP8 = 5000;   //durasi proses 8 in ms SolDrain

int LCD_refresh_rate = 200;

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

  pinMode(SW1, INPUT);
  pinMode(PB1, INPUT);
  pinMode(PB2, INPUT);
  pinMode(PB3, INPUT);

  pinMode(SolFill, OUTPUT);
  pinMode(SolDrain, OUTPUT);
  pinMode(Pump, OUTPUT);
  pinMode(R1, OUTPUT);
  pinMode(R2, OUTPUT);


  digitalWrite(SolFill, LOW);
  digitalWrite(SolDrain, LOW);
  digitalWrite(Pump, LOW);
  digitalWrite(R1, LOW);
  digitalWrite(R2, LOW);

  lcd.init();
  lcd.backlight();
  lcd.clear();
  info(0, 0, "System Standby", "", false);
}

void loop()
{
  
  if (digitalRead(PB1)) {
    pb1_pressed = false;
    if (!pb1_pressed) {
      ReadyStatus = true;
      Serial.println("ready");
      lcd.clear();
      info(0, 0, "System Ready", "", false);
      info(0, 1, "PB2 to Start", "", false);
      pb1_pressed = !pb1_pressed;
    }
  } else pb1_pressed = !pb1_pressed;

  if (digitalRead(PB2)) {
    pb2_pressed = false;

    if (!pb2_pressed && ReadyStatus) {
      ProcessStatus = 1;
      pb2_pressed = !pb2_pressed;
    }
  } else pb2_pressed = !pb2_pressed;


  if (ProcessStatus != 0) {
    int turbidity = ReadTurbidity();


    switch (ProcessStatus) {
      case 1: //SolFill Activation
        if (ProcessStatus_buff != 1) {
          Serial.println("Process 1 Running SolFill Activation");
          lcd.clear();
          info(0, 0, "Process 1", "", false);
          info(0, 1, "SolFill Active", " ", false);
          ProcessStatus_buff = 1;
        }

        digitalWrite(SolFill, !digitalRead(SW1));
        if (digitalRead(SW1))
        {
          ProcessStatus = 2;
          digitalWrite(SolFill, LOW);
          digitalWrite(Pump, HIGH);
          T_nowP2 = millis();  //get first time for proses 2
        }
        break;
      case 2: //Pump Running
        if (ProcessStatus_buff != 2) {
          Serial.println("Process 2 Running Pump Running");
          lcd.clear();
          info(0, 0, "Process 2", "", false);
          info(0, 1, "Pump Running "+String(turbidity), " ", false);
          ProcessStatus_buff = 2;
        }


        if ((millis() - T_nowP2 > DurationP2) && turbidity > 20)
        {
          digitalWrite(Pump, LOW);
          ProcessStatus = 3;
          T_nowP3 = millis();  //get first time for proses 3a
        }else{
    
          info(0, 1, "Pump Running "+String(turbidity), " ", false);
        }

        break;
      case 3: //Motor Running
        switch (MotorStatus) {
          case 0:
            if (ProcessStatus_buff != 30) {
              Serial.println("Process 3 Running Motor Running");
              lcd.clear();
              info(0, 0, "Process 3", "", false);
              info(0, 1, "Motor Running R1", " ", false);
              ProcessStatus_buff = 3;
              digitalWrite(R1, HIGH);
              ProcessStatus_buff = 30;
              T_nowP3 = millis();

            }
            if (millis() - T_nowP3 > (DurationR1))
            {
              lcd.clear();
              info(0, 0, "Process 3", "", false);
              info(0, 1, "Motor Stop", " ", false);
              digitalWrite(R1, LOW);
              T_nowP3 = millis();
              MotorStatus = 1;
            }
            break;
          case 1:
            if (millis() - T_nowP3 > (DurationStop))
            {
              lcd.clear();
              info(0, 0, "Process 3", "", false);
              info(0, 1, "Motor Running R2", " ", false);
              digitalWrite(R2, HIGH);
              T_nowP3 = millis();
              MotorStatus = 2;
            }
            break;
          case 2:
            if (millis() - T_nowP3 > (DurationR2))
            {
              lcd.clear();
              info(0, 0, "Process 3", "", false);
              info(0, 1, "Motor Stop", " ", false);
              digitalWrite(R2, LOW);
              T_nowP3 = millis();
              MotorStatus = 3;
            }
            break;
          case 3:
            if (millis() - T_nowP3 > (DurationStop))
            {
              MotorStatus = 0;
              ProcessStatus = 4;
              digitalWrite(SolDrain, HIGH);
              T_nowP4 = millis();  //get first time for proses 4
            }
            break;
        }
        break;
      case 4: //SolDrain Activation
        if (ProcessStatus_buff != 4) {
          Serial.println("Process 4 Running SolDrain Activation");
          lcd.clear();
          info(0, 0, "Process 4", "", false);
          info(0, 1, "SolDrain Active", " ", false);
          ProcessStatus_buff = 4;
        }
        if (millis() - T_nowP4 > DurationP4)
        {
          digitalWrite(SolDrain, LOW);
          ProcessStatus = 5;
          digitalWrite(SolDrain, LOW);
          T_nowP5 = millis();  //get first time for proses 5a
        }
        break;
      case 5: //Motor Running
        switch (MotorStatus) {
          case 0:
            if (ProcessStatus_buff != 30) {
              Serial.println("Process 5 Running Motor Running");
              lcd.clear();
              info(0, 0, "Process 5", "", false);
              info(0, 1, "Motor Running R1", " ", false);
              ProcessStatus_buff = 3;
              digitalWrite(R1, HIGH);
              ProcessStatus_buff = 30;
              T_nowP3 = millis();

            }
            if (millis() - T_nowP3 > (DurationR1))
            {
              lcd.clear();
              info(0, 0, "Process 5", "", false);
              info(0, 1, "Motor Stop", " ", false);
              digitalWrite(R1, LOW);
              T_nowP3 = millis();
              MotorStatus = 1;
            }
            break;
          case 1:
            if (millis() - T_nowP3 > (DurationStop))
            {
              lcd.clear();
              info(0, 0, "Process 5", "", false);
              info(0, 1, "Motor Running R2", " ", false);
              digitalWrite(R2, HIGH);
              T_nowP3 = millis();
              MotorStatus = 2;
            }
            break;
          case 2:
            if (millis() - T_nowP3 > (DurationR2))
            {
              lcd.clear();
              info(0, 0, "Process 5", "", false);
              info(0, 1, "Motor Stop", " ", false);
              digitalWrite(R2, LOW);
              T_nowP3 = millis();
              MotorStatus = 3;
            }
            break;
          case 3:
            if (millis() - T_nowP3 > (DurationStop))
            {
              MotorStatus = 0;
              ProcessStatus = 6;
              digitalWrite(SolFill, HIGH);
              T_nowP6 = millis();  //get first time for proses 4
            }
            break;
        }
        break;
      case 6: //SolFill Activation
        if (ProcessStatus_buff != 6) {
          Serial.println("Process 6 Running SolFill Activation");
          lcd.clear();
          info(0, 0, "Process 6", "", false);
          info(0, 1, "SolFill Active", " ", false);
          ProcessStatus_buff = 6;
        }
        digitalWrite(SolFill, !digitalRead(SW1));
        if (digitalRead(SW1))
        {
          ProcessStatus = 7;
          digitalWrite(SolFill, LOW);

          T_nowP7 = millis();  //get first time for proses 7a
        }
        break;
      case 7: //Motor Running
        switch (MotorStatus) {
          case 0:
            if (ProcessStatus_buff != 30) {
              Serial.println("Process 7 Running Motor Running");
              lcd.clear();
              info(0, 0, "Process 7", "", false);
              info(0, 1, "Motor Running R1", " ", false);
              ProcessStatus_buff = 3;
              digitalWrite(R1, HIGH);
              ProcessStatus_buff = 30;
              T_nowP3 = millis();

            }
            if (millis() - T_nowP3 > (DurationR1))
            {
              lcd.clear();
              info(0, 0, "Process 7", "", false);
              info(0, 1, "Motor Stop", " ", false);
              digitalWrite(R1, LOW);
              T_nowP3 = millis();
              MotorStatus = 1;
            }
            break;
          case 1:
            if (millis() - T_nowP3 > (DurationStop))
            {
              lcd.clear();
              info(0, 0, "Process 7", "", false);
              info(0, 1, "Motor Running R2", " ", false);
              digitalWrite(R2, HIGH);
              T_nowP3 = millis();
              MotorStatus = 2;
            }
            break;
          case 2:
            if (millis() - T_nowP3 > (DurationR2))
            {
              lcd.clear();
              info(0, 0, "Process 7", "", false);
              info(0, 1, "Motor Stop", " ", false);
              digitalWrite(R2, LOW);
              T_nowP3 = millis();
              MotorStatus = 3;
            }
            break;
          case 3:
            if (millis() - T_nowP3 > (DurationStop))
            {

              MotorStatus = 0;
              ProcessStatus = 8;
              T_nowP8 = millis();  //get first time for proses 8
              digitalWrite(SolDrain, HIGH);
            }
            break;
        }


        break;
      case 8: //SolDrain Activation
        if (ProcessStatus_buff != 8) {
          Serial.println("Process 8 Running SolDrain Activation");
          lcd.clear();
          info(0, 0, "Process 8", "", false);
          info(0, 1, "SolDrain Active", " ", false);
          ProcessStatus_buff = 8;
        }

        if (millis() - T_nowP8 > DurationP8)
        {
          digitalWrite(SolDrain, LOW);
          ProcessStatus = 0;
          ReadyStatus = 0;
          Serial.println("Process Finish");
          lcd.clear();
          info(0, 0, "Process Finish", "", false);
        }
        break;
    }
  }

}


void info(uint8_t lcd_cols, uint8_t lcd_rows, String s, String s2, boolean eval) {
  lcd.setCursor(lcd_cols, lcd_rows);

  if (!eval) {
    lcd.print(s);
  }
  else {
    int len = s.indexOf(' ');
    //1 porsi
    //10 porsi
    lcd.print(s.substring(0, len));
    lcd.setCursor(lcd_cols + len, lcd_rows);
    lcd.print(" ");
    lcd.setCursor(lcd_cols + len + 1, lcd_rows);
    lcd.print(s2 + " ");
  }
}
int ReadTurbidity(){
  int sensorValue = analogRead(sensorPin);
  int turbidity = map(sensorValue, 0,640, 100, 0);
  return turbidity;
  delay(100);

}