// --- ESP32 + Adafruit_ILI9341 Radar-Demo (erweitert) ---
// Halbkreis-Radar mit:
// - Reichweitenringen & Tickmarks
// - Driftenden Returns (grün/gelb/rot) mit "Glow" beim Sweep
// - Animiertem Sweep (Zeiger)
// - Rotierendem Heading-Bug (Magenta-Dreieck)
// - Kurzlebigen Sparks (helle Echos)

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

// ==== Display-Pins (an dein Setup anpassen) ====
#define TFT_DC   4
#define TFT_CS   5
Adafruit_ILI9341 tft(TFT_CS, TFT_DC);  // VSPI (ESP32): SCK=18, MOSI=23, MISO=19

// ==== Farben / Helfer ====
static inline uint16_t RGB565(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}
const uint16_t COLOR_DARKGREY  = RGB565(60,60,60);
const uint16_t COLOR_LIGHTGREY = RGB565(200,200,200);
const uint16_t COLOR_BG        = ILI9341_BLACK;
const uint16_t COLOR_SWEEP     = ILI9341_MAGENTA;

// ==== Geometrie des Radar-Scopes ====
int16_t W=0, H=0;
int16_t cx=0, cy=0;           // Zentrum (unten mittig)
const int16_t MARGIN = 8;     // Rand unten
int16_t rings[3] = {60, 100, 140};  // Beispiel-Radien

// ==== Sweep / Animation ====
float sweepDeg = 0.0f;                // Winkel 0..180°
float sweepSpeedDeg = 1.6f;           // Schritt pro Frame

// ==== Heading Bug (kleiner Marker am Außenring) ====
float bugDeg = 182.0f;                // Start "182°" (optisch)
float bugSpeedDeg = 0.25f;            // langsame Rotation
float bugLastDeg = -1.0f;

// ==== Returns (Flecken) ====
struct ReturnBlob {
  float angleDeg;        // 0..180
  int16_t radius;        // entlang Halbkreis
  uint16_t baseColor;    // Grundfarbe (grün/gelb/rot)
  uint8_t size;          // Pixelradius
  float driftDeg;        // langsame Winkel-Drift (+/-)
  int8_t driftR;         // kleine Radial-Drift (+/-)
  uint8_t energy;        // 0..255 "Glow"-Intensität
  int16_t x, y;          // aktuelle Position
  int16_t px, py;        // vorige Position (zum sauberen Löschen)
};
const uint8_t N_RETURNS = 28;
ReturnBlob blobs[N_RETURNS];

// ==== Sparks (kurze helle Echos) ====
struct Spark {
  bool alive;
  int16_t x,y;
  uint8_t ttl;           // Lebensdauer in Frames
};
const uint8_t N_SPARKS = 10;
Spark sparks[N_SPARKS];

// ==== Utilities ====

// Grad -> Radiant
inline float deg2rad(float d) { return d * 3.14159265f / 180.0f; }

// Punkt auf Halbkreis aus Winkel & Radius
inline void polarToXY(float deg, int16_t r, int16_t &x, int16_t &y) {
  float rad = deg2rad(deg);
  x = cx + (int16_t)(r * cos(rad));
  y = cy - (int16_t)(r * sin(rad));
}

// Bogen aus Pixeln (dicker = mehrere Spuren)
void drawArc(float degStart, float degEnd, int16_t radius, uint16_t color, uint8_t thickness=1) {
  float step = 1.0f; // 1° Schritt
  for (float d = degStart; d <= degEnd; d += step) {
    int16_t x,y;
    polarToXY(d, radius, x, y);
    for (uint8_t t=0; t<thickness; ++t) {
      tft.drawPixel(x, y-t, color);
    }
  }
}

// Tickmarks alle 10°
void drawTicks(int16_t rOuter, int16_t rInner, uint16_t color) {
  for (int deg=0; deg<=180; deg+=10) {
    int16_t x1,y1,x2,y2;
    polarToXY(deg, rOuter, x1, y1);
    polarToXY(deg, rInner, x2, y2);
    tft.drawLine(x1,y1,x2,y2,color);
  }
}

