#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <ESP32Servo.h>

#define TRIG_PIN 1
#define ECHO_PIN 2

#define SERVO_PIN 4
#define BUZZER_PIN 27

#define TFT_CS   15
#define TFT_DC   16
#define TFT_RST  5

Adafruit_ILI9341 tft(TFT_CS, TFT_DC, TFT_RST);
Servo radarServo;

int getDistance() {

  long totalDistance = 0;

  for (int i = 0; i < 3; i++) {

    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(5);

    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    long duration = pulseIn(ECHO_PIN, HIGH, 35000);

    int distance = duration * 0.0343 / 2;

    if (distance > 0 && distance < 400) {
      totalDistance += distance;
    }

    delay(20);
  }

  return totalDistance / 3;
}

void showData(int angle, int distance) {

  tft.fillRect(0, 40, 320, 180, ILI9341_BLACK);

  tft.setTextSize(2);

  tft.setCursor(10, 50);
  tft.setTextColor(ILI9341_CYAN);
  tft.print("Angle: ");
  tft.print(angle);

  tft.setCursor(10, 90);
  tft.setTextColor(ILI9341_YELLOW);
  tft.print("Distance: ");
  tft.print(distance);
  tft.print(" cm");

  if (distance < 20 && distance > 0) {

    tft.setCursor(10, 140);
    tft.setTextColor(ILI9341_RED);
    tft.print("OBJECT DETECTED");

    digitalWrite(BUZZER_PIN, HIGH);

  } else {

    tft.setCursor(10, 140);
    tft.setTextColor(ILI9341_GREEN);
    tft.print("AREA CLEAR");

    digitalWrite(BUZZER_PIN, LOW);
  }
}

void setup() {

  Serial.begin(115200);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  radarServo.attach(SERVO_PIN);

  tft.begin();
  tft.setRotation(1);
  tft.fillScreen(ILI9341_BLACK);

  tft.setTextSize(2);
  tft.setTextColor(ILI9341_GREEN);
  tft.setCursor(10, 10);
  tft.println("ESP32 RADAR");
}

void loop() {

  for (int angle = 0; angle <= 180; angle += 5) {

    radarServo.write(angle);

    delay(40);

    int distance = getDistance();

    Serial.print("Angle=");
    Serial.print(angle);
    Serial.print(" Distance=");
    Serial.println(distance);

    showData(angle, distance);
  }

  for (int angle = 180; angle >= 0; angle -= 5) {

    radarServo.write(angle);

    delay(40);

    int distance = getDistance();

    Serial.print("Angle=");
    Serial.print(angle);
    Serial.print(" Distance=");
    Serial.println(distance);

    showData(angle, distance);
  }
}