#include <LiquidCrystal_I2C.h>  //ไลบรารี่จอ LCD 
LiquidCrystal_I2C lcd(0x27,16,2); //แอดเดรส 0x27 ใส่ขนาดหน้าจอ 16 ตัวอักษร 2 แถว

// constants won't change
const int TRIG_PIN1  = 10;  // Arduino pin connected to Ultrasonic Sensor's TRIG pin
const int ECHO_PIN1  = 9;  // Arduino pin connected to Ultrasonic Sensor's ECHO pin
const int TRIG_PIN2  = 8;  // Arduino pin connected to Ultrasonic Sensor's TRIG pin
const int ECHO_PIN2  = 7;  // Arduino pin connected to Ultrasonic Sensor's ECHO pin
const int TRIG_PIN3  = 6;  // Arduino pin connected to Ultrasonic Sensor's TRIG pin
const int ECHO_PIN3  = 5;  // Arduino pin connected to Ultrasonic Sensor's ECHO pin
const int TRIG_PIN4  = 4;  // Arduino pin connected to Ultrasonic Sensor's TRIG pin
const int ECHO_PIN4  = 3;  // Arduino pin connected to Ultrasonic Sensor's ECHO pin
const int RELAY_PIN1 = A0; // Arduino pin connected to Relay's pin
const int RELAY_PIN2 = A1; // Arduino pin connected to Relay's pin
const int RELAY_PIN3 = A2; // Arduino pin connected to Relay's pin
const int RELAY_PIN4 = A3; // Arduino pin connected to Relay's pin
const int DISTANCE_THRESHOLD = 5; // centimeters

// variables will change:
float duration_us, distance_cm1, distance_cm2, distance_cm3, distance_cm4;
int Sw1=0; //Status of Switch 1 
int Sw2=0; //Status of Switch 2
int Sw3=0; //Status of Switch 3
int Sw4=0; //Status of Switch 4
long pTime=0;  //Previous time

void setup() {
  Serial.begin (9600);        // initialize serial port
  pinMode(TRIG_PIN1, OUTPUT);  // set arduino pin to output mode
  pinMode(ECHO_PIN1, INPUT);   // set arduino pin to input mode

  pinMode(TRIG_PIN2, OUTPUT);  // set arduino pin to output mode
  pinMode(ECHO_PIN2, INPUT);   // set arduino pin to input mode

  pinMode(TRIG_PIN3, OUTPUT);  // set arduino pin to output mode
  pinMode(ECHO_PIN3, INPUT);   // set arduino pin to input mode

  pinMode(TRIG_PIN4, OUTPUT);  // set arduino pin to output mode
  pinMode(ECHO_PIN4, INPUT);   // set arduino pin to input mode

  pinMode(RELAY_PIN1, OUTPUT); // set arduino pin to output mode
  pinMode(RELAY_PIN2, OUTPUT); // set arduino pin to output mode
  pinMode(RELAY_PIN3, OUTPUT); // set arduino pin to output mode
  pinMode(RELAY_PIN4, OUTPUT); // set arduino pin to output mode

  attachInterrupt(digitalPinToInterrupt(18),Switch1,RISING);
  attachInterrupt(digitalPinToInterrupt(19),Switch2,RISING);
  attachInterrupt(digitalPinToInterrupt(20),Switch3,RISING);
  attachInterrupt(digitalPinToInterrupt(21),Switch4,RISING);

  lcd.init();         //ใช้งาน จอ LCD
  lcd.backlight();  //ใช้งานไฟหน้าจอ 
  lcd.setCursor(1,0);   //เซตตำแหน่งการแสดงผลหน้าจอ
  lcd.print("HELLO");   //แสดงตัวอักษร
  


}

void loop() {
  Ultrasonic1();
  Ultrasonic2();  
  Ultrasonic3();
  Ultrasonic4();
  delay(500);
}

