#include <Servo.h>
#include <SoftwareSerial.h>

// Ultrasonic sensor pins
#define uTrig 52
#define uEcho 53
#define uTrig1 48
#define uEcho1 49
#define uTrig2 44
#define uEcho2 45
#define uTrig3 40
#define uEcho3 41

// Relay pin
#define relay 37

// Phone number to send SMS notification to
const String number = "+639192279506";

// SoftwareSerial object for communicating with SIM800 module
SoftwareSerial sim800(11, 10);

// SMS message content
String SMS1 = "System: The Bin1 is Full";
String SMS2 = "System: The Bin2 is Full";

// Variables for storing sensor readings and servo motor position
int read;
int read1;
int read2;
int read3;
double rotate;

// Servo motor objects for controlling bin lids
Servo sMotor1;
Servo sMotor2;

// Setup function - runs once at startup
void setup()
{
  Serial.begin(9600);
  
  // Reserve space in memory for string buffer
  _buffer.reserve(50);
  
  // Initialize communication with SIM800 module
  sim800.begin(115200);
  
  // Set relay pin as output
  pinMode(relay, OUTPUT);
  
  // Attach servo motor objects to pins
  sMotor1.attach(6);
  sMotor2.attach(7);
  
  // Delay for 1 second
  delay(1000);
}

// Function for reading ultrasonic distance sensor
long readUltrasonicDistance(int triggerPin, int echoPin)
{
  // Set trigger pin to output and send pulse
  pinMode(triggerPin, OUTPUT);
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  
  // Set echo pin to input and read pulse duration
  pinMode(echoPin, INPUT);
  long reading = 0.01723 * pulseIn(echoPin, HIGH);
  
  // Delay for 100ms to prevent sensor interference
  delay(100);

  return reading;
}

// Function for sending SMS message
String SendMessage(String SMS)
{
  // Set GSM module to text mode
  sim800.println("AT+CMGF=1");
  delay(200);
  
  // Set SMS recipient phone number
  sim800.println("AT+CMGS=\"" + number + "\"\r");
  delay(200);
  
  // Send SMS message content
  sim800.println(SMS);
  delay(100);
  
  // Send CTRL+Z to terminate message and send
  sim800.println((char)26);
  delay(200);
}

// Main loop function - runs repeatedly
void loop()
{
  // Read sensor values and map to 0-300 range
  read = readUltrasonicDistance(uTrig, uEcho);
  read = map(read, 2, 335, 0, 300);
  read = constrain(read, 0, 300);

  read1 = readUltrasonicDistance(uTrig1, uEcho1);
  read1 = map(read1, 2, 335, 0, 300);
  read1 = constrain(read1, 0, 300);

  read2 = readUltrasonicDistance(uTrig2, uEcho2);
  read2 = map(read2, 2, 335, 0, 300);
  read2 = constrain(read2, 0, 300);

  read3 = readUltrasonicDistance(uTrig3, uEcho3);
  read3 = map(read3, 2, 335, 0, 300);
  read3 = constrain(read3, 0, 300);
  
  
  Serial.print(read);
  Serial.print(read1);
  Serial.print(read2);
  Serial.println(read3);
  delay(15);

  if (read >= 50){
    sMotor1.write(90);
    digitalWrite(relay, HIGH);
    delay(1000);
  }
  else if (read1 >= 50){
    sMotor2.write(90);
    digitalWrite(relay, HIGH);
    delay(1000);
  }
  else if (read2 >= 50){
    SendMessage(SMS1);
    delay(1000);
  }
  else if (read3 >= 50){
    SendMessage(SMS2);
    delay(1000);
  }
  else{
    sMotor1.write(0);
    sMotor2.write(0);
    digitalWrite(relay, LOW);
    delay(1000);
  }

}