#include <SoftwareSerial.h>
#include <LiquidCrystal.h> 
#include <Servo.h> 
LiquidCrystal lcd(12, 11, 10, 9, 8, 7); // RS E D4 D5 D6 D7
Servo A;
Servo B;

int MoistureS = A7 ;
int MetalS = A6;
int ProximityS = 6;
int MetalThreshold = 1024;
int MoistureThreshold = 900;
int Led = 5;
int count=0;
int state=0;

const byte npulse = 12; // number of pulses to charge the capacitor before each measurement
const byte pin_pulse = A0; // sends pulses to charge the capacitor (can be a digital pin)
const byte pin_cap  = A1; // measures the capacitor charge
const byte pin_LED = 13; // LED that turns on when metal is detected

const int nmeas = 256; //measurements to take
long int sumsum = 0; //running sum of 64 sums
long int skip = 0; //number of skipped sums
long int diff = 0;      //difference between sum and avgsum
long int flash_period = 0; //period (in ms)
long unsigned int prev_flash = 0; //time stamp of previous flash

void setup() {
  Serial.begin(9600);
  // put your setup code here, to run once:
  A.attach(4); // Servo A Tray
  B.attach(3); // Servo B Holder
  // 0 degrees for plastics
  // 90 degrees for Metals
  // 180 degrees for Wet
  lcd.begin(16, 2);// LCD 16X2
  lcd.setCursor(0,0);
  lcd.print("WASTE SEGREGATOR");
  lcd.setCursor(5,1);
  lcd.print("SYSTEM");
  delay(2000);
  lcd.clear();
  lcd.setCursor(5,0);
  lcd.print("SYSTEM ");
  lcd.setCursor(2,1);
  lcd.print("INITIALIZING");
  for (int a=0; a<181; a++)
  {
    A.write(a);
    delay(10);
    }
      
    for (int a=0; a<91; a++)
  {
      B.write(a);
      delay(10);
    }

  A.write(60);
  B.write(0);

  pinMode(MoistureS, INPUT);  
  pinMode(MetalS, INPUT);  
  pinMode(ProximityS, INPUT);  
  digitalWrite(ProximityS, HIGH);
  pinMode(Led, OUTPUT);  

  pinMode(pin_pulse, OUTPUT);
  digitalWrite(pin_pulse, LOW);
  pinMode(pin_cap, INPUT);
  pinMode(pin_LED, OUTPUT);
  digitalWrite(pin_LED, LOW);

  delay(2000);
  lcd.clear();
}

