//#include <servo.h>
#include <ServoEasing.h>
#include <LiquidCrystal_I2C.h>
int servoPin = 10;
Servo Servo1; 
// Include the library
//#include <Servo.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);
int sensorInterrupt = 0;
int sensorPin = 2;

//int servo = servo;
int angle = 0;

//int solenoidValve = 5;
int in1 = A0;
int in2 = A1;
int in3 = A2;
int in4 = A3;
int in5 = A4;
int in6 = A5;
int echoPin = A2;
int remotePins [] = {A0, A1, A2, A3, A4, A5};
int trigPin = 1;
int set = 0;
int stop = 0, stop1;
int mode = 0;

float duration_us, distance_cm;

int buz = 13;

int timer = 0;
 
int relay_1 = 2;
int relay_2 = 3;
int relay_3 = 4;
int relay_4 = 5;
int relay_5 = 6;
//nt motor = 9;
int input4Pin = A3;
int input3Pin = 9;
int input2Pin =8;
int input1Pin = 7;
int ir_FILL = A0;
int ir_FOIL = A1;
int ir_EJECT = A2;


void setup() {
  lcd.begin(16, 2);
Serial.begin(115200);
lcd.setCursor(0,0);lcd.print("SITA DAIRY YOGHURT  ");
//lcd.setCursor(0,1);lcd.print("      YOGHURT");
//lcd.setCursor(0,2);lcd.print("  Filling");
lcd.setCursor(0,3);lcd.print(" FILLING MACHINE");
delay(3000); 
lcd.clear();

Servo1.attach(servoPin);



//myStepper.setSpeed(60);
//void loop() {
 // for (int i = 0; i < 5; i ++) {
 // pinMode (remotePins [i], INPUT);
  //digitalWrite(remotePins [i], HIGH);

pinMode(trigPin, OUTPUT); // config trigger pin to output mode
pinMode(echoPin, INPUT);  // config echo pin to input mode
pinMode(ir_FILL, INPUT);
pinMode(ir_FOIL, INPUT);
pinMode(ir_EJECT, INPUT);

//pinMode (trigpin, OUTPUT);
//pinMode (echopin, INPUT);

pinMode(relay_1, OUTPUT);
pinMode(relay_2, OUTPUT);
pinMode(relay_3, OUTPUT);
pinMode(relay_4, OUTPUT);
pinMode(relay_5, OUTPUT);

//pinMode(pump, OUTPUT);
//Serial.begin(57600); // this begins the serial connection with a baud rate of 57600
pinMode(input4Pin, INPUT);
pinMode(input3Pin, INPUT);
pinMode(input2Pin, INPUT);
pinMode(input1Pin, INPUT); // these lines declare each of the buttons as an input
pinMode(buz, OUTPUT);

/*lcd.begin(16, 2);
Serial.begin(115200);
lcd.setCursor(0,0);lcd.print("SITA DRY YOGHURT  ");
//lcd.setCursor(0,1);lcd.print("      YOGHURT");
//lcd.setCursor(0,2);lcd.print("  Filling");
lcd.setCursor(0,3);lcd.print(" FILLING MACHINE");
delay(3000); 
lcd.clear();*/
}