// Farbe "aufhellen" abhängig von Energie 0..255 (einfaches Blend zu Weiß)
uint16_t brighten(uint16_t c, uint8_t energy) {
  // 565 zu 8-bit
  uint8_t r = (c >> 11) & 0x1F; r <<= 3;
  uint8_t g = (c >> 5)  & 0x3F; g <<= 2;
  uint8_t b =  c        & 0x1F; b <<= 3;
  // simple blend: c' = c + alpha*(255-c)
  float a = energy / 255.0f;
  r = (uint8_t)(r + a * (255 - r));
  g = (uint8_t)(g + a * (255 - g));
  b = (uint8_t)(b + a * (255 - b));
  return RGB565(r,g,b);
}

// ==== Returns initialisieren ====
void initReturns() {
  for (uint8_t i=0; i<N_RETURNS; ++i) {
    float base = (i < 20) ? (20 + random(0,70)) : random(0,180); // Winkelband für "Wetterfront"
    blobs[i].angleDeg = base + random(-4, 5) * 0.5f;             // Jitter
    blobs[i].radius   = rings[random(0,3)] + random(-6, 7);      // nahe am Ring
    int r = random(0,100);
    blobs[i].baseColor = (r < 70) ? ILI9341_GREEN : (r < 92 ? ILI9341_YELLOW : ILI9341_RED);
    blobs[i].size  = 2 + (r > 92 ? 1 : 0);
    blobs[i].driftDeg = (random(-4,5)) * 0.02f;                  // sehr langsam
    blobs[i].driftR   = (int8_t)random(-1,2);
    blobs[i].energy   = 0;
    polarToXY(blobs[i].angleDeg, blobs[i].radius, blobs[i].x, blobs[i].y);
    blobs[i].px = blobs[i].x; blobs[i].py = blobs[i].y;
  }
}

// ==== Sparks initialisieren ====
void initSparks() {
  for (uint8_t i=0; i<N_SPARKS; ++i) {
    sparks[i].alive = false;
    sparks[i].x = sparks[i].y = 0;
    sparks[i].ttl = 0;
  }
}

// ==== statisches Radar-Layout zeichnen ====
void renderScopeStatic() {
  tft.fillScreen(COLOR_BG);

  // Ringe
  for (uint8_t i=0; i<3; ++i) {
    drawArc(0, 180, rings[i], COLOR_DARKGREY, 1);
  }
  // Tickmarks am Außenring
  drawTicks(rings[2], rings[2]-8, COLOR_LIGHTGREY);

  // Mittellinie (0° - 180°)
  int16_t xL,yL,xR,yR;
  polarToXY(0,  rings[2], xR, yR);
  polarToXY(180,rings[2], xL, yL);
  tft.drawLine(xL,yL, xR,yR, COLOR_LIGHTGREY);

  // 90°-Achse (nach oben)
  int16_t xU,yU;
  polarToXY(90, rings[2], xU, yU);
  tft.drawLine(cx,cy, xU,yU, COLOR_LIGHTGREY);

  // Kopfzeile (Mini-Labels)
  tft.setTextColor(COLOR_LIGHTGREY);
  tft.setTextSize(1);
  tft.setCursor(8, 8);
  tft.print("TRK   MAG");
  tft.setCursor(W/2 - 14, 6);
  tft.setTextSize(2);
  tft.print("182");
  tft.setTextSize(1);
  tft.setCursor(W-70, 10);
  tft.print("TFC");
}

// ==== Returns zeichnen (Full-Refresh) ====
void drawReturnsFull() {
  for (uint8_t i=0; i<N_RETURNS; ++i) {
    uint16_t col = brighten(blobs[i].baseColor, blobs[i].energy);
    tft.fillCircle(blobs[i].x, blobs[i].y, blobs[i].size, col);
  }
}

// ==== Sweep aktualisieren (löschen + neu zeichnen) ====
void updateSweep() {
  static float lastDeg = -1.0f;
  // alten Sweep löschen
  if (lastDeg >= 0.0f) {
    int16_t xOld,yOld;
    polarToXY(lastDeg, rings[2], xOld, yOld);
    tft.drawLine(cx,cy, xOld,yOld, COLOR_BG);
  }
  // neuen Sweep
  int16_t xNew,yNew;
  polarToXY(sweepDeg, rings[2], xNew, yNew);
  tft.drawLine(cx,cy, xNew,yNew, COLOR_SWEEP);
  lastDeg = sweepDeg;

  // Winkel fortschreiben
  sweepDeg += sweepSpeedDeg;
  if (sweepDeg > 180.0f) sweepDeg -= 180.0f;
}