void Switch1(){
  if(millis()-pTime > 1000){
    if(distance_cm1 < 5){
    
      if(Sw1==0 && distance_cm1 < 5){
        Sw1 = 1;
        digitalWrite(RELAY_PIN1, HIGH); // turn on Relay
        Serial.println("Tray 1 Activated");
      }else{
        Sw1 = 0;
        digitalWrite(RELAY_PIN1, LOW); // turn on Relay
        Serial.println("Tray 1 Deactivated");
      }
      
    }else{
      Serial.println("Please close the tray");
    }
    pTime = millis();
  }
}

void Switch2(){
  if(millis()-pTime > 1000){
    if(Sw2==0){
      Sw2 = 1;
      digitalWrite(RELAY_PIN2, HIGH); // turn on Relay
      Serial.println("Tray 2 Activated");
    }else{
      Sw2 = 0;
      digitalWrite(RELAY_PIN2, LOW); // turn on Relay
      Serial.println("Tray 2 Deactivated");
    }
    pTime = millis();
  }  
}

void Switch3(){
  if(millis()-pTime > 1000){
    if(Sw3==0){
      Sw3 = 1;
      digitalWrite(RELAY_PIN3, HIGH); // turn on Relay
      Serial.println("Tray 3 Activated");
    }else{
      Sw3 = 0;
      digitalWrite(RELAY_PIN3, LOW); // turn on Relay
      Serial.println("Tray 3 Deactivated");
    }
    pTime = millis();
  }  
}

void Switch4(){
  if(millis()-pTime > 1000){
    if(Sw4==0){
      Sw4 = 1;
      digitalWrite(RELAY_PIN4, HIGH); // turn on Relay
      Serial.println("Tray 1 Activated");
    }else{
      Sw4 = 0;
      digitalWrite(RELAY_PIN4, LOW); // turn on Relay
      Serial.println("Tray 4 Deactivated");
    }
    pTime = millis();
  }  
}
void Ultrasonic1(){
  ///////////////Ultrasonic-1///////////////
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_PIN1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN1, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(ECHO_PIN1, HIGH);
  // calculate the distance
  distance_cm1 = 0.017 * duration_us;

  if (distance_cm1 > DISTANCE_THRESHOLD && Sw1 == HIGH) {
    Sw1 = LOW;
    digitalWrite(RELAY_PIN1, LOW); // turn off Relay
    // print the value to Serial Monitor
    Serial.print("US-distance 1: ");
    Serial.print(distance_cm1);
    Serial.println(" cm");
    delay(1000);
  }
}

void Ultrasonic2(){
  ///////////////Ultrasonic-2///////////////
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_PIN2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN2, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(ECHO_PIN2, HIGH);
  // calculate the distance
  distance_cm2 = 0.017 * duration_us;

  if (distance_cm2 > DISTANCE_THRESHOLD && Sw2 == HIGH) {
    Sw2 = LOW;
    digitalWrite(RELAY_PIN2, LOW); // turn off Relay
    // print the value to Serial Monitor
    Serial.print("US-distance 2: ");
    Serial.print(distance_cm2);
    Serial.println(" cm");
    delay(1000);
  }
}

void Ultrasonic3(){
  ///////////////Ultrasonic-3///////////////
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_PIN3, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN3, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(ECHO_PIN3, HIGH);
  // calculate the distance
  distance_cm3 = 0.017 * duration_us;

  if (distance_cm3 > DISTANCE_THRESHOLD && Sw3 == HIGH) {
    Sw3 = LOW;
    digitalWrite(RELAY_PIN3, LOW); // turn off Relay
    // print the value to Serial Monitor
    Serial.print("US-distance 3: ");
    Serial.print(distance_cm3);
    Serial.println(" cm");
    delay(1000);
  }
}

void Ultrasonic4(){
  ///////////////Ultrasonic-4///////////////
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_PIN4, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN4, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(ECHO_PIN4, HIGH);
  // calculate the distance
  distance_cm4 = 0.017 * duration_us;

  if (distance_cm4 > DISTANCE_THRESHOLD && Sw4 == HIGH) {
    Sw4 = LOW;
    digitalWrite(RELAY_PIN4, LOW); // turn off Relay
    // print the value to Serial Monitor
    Serial.print("US-distance 4: ");
    Serial.print(distance_cm4);
    Serial.println(" cm");
    delay(1000);
  }
}