#include <Adafruit_GFX.h>
#include <Adafruit_ST7735.h>
#include <Servo.h>

// ===== TFT Pins =====
#define TFT_CS     6
#define TFT_DC     7
#define TFT_RST    8

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

// ===== Servo & Ultrasonic =====
Servo myServo;
#define SERVO_PIN 3
#define TRIG_PIN 9
#define ECHO_PIN 10

long duration;
int distance;

void setup() {
  Serial.begin(9600);

  // TFT Init
  tft.initR(INITR_BLACKTAB); 
  tft.setRotation(1);
  tft.fillScreen(ST77XX_BLACK);

  // Text Title
  tft.setTextColor(ST77XX_WHITE);
  tft.setTextSize(2);
  tft.setCursor(10, 5);
  tft.print("SONAR RADAR");

  myServo.attach(SERVO_PIN);

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

int getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH, 20000); // 20ms timeout
  int dist = duration * 0.034 / 2;
  if (dist == 0 || dist > 200) dist = 200; // limit to 200cm
  return dist;
}

void loop() {
  // Sweep from 0° to 180°
  for (int pos = 0; pos <= 180; pos += 2) {
    myServo.write(pos);
    distance = getDistance();
    drawRadar(pos, distance);
  }

  // Sweep back
  for (int pos = 180; pos >= 0; pos -= 2) {
    myServo.write(pos);
    distance = getDistance();
    drawRadar(pos, distance);
  }
}

void drawRadar(int angle, int dist) {
  // Clear radar area
  tft.fillRect(0, 30, 160, 128, ST77XX_BLACK);

  // Draw semi-circle grid
  tft.drawCircle(80, 120, 40, ST77XX_DARKGREEN);
  tft.drawCircle(80, 120, 70, ST77XX_DARKGREEN);
  tft.drawCircle(80, 120, 100, ST77XX_DARKGREEN);
  tft.drawLine(80, 120, 80, 20, ST77XX_DARKGREEN);

  // Convert angle to radians
  float rad = radians(angle);
  int x = 80 + (dist * cos(rad) * 0.6); // scale
  int y = 120 - (dist * sin(rad) * 0.6);

  // Draw sweep line
  int sx = 80 + 100 * cos(rad);
  int sy = 120 - 100 * sin(rad);
  tft.drawLine(80, 120, sx, sy, ST77XX_GREEN);

  // Draw detected point
  if (dist < 200) {
    tft.fillCircle(x, y, 3, ST77XX_RED);
  }

  // Print distance text
  tft.setTextColor(ST77XX_YELLOW, ST77XX_BLACK);
  tft.setTextSize(2);
  tft.setCursor(10, 150);
  tft.print("Dist: ");
  tft.print(dist);
  tft.print("cm");

  delay(60);
}