void loop(){ 


 if (digitalRead(input1Pin) == HIGH){
   digitalRead(ir_FILL) == 0;
   lcd.begin(0,0);  
   lcd.print("FILLING 100ML");
   delay(1000);
   lcd.clear();
     if (digitalRead(ir_FILL) == 0){
         lcd.begin(0,0);  
         lcd.print("FILLING 100ML");
        delay(1000);
        digitalWrite (relay_1, HIGH); 
        delay(4000);// filling for 4 s
        digitalWrite (relay_1, LOW);
         lcd.clear();
       }
      else {
      lcd.setCursor(0,0); 
      lcd.print("CUP MISSING");
      delay(10000);
      digitalWrite(buz, HIGH);  
      delay(3000);
      digitalWrite(buz, LOW);
      digitalWrite (relay_1, LOW);
      digitalWrite (relay_2, LOW);
      digitalWrite (relay_3, LOW);
      digitalWrite (relay_4, LOW);
      digitalWrite (relay_5, LOW);
      delay(10000);
      lcd.setCursor(0,0); 
      lcd.print("REFILLING AGAIN");
      digitalWrite (relay_1, HIGH); 
      delay(4000);// filling for 4 s
      digitalWrite (relay_1, LOW);

     lcd.clear();
         }
  if (digitalRead(ir_FOIL) ==1){
      lcd.setCursor(0,0); lcd.print("SEALING YOGHURT");
      digitalWrite (relay_2, HIGH);
      delay(2000);
      digitalWrite (relay_2, LOW);
      lcd.clear();
      }
    else {
     lcd.setCursor(0,0); lcd.print("FOIL MISSING");
     digitalWrite (relay_2, LOW);
     delay(2000);
     lcd.clear();
     }


     lcd.setCursor(0,0); lcd.print("PLACING FOIL ON A CUP"); 
   // Make servo go to 0 degrees 
   Servo1.write(0); 
   delay(1000); 
   // Make servo go to 90 degrees 
   Servo1.write(180); 
   delay(1000); 
   // Make servo go to 180 degrees 
   Servo1.write(0); 
   delay(1000); 

 lcd.setCursor(0, 0);
 lcd.print("EJECTING CUP");
 digitalWrite(relay_3, HIGH);
 delay(2000);
 digitalWrite(relay_3, LOW);
 lcd.clear();
 
     //if (digitalRead(ir_EJECT==0){
  lcd.setCursor(0,0); lcd.print("LOADING CUP");
  digitalWrite(relay_4, HIGH);
  delay(2000);
  digitalWrite(relay_4, LOW);
  lcd.clear();
      /*  }
      else {
       lcd.setCursor(0,0); 
       lcd.print("SEAL CUP PRESENT");
     digitalWrite(relay_4, LOW);
     digitalWrite(buz, HIGH);  
     delay(2000);
     digitalWrite(buz, LOW);
         }*/

  lcd.setCursor(0,0); lcd.print("SPINNING");
  digitalWrite(buz, HIGH);  
  delay(1000);
  digitalWrite(buz, LOW);
  digitalWrite(relay_5, HIGH);
  delay(3000);
  digitalWrite(relay_5, LOW);
  lcd.clear();
 }

  else {
    
 lcd.setCursor(0,0); 
 lcd.print("PRESS TO START");
 delay(2000);
 digitalWrite(relay_1, LOW);
 digitalWrite (relay_2, LOW);
 digitalWrite(relay_3, LOW);
 digitalWrite(relay_4, LOW);
 digitalWrite(relay_5, LOW);
 // generate 10-microsecond pulse to TRIG pin
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(echoPin, HIGH);

  // calculate the distance
  distance_cm = 0.017 * duration_us;

  lcd.clear();
  lcd.setCursor(0, 0); // start to print at the first row
  lcd.print("YOGHURT LEVEL AMT: ");
  lcd.setCursor(0, 1);
  lcd.print(distance_cm/400 *100);
   lcd.print("%");
  delay(3000);
  lcd.clear();
  
  }
  if (digitalRead(input2Pin) == HIGH){
 digitalRead(ir_FILL) == 0;
 lcd.begin(0,0);  
 lcd.print("FILLING 150ML");
 delay(1000);
  lcd.clear();
     if (digitalRead(ir_FILL) == 0){
        digitalWrite (relay_1, HIGH); 
        delay(6000);// filling for 6 s
        digitalWrite (relay_1, LOW);
         lcd.clear();
       }
      else {
      lcd.setCursor(0,0); 
      lcd.print("CUP MISSING");
      delay(10000);
      digitalWrite(buz, HIGH);  
      delay(3000);
      digitalWrite(buz, LOW);
      digitalWrite (relay_1, LOW);
      digitalWrite (relay_2, LOW);
      digitalWrite (relay_3, LOW);
      digitalWrite (relay_4, LOW);
      digitalWrite (relay_5, LOW);
      delay(10000);
      lcd.setCursor(0,0); 
      lcd.print("REFILLING AGAIN");
      digitalWrite (relay_1, HIGH); 
      delay(6000);// filling for 6 s
      digitalWrite (relay_1, LOW);
     lcd.clear();
         }
  if (digitalRead(ir_FOIL) ==1){
      lcd.setCursor(0,0); lcd.print("SEALING YOGHURT");
      digitalWrite (relay_2, HIGH);
      delay(2000);
      digitalWrite (relay_2, LOW);
      lcd.clear();
      }
    else {
     lcd.setCursor(0,0); lcd.print("FOIL MISSING");
     digitalWrite (relay_2, LOW);
     delay(2000);
     lcd.clear();
     }
lcd.setCursor(0,0); lcd.print("PLACING FOIL ON A CUP"); 
   // Make servo go to 0 degrees 
Servo1.write(0); 
delay(1000); 
   // Make servo go to 90 degrees 
Servo1.write(180); 
 delay(3000); 
   // Make servo go to 180 degrees 
Servo1.write(0); 
 delay(1000);
 lcd.setCursor(0, 0);
 lcd.print("ejecting cup");
 digitalWrite(relay_3, HIGH);
 delay(2000);
 digitalWrite(relay_3, LOW);
 lcd.clear();
 
     //if (digitalRead(ir_EJECT==0){
  lcd.setCursor(0,0); lcd.print("LOADING CUP");
  digitalWrite(relay_4, HIGH);
  delay(2000);
  digitalWrite(relay_4, LOW);
  lcd.clear();
      /*  }
      else {
       lcd.setCursor(0,0); 
       lcd.print("SEAL CUP PRESENT");
     digitalWrite(relay_4, LOW);
     digitalWrite(buz, HIGH);  
     delay(2000);
     digitalWrite(buz, LOW);
         }*/

  lcd.setCursor(0,0); lcd.print("SPINNING");
  digitalWrite(buz, HIGH);  
  delay(1000);
  digitalWrite(buz, LOW);
  digitalWrite(relay_5, HIGH);
  delay(3000);
  digitalWrite(relay_5, LOW);
  lcd.clear();
 }

  else {
    
 lcd.setCursor(0,0); 
 lcd.print("PRESS TO START");
 delay(3000);
 digitalWrite(relay_1, LOW);
 digitalWrite (relay_2, LOW);
 digitalWrite(relay_3, LOW);
 digitalWrite(relay_4, LOW);
 digitalWrite(relay_5, LOW);
  lcd.clear();

  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(echoPin, HIGH);

  // calculate the distance
  distance_cm = 0.017 * duration_us;

  lcd.clear();
  lcd.setCursor(0, 0); // start to print at the first row
  lcd.print("YOGHURT LEVEL AMT: ");
  lcd.setCursor(0, 1);
  lcd.print(distance_cm/400 *100);
   lcd.print("%");
  delay(3000);
  lcd.clear();
  
  
  
  }
  if (digitalRead(input3Pin) == HIGH){
 digitalRead(ir_FILL) == 0;
 lcd.begin(0,0);  
 lcd.print("FILLING 250ML");
 delay(1000);
  lcd.clear();
     if (digitalRead(ir_FILL) == 0){
        digitalWrite (relay_1, HIGH); 
        delay(7000);// filling for 7 s
        digitalWrite (relay_1, LOW);
         lcd.clear();
       }
      else {
      
      lcd.setCursor(0,0); 
      lcd.print("CUP MISSING");
      delay(10000);
      digitalWrite(buz, HIGH);  
      delay(3000);
      digitalWrite(buz, LOW);
      digitalWrite (relay_1, LOW);
      digitalWrite (relay_2, LOW);
      digitalWrite (relay_3, LOW);
      digitalWrite (relay_4, LOW);
      digitalWrite (relay_5, LOW);
      delay(10000);
      lcd.setCursor(0,0); 
      lcd.print("REFILLING AGAIN");
      digitalWrite (relay_1, HIGH); 
      delay(7000);// filling for 7 s
      digitalWrite (relay_1, LOW);
     lcd.clear();
         }
  if (digitalRead(ir_FOIL) ==1){
      lcd.setCursor(0,0); lcd.print("SEALING YOGHURT");
      digitalWrite (relay_2, HIGH);
      delay(2000);
      digitalWrite (relay_2, LOW);
      lcd.clear();
      }
    else {
     lcd.setCursor(0,0); lcd.print("FOIL MISSING");
     digitalWrite (relay_2, LOW);
     delay(2000);
     lcd.clear();
     }
  lcd.setCursor(0,0); lcd.print("PLACING FOIL ON A CUP"); 
   // Make servo go to 0 degrees 
   Servo1.write(0); 
   delay(1000); 
   // Make servo go to 90 degrees 
   Servo1.write(180); 
   delay(3000); 
   // Make servo go to 180 degrees 
   Servo1.write(0); 
   delay(1000);
 lcd.setCursor(0, 0);
 lcd.print("ejecting cup");
 digitalWrite(relay_3, HIGH);
 delay(2000);
 digitalWrite(relay_3, LOW);
 lcd.clear();
 
     //if (digitalRead(ir_EJECT==0){
  lcd.setCursor(0,0); lcd.print("LOADING CUP");
  digitalWrite(relay_4, HIGH);
  delay(2000);
  digitalWrite(relay_4, LOW);
  lcd.clear();
      /*  }
      else {
       lcd.setCursor(0,0); 
       lcd.print("SEAL CUP PRESENT");
     digitalWrite(relay_4, LOW);
     digitalWrite(buz, HIGH);  
     delay(2000);
     digitalWrite(buz, LOW);
         }*/

  lcd.setCursor(0,0); lcd.print("SPINNING");
  digitalWrite(buz, HIGH);  
  delay(1000);
  digitalWrite(buz, LOW);
  digitalWrite(relay_5, HIGH);
  delay(3000);
  digitalWrite(relay_5, LOW);
  lcd.clear();
 }

  else {
    
 lcd.setCursor(0,0); 
 lcd.print("PRESS TO START");
 delay(3000);
 digitalWrite(relay_1, LOW);
 digitalWrite (relay_2, LOW);
 digitalWrite(relay_3, LOW);
 digitalWrite(relay_4, LOW);
 digitalWrite(relay_5, LOW);
  lcd.clear();
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(echoPin, HIGH);

  // calculate the distance
  distance_cm = 0.017 * duration_us;

  lcd.clear();
  lcd.setCursor(0, 0); // start to print at the first row
  lcd.print("YOGHURT LEVEL AMT: ");
  lcd.setCursor(0, 1);
  lcd.print(distance_cm/400 *100);
   lcd.print("%");
  delay(3000);
  lcd.clear();
  
  
  
  }
 }