/**************************************************
*  SMART OBJECT DETECTION SYSTEM USING ESP32      * 
*  SUBMITTED BY: ALO, CHRISTINE    * 
*    SUBMITTED TO: PROF. MICHAEL T. SAMONTE       * 
* *************************************************/

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#include <math.h> //math library for trigonometry

#define PIN_TRIG 14
#define PIN_ECHO 5
#define SERVO_PIN 18

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

void setup() {
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  lcd.init();
  lcd.backlight();
  lcd.begin(0x27, 20, 4);

  servo.attach(SERVO_PIN);
}

void loop() {
  long duration;
  float distance;

  lcd.setCursor(0, 0);
  lcd.print("Object Detector");

  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  duration = pulseIn(PIN_ECHO, HIGH);
  distance = 0.017 * duration; 

  lcd.setCursor(0, 1);
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print("cm");

  float heightOfSensor = 5; 
  float angleRad = atan(distance / heightOfSensor);
  float angleDeg = angleRad * 180 / PI; 

  int angle = map(angleDeg, 0, 90, 0, 180); 
  servo.write(angle);

  lcd.setCursor(0, 2);
  lcd.print("Angle: ");
  lcd.print(angle);
  lcd.print("deg");
  
  lcd.setCursor(0, 3);
  lcd.print("Christine Alo_CPET3C");

  delay(2000);
}