void loop() {
  

  int minval = 2000;
  int maxval = 0;

 //perform measurement
  long unsigned int sum = 0;
  for (int imeas = 0; imeas < nmeas + 2; imeas++) {
    //reset the capacitor
    pinMode(pin_cap, OUTPUT);
    digitalWrite(pin_cap, LOW);
    delayMicroseconds(20);
    pinMode(pin_cap, INPUT);
    //apply pulses
    for (int ipulse = 0; ipulse < npulse; ipulse++) {
      digitalWrite(pin_pulse, HIGH); //takes 3.5 microseconds
      delayMicroseconds(3);
      digitalWrite(pin_pulse, LOW); //takes 3.5 microseconds
      delayMicroseconds(3);
    }

     //read the charge on the capacitor
    int val = analogRead(pin_cap); //takes 13x8=104 microseconds
    minval = min(val, minval);
    maxval = max(val, maxval);
    sum += val;


 //determine if LEDs should be on or off
    long unsigned int timestamp = millis();
    byte ledstat = 0;
    if (timestamp < prev_flash +12) {
      if (diff > 0)ledstat = 1;
      if (diff < 0)ledstat = 2;
    }
    if (timestamp > prev_flash + flash_period) {
      if (diff > 0)ledstat = 1;
      if (diff < 0)ledstat = 2;
      prev_flash = timestamp;
    }
    if (flash_period > 1000)ledstat = 0;
 
    //switch the LEDs to this setting
    if (ledstat == 0) {
      digitalWrite(pin_LED, LOW);
    }
    if (ledstat == 1) {
      digitalWrite(pin_LED, LOW);
    }
    if (ledstat == 2) {
      digitalWrite(pin_LED, HIGH);
    }
 
  }
 
  //subtract minimum and maximum value to remove spikes
  sum -= minval; sum -= maxval;
 
  //process
  if (sumsum == 0) sumsum = sum << 6; //set sumsum to expected value
  long int avgsum = (sumsum + 32) >> 6;
  diff = sum - avgsum;
  if (abs(diff)<avgsum >> 10) {   //adjust for small changes
    sumsum = sumsum + sum - avgsum;
    skip = 0;
  } else {
    skip++;
  }
  if (skip > 64) {  // break off in case of prolonged skipping
    sumsum = sum << 6;
    skip = 0;
  }
 
  // one permille change = 2 ticks/s
  if (diff == 0) flash_period = 1000000;
  else flash_period = avgsum / (2 * abs(diff));


  count++;
  if(count>0 && count<15){
  lcd.setCursor(0,0);
  lcd.print("   AUTOMATIC    ");
  lcd.setCursor(0,1);
  lcd.print("WASTE SEGREGATOR");
  }
  else{
  lcd.setCursor(0,0);
  lcd.print(" SYSTEM ACTIVE  ");
  lcd.setCursor(0,1);
  lcd.print("|-_-_-_-_-_-_-_|");
  }
  if (count>=30){
    count=0;
  }
if (digitalRead(ProximityS) == LOW){
  
  Serial.print("MetalS:");
  Serial.println(analogRead(MetalS));
  Serial.print("MoistureS:");
  Serial.println(analogRead(MoistureS));

  digitalWrite(Led, HIGH);
  
  if(analogRead(MetalS) > MetalThreshold){
    // metallic
  lcd.setCursor(0,0);
  lcd.print("METALIC DETECTED");
  lcd.setCursor(0,1);
  lcd.print("ROTATING BIN:   ");
  
 for (int a=60; a<91; a++)
  {
    lcd.setCursor(13,1);
    lcd.print(a);
    A.write(a);
    if(a<100 && a>10){
     lcd.setCursor(15,1);
     lcd.print("   ");
      }
      else if (a<10){
     lcd.setCursor(14,1);
     lcd.print("   ");
        }
    delay(20);  
    }
    
    delay(1000);
    B.write(90);
    delay(1000);
    state=90;
  }

  else if(analogRead(MoistureS) < MoistureThreshold){
    // Moisture
  lcd.setCursor(0,0);
  lcd.print("  WET DETECTED  ");
  lcd.setCursor(0,1);
  lcd.print("ROTATING BIN:   ");

    for (int a=60; a<181; a++)
  {
    lcd.setCursor(13,1);
    lcd.print(a);
    A.write(a);
    if(a<100 && a>10){
     lcd.setCursor(15,1);
     lcd.print("   ");
      }
      else if (a<10){
     lcd.setCursor(14,1);
     lcd.print("   "); 
    }
    delay(20);
    }
    delay(1000);
    B.write(90); 
    delay(1000); 
    state=180;  
  }
  
  
  else {
    // OTHER WASTE //plastic 
  lcd.setCursor(0,0);
  lcd.print("OTHERS DETECTED");
  lcd.setCursor(0,1);
  lcd.print("ROTATING BIN:   ");
  state=0;
  for (int a=60; a>-1; a--)
  { 
    lcd.setCursor(13,1);
    lcd.print(a);
    A.write(a);
    if(a<100 && a>10){
     lcd.setCursor(15,1);
     lcd.print("   ");
      }
      else if (a<10){
     lcd.setCursor(14,1);
     lcd.print("   ");
    }
    delay(20);
    }
    delay(1000);
    B.write(90); 
    delay(1000); 
    state=0;  
    }

if(state==90){
   for (int a=90; a>59; a--)
  { 
    A.write(a);
    delay(20);
    }
  }

  else if(state==180){
     for (int a=180; a>59; a--)
  { 
    A.write(a);
    delay(20);
    }
  }

  else{
     for (int a=0; a<61; a++)
  { 
    A.write(a);
    delay(20);
    }
  }
    delay(1000);
    digitalWrite(Led, LOW);
    B.write(0);
}
delay(100);
}