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

#define TFT_CS   5
#define TFT_DC   2
#define TFT_RST  4
#define TRIG_PIN 26
#define ECHO_PIN 25
#define SERVO_PIN 13

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

#define BLACK 0x0000
#define GREEN 0x07E0
#define RED 0xF800
#define WHITE 0xFFFF
#define DARKGREEN 0x03E0

const int Xmax = 320;
const int Ymax = 240;
const int Xcent = Xmax / 2;
const int BaseY = Ymax - 40;
const int ScanRadius = 150;
const int minDistance = 30;
const int maxDistance = 400;

const int angleStep = 2;
const int minAngle = 0;
const int maxAngle = 180;
int servoAngle = minAngle;
int direction = 1;

int lastSweepAngle = -1;
bool backgroundDrawn = false;

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

  long duration = pulseIn(ECHO_PIN, HIGH, 15000);
  return (duration == 0) ? 999 : duration * 0.034 / 2;
}

void drawRadarFrame() {
  if (backgroundDrawn) return;
  
  tft.fillScreen(BLACK);
  
  // Tylko najważniejsze elementy siatki
  for (int r = 1; r <= 4; r++) {
    int radiusPixels = map(r * 100, 0, 400, 0, ScanRadius);
    for (int angle = 0; angle <= 180; angle += 5) {
      float rad = radians(angle);
      int x = Xcent + cos(rad) * radiusPixels;
      int y = BaseY - sin(rad) * radiusPixels;
      tft.drawPixel(x, y, DARKGREEN);
    }
  }

  tft.drawLine(0, BaseY, Xmax, BaseY, DARKGREEN);
  
  int angles[] = {0, 45, 90, 135, 180};
  for (int i = 0; i < 5; i++) {
    float rad = radians(angles[i]);
    int x = Xcent - ScanRadius * cos(rad);
    int y = BaseY - ScanRadius * sin(rad);
    tft.drawLine(Xcent, BaseY, x, y, DARKGREEN);
  }
  
  tft.drawRect(0, 0, 320, 240, WHITE);
  backgroundDrawn = true;
}

void drawSweepLine(int angle) {
  // 1. Usuń TYLKO starą linię (jeśli istnieje)
  if (lastSweepAngle != -1) {
    int prevRadarAngle = 180 - lastSweepAngle;
    float prevRad = radians(prevRadarAngle);
    int prevX = Xcent + ScanRadius * cos(prevRad);
    int prevY = BaseY - ScanRadius * sin(prevRad);
    
    // Rysuj czarną linię TYLKO tam gdzie była poprzednia linia
    tft.drawLine(Xcent, BaseY, prevX, prevY, BLACK);
    
    // Odtwórz tło POD starą linią (tylko siatkę)
    for (int r = 1; r <= 4; r++) {
      int radiusPixels = map(r * 100, 0, 400, 0, ScanRadius);
      for (int a = lastSweepAngle - 1; a <= lastSweepAngle + 1; a++) {
        float rad = radians(180 - a);
        int x = Xcent + radiusPixels * cos(rad);
        int y = BaseY - radiusPixels * sin(rad);
        if (x >= 0 && x < Xmax && y >= 0 && y < Ymax) {
          tft.drawPixel(x, y, DARKGREEN);
        }
      }
    }
  }
  
  // 2. Narysuj nową linię
  int radarAngle = 180 - angle;
  float rad = radians(radarAngle);
  int x = Xcent + ScanRadius * cos(rad);
  int y = BaseY - ScanRadius * sin(rad);
  tft.drawLine(Xcent, BaseY, x, y, GREEN);
  
  lastSweepAngle = angle;
}

void drawTarget(int angle, int distance) {
  int radarAngle = 180 - angle;
  float rad = radians(radarAngle);
  int distancePixels = map(distance, 0, maxDistance, 0, ScanRadius);
  int x = Xcent + distancePixels * cos(rad);
  int y = BaseY - distancePixels * sin(rad);

  tft.fillCircle(x, y, 3, RED);
}

void displayAngle() {
  tft.fillRect(0, Ymax - 24, 100, 20, BLACK);
  tft.setCursor(3, Ymax - 20);
  tft.setTextColor(GREEN);
  tft.setTextSize(2);
  tft.print("DEG:");
  tft.print(servoAngle);
}

void setup() {
  Serial.begin(115200);
  tft.begin();
  tft.setRotation(1);
  tft.setSPISpeed(40000000);
  
  drawRadarFrame();
  
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  radarServo.attach(SERVO_PIN);
  radarServo.write(servoAngle);
  
  Serial.println("Radar ESP32 - optimized");
}

void loop() {
  radarServo.write(servoAngle);
  delay(10);
  
  int distance = readDistance();
  drawSweepLine(servoAngle);
  
  if (distance <= maxDistance && distance > 0) {
    drawTarget(servoAngle, distance);
  }
  
  displayAngle();
  
  servoAngle += direction * angleStep;
  if (servoAngle >= maxAngle || servoAngle <= minAngle) {
    direction = -direction;
    delay(50);
  }
}