// ==== Heading-Bug (kleines Dreieck am Außenring) ====
void updateHeadingBug() {
  // alten Bug löschen
  if (bugLastDeg >= 0.0f) {
    int16_t xa,ya,xb,yb,xc,yc;
    polarToXY(bugLastDeg,   rings[2], xb, yb);       // Spitze
    polarToXY(bugLastDeg-4, rings[2]-10, xa, ya);    // linke Basis
    polarToXY(bugLastDeg+4, rings[2]-10, xc, yc);    // rechte Basis
    tft.fillTriangle(xa,ya, xb,yb, xc,yc, COLOR_BG);
  }
  // neuen Bug zeichnen
  int16_t xa,ya,xb,yb,xc,yc;
  polarToXY(bugDeg,   rings[2], xb, yb);
  polarToXY(bugDeg-4, rings[2]-10, xa, ya);
  polarToXY(bugDeg+4, rings[2]-10, xc, yc);
  tft.fillTriangle(xa,ya, xb,yb, xc,yc, COLOR_SWEEP);

  bugLastDeg = bugDeg;
  bugDeg += bugSpeedDeg;
  if (bugDeg > 180.0f) bugDeg -= 180.0f;
}

// ==== Returns bewegen + Glow beim Sweep ====
void updateReturns() {
  // Toleranz für "Treffer" (Sweep nahe am Blob)
  const float hitTolDeg = 2.0f;

  for (uint8_t i=0; i<N_RETURNS; ++i) {
    // alten Blob löschen (wenn Position sich ändert)
    tft.fillCircle(blobs[i].px, blobs[i].py, blobs[i].size, COLOR_BG);

    // Bewegung
    blobs[i].angleDeg += blobs[i].driftDeg;
    if (blobs[i].angleDeg < 0)   blobs[i].angleDeg += 180.0f;
    if (blobs[i].angleDeg > 180) blobs[i].angleDeg -= 180.0f;

    blobs[i].radius += blobs[i].driftR;
    // Begrenzen auf sinnvollen Bereich
    int16_t maxR = (int16_t)(H - 2*MARGIN);
    if (blobs[i].radius < 30)  blobs[i].radius = 30;
    if (blobs[i].radius > maxR) blobs[i].radius = maxR;

    // neue Position
    blobs[i].px = blobs[i].x; blobs[i].py = blobs[i].y;
    polarToXY(blobs[i].angleDeg, blobs[i].radius, blobs[i].x, blobs[i].y);

    // Glow-Logik: wenn Sweep nahe dem Blob -> Energie hoch
    float diff = fabs(blobs[i].angleDeg - sweepDeg);
    if (diff < hitTolDeg) {
      uint8_t boost = 180; // kräftig aufhellen
      uint16_t e = blobs[i].energy + boost;
      blobs[i].energy = (e > 255) ? 255 : e;
    } else {
      // langsames Abklingen
      if (blobs[i].energy > 5) blobs[i].energy -= 5; else blobs[i].energy = 0;
    }

    // zeichnen mit aufgehellter Farbe
    uint16_t col = brighten(blobs[i].baseColor, blobs[i].energy);
    tft.fillCircle(blobs[i].x, blobs[i].y, blobs[i].size, col);
  }
}

// ==== Sparks: kurzlebige helle Echos ====
void spawnSparkRandom() {
  // mit kleiner Wahrscheinlichkeit einen Spark erzeugen
  if (random(0, 100) < 4) {
    // freien Slot suchen
    for (uint8_t i=0; i<N_SPARKS; ++i) {
      if (!sparks[i].alive) {
        // zufällige Polarposition im Halbkreis
        float a = random(0,181);
        int16_t r = rings[random(0,3)] + random(-10,11);
        polarToXY(a, r, sparks[i].x, sparks[i].y);
        sparks[i].ttl = 10 + random(0,8);
        sparks[i].alive = true;
        break;
      }
    }
  }
}

