#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Servo.h>
// Piny TFT (hardware SPI: MOSI=51, MISO=50, SCK=52)
#define TFT_CS   10
#define TFT_DC    9
#define TFT_RST   8
//color definition
#define BLACK 0x0000       ///<   0,   0,   0
#define NAVY 0x000F        ///<   0,   0, 123
#define DARKGREEN 0x03E0   ///<   0, 125,   0
#define DARKCYAN 0x03EF    ///<   0, 125, 123
#define MAROON 0x7800      ///< 123,   0,   0
#define PURPLE 0x780F      ///< 123,   0, 123
#define OLIVE 0x7BE0       ///< 123, 125,   0
#define LIGHTGREY 0xC618   ///< 198, 195, 198
#define DARKGREY 0x7BEF    ///< 123, 125, 123
#define BLUE 0x001F        ///<   0,   0, 255
#define GREEN 0x07E0       ///<   0, 255,   0
#define CYAN 0x07FF        ///<   0, 255, 255
#define RED 0xF800         ///< 255,   0,   0
#define MAGENTA 0xF81F     ///< 255,   0, 255
#define YELLOW 0xFFE0      ///< 255, 255,   0
#define WHITE 0xFFFF       ///< 255, 255, 255
#define ORANGE 0xFD20      ///< 255, 165,   0
#define GREENYELLOW 0xAFE5 ///< 173, 255,  41
#define PINK 0xFC18        ///< 255, 130, 198
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);
// Piny HC-SR04
#define TRIG_PIN 7
#define ECHO_PIN 6
// Piny serwa
#define SERVO_PIN 5
Servo radarServo;
// Parametry ekranu
const int Xmax = 320;
const int Ymax = 240;
const int Xcent = Xmax / 2;
const int BaseY = Ymax - 40; // Mniej miejsca na dolnym marginesie
const int ScanRadius = (Xmax - 20) / 2;  // Zmniejszony promień dla 4m
const int minDistance = 30;  // minialma odległość skanowania
// Ustawienia skanowania
const int angleStep = 1;
const int minAngle = 0;
const int maxAngle = 180;
int servoAngle = minAngle;
int direction = 1;
// Zakres odległości (do 4m)
const int maxDistance = 400; // 4 metry = 400 cm
// Buforowanie dla płynności
int prevAngle = minAngle;
int prevDistance = 0;
bool firstScan = true;
//  Pomiar odległości
int readDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  // Timeout dla ~4m
  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  if (duration == 0)
    return 999; // Brak sygnału

  int distance = duration * 0.034 / 2;
  if (distance < minDistance || distance > maxDistance) {
    return 999;  // ignorujemy zbyt blisko lub zbyt daleko
  }
  return distance;
}
// Rysowanie siatki radaru
void drawRadarFrame() {
  tft.fillScreen(ILI9341_BLACK);
  // Okręgi co 1m (100cm) - SKALA DOSTOSOWANA DO 4m
  for (int r = 1; r <= 4; r++) {
    int radiusPixels = map(r * 100, 0, 400, 0, ScanRadius);
    // rysowanie półokręgu zamiast pełnego okręgu
    for (int angle = 0; angle <= 180; angle++) {
      float rad = radians(angle);
      int x = Xcent + cos(rad) * radiusPixels;
      int y = BaseY - sin(rad) * radiusPixels;
      tft.drawPixel(x, y, ILI9341_DARKGREEN);
    }
    // etykiety odległości
    tft.setTextColor(ILI9341_GREEN);
    tft.setTextSize(1);
    tft.setCursor(Xcent + radiusPixels, BaseY + 5);
    tft.print(r);
    tft.setCursor(Xcent - radiusPixels, BaseY + 5);
    tft.print(r);
  }
  // Linia bazowa
  tft.drawLine(0, BaseY, Xmax, BaseY, ILI9341_DARKGREEN);
  // Linie pomocnicze kątów (0, 30, 60, 90, 120, 150, 180)
  int angles[] = {0, 30, 60, 90, 120, 150, 180};
  for (int i = 0; i < 7; i++) {
    int angle = angles[i];
    float rad = radians(angle);
    int x = Xcent - ScanRadius * cos(rad);  // MINUS zamiast PLUS
    int y = BaseY - ScanRadius * sin(rad);
    tft.drawLine(Xcent, BaseY, x, y, ILI9341_DARKGREEN);
    // Etykiety kątów
    int labelX = Xcent - (ScanRadius + 10) * cos(rad);
    int labelY = BaseY - (ScanRadius + 10) * sin(rad);
    tft.setCursor(labelX - 8, labelY - 4);
    tft.print(angle);
  }
}
// Rysowanie skanującej kreski
void drawSweepLine(int angle, uint16_t color) {
  // KOREKTA KIERUNKU: serwo 15°-165° = radar 165°-15°
  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, color);
}
// Rysowanie punktu celu
void drawTarget(int angle, int distance) {
  if (distance > maxDistance || distance <= 0) return;
  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);
  // tylko czerwony punkt (bez zielonej obwódki)
  tft.fillCircle(x, y, 3, ILI9341_RED);
}
// Wymazywanie poprzedniej linii
void erasePreviousScan() {
  if (firstScan) {
    firstScan = false;
    return;
  }
  // Wymaż poprzednią linię (z korektą kierunku)
  int prevRadarAngle = 180 - prevAngle;
  float prevRad = radians(prevRadarAngle);
  int prevX = Xcent + ScanRadius * cos(prevRad);
  int prevY = BaseY - ScanRadius * sin(prevRad);
  tft.drawLine(Xcent, BaseY, prevX, prevY, ILI9341_BLACK);
}

