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

#define TFT_CS     10
#define TFT_RST    9  
#define TFT_DC     8

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

const int trigPin = 2;
const int echoPin = 3;
const int servoPin = 6;

long duration;
int distance;

Servo myservo;

int minDistance = 40; // Maksimum untuk memulai
int maxDistance = 0;
#define MAX_READINGS 50
int readings[MAX_READINGS];
int index = 0;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  myservo.attach(servoPin);
  
  tft.begin();
  tft.setRotation(3);
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);

  tft.setCursor(10, 10);
  tft.println("Ultrasonic Radar");
  
  drawRadarGrid();
}

void loop() {
  for (int pos = 0; pos <= 180; pos += 1) {
    myservo.write(pos);    
    distance = getDistance();
    drawRadar(pos, distance);
    storeReading(distance);
    delay(50); 
  }

  for (int pos = 180; pos >= 0; pos -= 1) {
    myservo.write(pos);
    distance = getDistance();
    drawRadar(pos, distance);
    storeReading(distance);
    delay(50);
  }
}

int getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;
  if (distance > 40) {
    distance = 40;  // Maksimum range 40 cm untuk visualisasi
  }

  if (distance < minDistance) {
    minDistance = distance;
  }
  if (distance > maxDistance) {
    maxDistance = distance;
  }

  return distance;
}

void drawRadarGrid() {
  tft.drawCircle(160, 160, 40, ILI9341_WHITE);
  tft.drawCircle(160, 160, 80, ILI9341_WHITE);
  tft.drawCircle(160, 160, 120, ILI9341_WHITE);
  tft.drawLine(160, 40, 160, 280, ILI9341_WHITE);
  tft.drawLine(40, 160, 280, 160, ILI9341_WHITE);
}

void drawRadar(int angle, int dist) {
  tft.fillCircle(160, 160, 120, ILI9341_BLACK);
  drawRadarGrid();
  
  float rad = angle * 3.14159 / 180;
  int x = 160 + dist * cos(rad) * 3;  
  int y = 160 - dist * sin(rad) * 3;
  
  tft.drawLine(160, 160, x, y, getColorForDistance(dist));

  tft.fillRect(0, 280, 320, 40, ILI9341_BLACK);
  tft.setCursor(10, 280);
  tft.print("Angle: ");
  tft.print(angle);
  tft.println(" deg");

  tft.setCursor(160, 280);
  tft.print("Dist: ");
  tft.print(dist);
  tft.println(" cm");
  
  // Menampilkan Jarak Min dan Max
  tft.setCursor(10, 240);
  tft.fillRect(0, 240, 320, 20, ILI9341_BLACK);
  tft.print("Min: ");
  tft.print(minDistance);
  tft.print(" cm Max: ");
  tft.print(maxDistance);
  tft.println(" cm");

  // Peringatan
  if (dist < 10) {
    tft.fillRect(0, 280, 320, 40, ILI9341_RED);
    tft.setCursor(10, 280);
    tft.setTextColor(ILI9341_RED);
    tft.println("Warning: Object too close!");
  }
}

uint16_t getColorForDistance(int dist) {
  if (dist <= 10) {
    return ILI9341_RED;
  } else if (dist <= 20) {
    return ILI9341_YELLOW;
  } else {
    return ILI9341_GREEN;
  }
}

void storeReading(int dist) {
  readings[index] = dist;
  index = (index + 1) % MAX_READINGS;
}