/*****************************************
*  SMART OBJECT DETECTION SYSTEM USING   *
*         ESP32 MICROCONTROLLER          *
*     SUBMITTED BY: PAULO R. BENDIJO     *
*  SUMITTED TO: PROF. MICHAEL T. SAMONTE *
*****************************************/
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#include <math.h>
#define trigPIN 19
#define echoPIN 23
#define servoPIN 14

Servo servomotor;
LiquidCrystal_I2C lcd(0x27, 20, 4);

void setup() {
  lcd.init();
  lcd.backlight();
  pinMode(trigPIN, OUTPUT);
  pinMode(echoPIN, INPUT);
  servomotor.attach(servoPIN);
}

void loop() {
  long duration;
  float distance;

  digitalWrite(trigPIN, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPIN, LOW);
  duration = pulseIn(echoPIN, HIGH);
  distance = duration * 0.034 / 2; // c=340 m/s --> 0.034 cm/us

  //trigonometry and geometry
  float heightOfSensor = 5; 
  float angleRad = atan(distance / heightOfSensor); // Calculate the angle in radians
  float angleDeg = angleRad * 180 / PI; // Convert to degrees
  int angle = map(angleDeg, 0, 90, 0, 180); // set the angle to servo rotation
  servomotor.write(angle);

  if (distance > 25) {
    servomotor.write(90);
    lcd.setCursor(0, 0);
    lcd.print("Smart Obj Detector");
    lcd.setCursor(0, 1);
    lcd.print("Distance: ");
    lcd.print(distance);
    lcd.print(" cm");
    lcd.setCursor(0, 2);
    lcd.print("Out of Reach, Reset.");
    lcd.setCursor(0, 3);
    lcd.print("Cby:Paulo R. Bendijo");
  }
  else{
    lcd.setCursor(0, 0);
    lcd.print("Smart Obj Detector");
    lcd.setCursor(0, 1);
    lcd.print("Distance: ");
    lcd.print(distance);
    lcd.print(" cm");
    lcd.setCursor(0, 2);
    lcd.print("Direction: ");
    lcd.print(angle);
    lcd.print(" deg");
    lcd.setCursor(0, 3);
    lcd.print("Cby:Paulo R. Bendijo");
  }
  delay(2000);
}