void setup() {
  Serial.begin(115200);
  // TFT
  tft.begin();
  tft.setRotation(3);
  drawRadarFrame();
  // czujnik
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  // serwo
  radarServo.attach(SERVO_PIN);
  radarServo.write(servoAngle);
  // Początkowe wartości
  prevAngle = servoAngle;
  Serial.println("Radar system initialized - 0-4m range");
  tft.drawRect(0, 0, Xmax, Ymax, 0xFFFF);
  //tft.drawLine(320, 0, 0, 240, ILI9341_RED);
  //tft.drawLine(0, 0, 320, 240, ILI9341_BLUE);
  //tft.drawCircle(320/2, 240/2, 60, ILI9341_WHITE);
  //tft.drawCircleHelper(0, 0, 60, 0x4, ILI9341_RED);
  //tft.drawCircleHelper(320/2, 240/2, 60, 0x2, ILI9341_RED);
}

void loop() {
  // Aktualizuj pozycję serwa
  radarServo.write(servoAngle);
  delay(15); // Dla płynności serwa
  // Wymaż poprzednią linię skanowania
  erasePreviousScan();
  // Odczytaj odległość
  int distance = readDistance();
  // Narysuj nową linię skanowania
  drawSweepLine(servoAngle, ILI9341_GREEN);
  // Jeśli wykryto obiekt, narysuj cel
  if (distance <= maxDistance && distance > 0) {
    drawTarget(servoAngle, distance);
    // Debug - pokaż informacje o wykrytym obiekcie
    /*
      Serial.print("Object at: ");
      Serial.print(180 - servoAngle); // Kąt radaru
      Serial.print("°, ");
      Serial.print(distance);
      Serial.println("cm");*/
  }
  // Zapisz aktualną pozycję dla następnej iteracji
  prevAngle = servoAngle;
  prevDistance = distance;
  // Aktualizuj kąt dla następnego kroku
  servoAngle += direction * angleStep;
  // Zmiana kierunku na krańcach
  if (servoAngle >= maxAngle) {
    direction = -1;
    // Małe opóźnienie na końcu skanu
    delay(100);
  } else if (servoAngle <= minAngle) {
    direction = 1;
    // Małe opóźnienie na końcu skanu
    delay(100);
  }
  // Pasek statusu na dole
  tft.setCursor(3, Ymax - 17);
  tft.setTextColor(ILI9341_GREEN, ILI9341_BLACK); // nadpisuje czarnym tło
  tft.setTextSize(2);
  tft.print("DEG:");
  tft.print(servoAngle);
  // Krótkie opóźnienie dla stabilności
  delay(10);
}