void updateSparks() {
  for (uint8_t i=0; i<N_SPARKS; ++i) {
    if (!sparks[i].alive) continue;
    // zeichnen
    tft.fillCircle(sparks[i].x, sparks[i].y, 1, ILI9341_WHITE);
    // alter "Frame" löschen beim nächsten Schritt
    // (simpler Ansatz: lösche sofort leicht versetzt, damit ein kurzer Glanz bleibt)
    // Hier: wir lassen ihn einfach stehen und löschen, wenn TTL endet
    sparks[i].ttl--;
    if (sparks[i].ttl == 0) {
      tft.fillCircle(sparks[i].x, sparks[i].y, 1, COLOR_BG);
      sparks[i].alive = false;
    }
  }
}

// ==== Setup / Loop ====
void setup() {
  tft.begin();
  tft.setRotation(1); // Querformat
  W = tft.width();
  H = tft.height();
  cx = W / 2;
  cy = H - MARGIN;

  // Ringe begrenzen (ohne std::min)
  int16_t maxR = (int16_t)(H - 2 * MARGIN);
  for (uint8_t i = 0; i < 3; ++i) {
    if (rings[i] > maxR) rings[i] = maxR;
  }

  renderScopeStatic();
  initReturns();
  initSparks();

  // ersten Frame der Returns zeichnen
  drawReturnsFull();
}

// ----

// Helfer: einen "Ray" (einzelnen Winkel) für Ringe und Tickmarks nachzeichnen
void restoreAtAngle(float angleDeg) {
  // Ringe an diesem Winkel
  for (uint8_t i=0; i<3; ++i) {
    int16_t x,y;
    polarToXY(angleDeg, rings[i], x, y);
    tft.drawPixel(x, y, COLOR_DARKGREY);
  }

  // Tickmarks liegen alle 10°; wenn der "alte" Winkel nahe an einem Tick liegt, zeichnen wir ihn nach
  int nearestTick = (int)((angleDeg + 5) / 10) * 10;  // runden auf 10°
  if (fabs(angleDeg - nearestTick) < 0.7f) {          // Toleranz
    int16_t x1,y1,x2,y2;
    polarToXY(nearestTick, rings[2],     x1, y1);
    polarToXY(nearestTick, rings[2]-8,   x2, y2);
    tft.drawLine(x1,y1,x2,y2, COLOR_LIGHTGREY);
  }

  // Mittellinie (0°/180°) oder 90°-Achse trifft nur bei exakt 0,90,180°
  if (fabs(angleDeg-0.0f)   < 0.5f || fabs(angleDeg-180.0f) < 0.5f) {
    int16_t xL,yL,xR,yR;
    polarToXY(0,  rings[2], xR, yR);
    polarToXY(180,rings[2], xL, yL);
    tft.drawLine(xL,yL, xR,yR, COLOR_LIGHTGREY);
  }
  if (fabs(angleDeg-90.0f)  < 0.5f) {
    int16_t xU,yU;
    polarToXY(90, rings[2], xU, yU);
    tft.drawLine(cx,cy, xU,yU, COLOR_LIGHTGREY);
  }
}

// Update des Sweeps mit "Repair" am alten Winkel
void updateSweep() {
  static float lastDeg = -1.0f;

  // alten Sweep löschen
  if (lastDeg >= 0.0f) {
    int16_t xOld,yOld;
    polarToXY(lastDeg, rings[2], xOld, yOld);
    tft.drawLine(cx,cy, xOld,yOld, COLOR_BG);

    // Ringe/Ticks genau an diesem Winkel wiederherstellen
    restoreAtAngle(lastDeg);
  }

  // neuen Sweep zeichnen
  int16_t xNew,yNew;
  polarToXY(sweepDeg, rings[2], xNew, yNew);
  tft.drawLine(cx,cy, xNew,yNew, COLOR_SWEEP);

  lastDeg = sweepDeg;
  sweepDeg += sweepSpeedDeg;
  if (sweepDeg > 180.0f) sweepDeg -= 180.0f;
}

// ----


void loop() {
  updateSweep();
  renderScopeStatic();   // ganze Skala neu
  drawReturnsFull();     // Returns wieder drauf
  updateHeadingBug();
  spawnSparkRandom();
  updateSparks();
  delay(16);
}