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

// TFT LCD pins
#define TFT_CS   2
#define TFT_RST  3
#define TFT_DC   4
#define TFT_MOSI 5
#define TFT_SCK  6
#define TFT_MISO 7

// Ultrasonic sensor pins
#define TRIG_PIN 10
#define ECHO_PIN 9

// Servo pin
#define SERVO_PIN 8

// TM1637 4-digit display pins
#define DISPLAY_CLK A1
#define DISPLAY_DIO A0

// Radar parameters
#define SERVO_START_ANGLE 0
#define SERVO_END_ANGLE 180
#define SERVO_STEP 2
#define MAX_DISTANCE 200  // Maximum detection distance in cm
#define SCAN_DELAY 15     // Delay between servo movements (ms)

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCK, TFT_RST);
Servo radarServo;
TM1637Display display(DISPLAY_CLK, DISPLAY_DIO);

// Structure to store detected objects
struct Object {
  int angle;
  int distance;
  int x;
  int y;
};

Object detectedObjects[180];  // Store up to 180 objects
int objectCount = 0;
int totalPlanesDetected = 0;  // Counter for total planes detected

// Variables for tracking unique objects
int lastDetectedAngles[50];  // Store last detected angles
int lastDetectedCount = 0;
#define ANGLE_TOLERANCE 5  // Tolerance in degrees to consider same object

void setup() {
  Serial.begin(9600);
  
  // Initialize ultrasonic pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  // Initialize servo
  radarServo.attach(SERVO_PIN);
  radarServo.write(SERVO_START_ANGLE);
  
  // Initialize TFT
  tft.begin();
  tft.setRotation(1);  // Landscape orientation
  tft.fillScreen(ILI9341_BLACK);
  
  // Initialize 4-digit display
  display.setBrightness(7);  // Set brightness (0-7)
  display.clear();
  display.showNumberDec(0, true);  // Show initial count
  
  // Draw radar grid
  drawRadarGrid();
  
  // Wait for servo to initialize
  delay(1000);
}

void loop() {
  objectCount = 0;
  
  // Scan from left to right
  for (int angle = SERVO_START_ANGLE; angle <= SERVO_END_ANGLE; angle += SERVO_STEP) {
    radarServo.write(angle);
    delay(SCAN_DELAY);
    
    int distance = measureDistance();
    
    if (distance > 0 && distance <= MAX_DISTANCE) {
      // Check if this is a new object (not detected in recent scan)
      if (isNewObject(angle)) {
        totalPlanesDetected++;
        updateDisplay(totalPlanesDetected);
        
        // Store this angle to avoid duplicate counting
        storeDetectedAngle(angle);
      }
      
      // Object detected
      int x = calculateX(angle, distance);
      int y = calculateY(angle, distance);
      
      // Store detected object
      detectedObjects[objectCount].angle = angle;
      detectedObjects[objectCount].distance = distance;
      detectedObjects[objectCount].x = x;
      detectedObjects[objectCount].y = y;
      objectCount++;
      
      // Display object on radar
      drawDetectedObject(x, y);
      
      // Show distance on display
      showDistance(angle, distance);
      
      // Send to serial monitor for debugging
      Serial.print("Object detected at ");
      Serial.print(angle);
      Serial.print(" degrees, ");
      Serial.print(distance);
      Serial.println(" cm");
    }
    
    // Clear previous sweep line and draw new one
    drawSweepLine(angle - SERVO_STEP, false);
    drawSweepLine(angle, true);
  }
  
  // Scan from right to left
  for (int angle = SERVO_END_ANGLE; angle >= SERVO_START_ANGLE; angle -= SERVO_STEP) {
    radarServo.write(angle);
    delay(SCAN_DELAY);
    
    int distance = measureDistance();
    
    if (distance > 0 && distance <= MAX_DISTANCE) {
      // Check if this is a new object
      if (isNewObject(angle)) {
        totalPlanesDetected++;
        updateDisplay(totalPlanesDetected);
        storeDetectedAngle(angle);
      }
      
      int x = calculateX(angle, distance);
      int y = calculateY(angle, distance);
      
      detectedObjects[objectCount].angle = angle;
      detectedObjects[objectCount].distance = distance;
      detectedObjects[objectCount].x = x;
      detectedObjects[objectCount].y = y;
      objectCount++;
      
      drawDetectedObject(x, y);
      showDistance(angle, distance);
      
      Serial.print("Object detected at ");
      Serial.print(angle);
      Serial.print(" degrees, ");
      Serial.print(distance);
      Serial.println(" cm");
    }
    
    drawSweepLine(angle + SERVO_STEP, false);
    drawSweepLine(angle, true);
  }
  
  // Clear detected angles for next scan cycle
  clearDetectedAngles();
  
  // Clear detected objects after each full scan
  delay(100);
  clearRadarScreen();
  drawRadarGrid();
}

int measureDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // Timeout after 30ms (~5m)
  
  if (duration == 0) {
    return -1; // No object detected
  }
  
  int distance = duration * 0.034 / 2;
  return distance;
}

int calculateX(int angle, int distance) {
  // Convert angle to radians and calculate x coordinate
  // Map angle: 0° = right side, 180° = left side
  float rad = radians(angle);
  int x = tft.width() - (distance * 2.5 * cos(rad)); // Scale factor for display
  return constrain(x, 0, tft.width() - 1);
}

int calculateY(int angle, int distance) {
  // Calculate y coordinate (height of the display)
  float rad = radians(angle);
  int y = tft.height() - (distance * 2.0 * sin(rad)); // Scale factor for display
  return constrain(y, 0, tft.height() - 1);
}

void drawRadarGrid() {
  int centerX = tft.width() / 2;
  int centerY = tft.height() - 20;
  
  // Draw distance circles
  tft.drawCircle(centerX, centerY, 50, ILI9341_DARKGREEN);
  tft.drawCircle(centerX, centerY, 100, ILI9341_DARKGREEN);
  tft.drawCircle(centerX, centerY, 150, ILI9341_DARKGREEN);
  
  // Draw angle lines
  for (int angle = 0; angle <= 180; angle += 30) {
    float rad = radians(angle);
    int x1 = centerX;
    int y1 = centerY;
    int x2 = centerX + 150 * cos(rad);
    int y2 = centerY - 150 * sin(rad);
    tft.drawLine(x1, y1, x2, y2, ILI9341_DARKGREEN);
    
    // Draw angle labels
    tft.setCursor(x2 - 10, y2 - 5);
    tft.setTextColor(ILI9341_GREEN);
    tft.setTextSize(1);
    tft.print(angle);
    tft.print("°");
  }
  
  // Draw distance labels
  tft.setCursor(centerX + 50, centerY - 5);
  tft.print("50");
  tft.setCursor(centerX + 100, centerY - 5);
  tft.print("100");
  tft.setCursor(centerX + 150, centerY - 5);
  tft.print("150");
  
  // Draw title
  tft.setTextSize(2);
  tft.setCursor(10, 10);
  tft.setTextColor(ILI9341_RED);
  tft.print("RADAR SYSTEM");
  
  tft.setTextSize(1);
  tft.setCursor(10, 35);
  tft.setTextColor(ILI9341_YELLOW);
  tft.print("Objects Detected: 0");
  
  // Draw total planes counter on TFT
  tft.setCursor(10, 50);
  tft.setTextColor(ILI9341_CYAN);
  tft.print("Total Planes: 0");
}

void drawSweepLine(int angle, boolean draw) {
  int centerX = tft.width() / 2;
  int centerY = tft.height() - 20;
  
  float rad = radians(angle);
  int x2 = centerX + 180 * cos(rad);
  int y2 = centerY - 180 * sin(rad);
  
  uint16_t color = draw ? ILI9341_YELLOW : ILI9341_BLACK;
  tft.drawLine(centerX, centerY, x2, y2, color);
}

void drawDetectedObject(int x, int y) {
  tft.fillCircle(x, y, 3, ILI9341_RED);
  tft.fillCircle(x, y, 2, ILI9341_ORANGE);
  tft.fillCircle(x, y, 1, ILI9341_YELLOW);
}

void showDistance(int angle, int distance) {
  // Show detected object info on the top right
  tft.fillRect(180, 35, 140, 25, ILI9341_BLACK);
  tft.setCursor(180, 35);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.print("Angle: ");
  tft.print(angle);
  tft.print("°  Dist: ");
  tft.print(distance);
  tft.print("cm");
  
  // Update current scan object count
  tft.fillRect(10, 35, 140, 15, ILI9341_BLACK);
  tft.setCursor(10, 35);
  tft.setTextColor(ILI9341_YELLOW);
  tft.print("Objects Detected: ");
  tft.print(objectCount + 1);
  
  // Update total planes counter on TFT
  tft.fillRect(10, 50, 140, 15, ILI9341_BLACK);
  tft.setCursor(10, 50);
  tft.setTextColor(ILI9341_CYAN);
  tft.print("Total Planes: ");
  tft.print(totalPlanesDetected);
}

void clearRadarScreen() {
  // Clear only the radar area (center area)
  int centerX = tft.width() / 2;
  int centerY = tft.height() - 20;
  int radius = 180;
  
  for (int y = 0; y < tft.height(); y++) {
    for (int x = 0; x < tft.width(); x++) {
      int dx = x - centerX;
      int dy = y - centerY;
      if (sqrt(dx*dx + dy*dy) <= radius) {
        tft.drawPixel(x, y, ILI9341_BLACK);
      }
    }
  }
}

// Function to check if detected object is new (not detected in current scan)
boolean isNewObject(int angle) {
  for (int i = 0; i < lastDetectedCount; i++) {
    if (abs(lastDetectedAngles[i] - angle) <= ANGLE_TOLERANCE) {
      return false;  // Object already detected in this scan
    }
  }
  return true;  // New object
}

// Function to store detected angle to avoid duplicate counting
void storeDetectedAngle(int angle) {
  if (lastDetectedCount < 50) {
    lastDetectedAngles[lastDetectedCount] = angle;
    lastDetectedCount++;
  }
}

// Function to clear detected angles array for next scan
void clearDetectedAngles() {
  lastDetectedCount = 0;
  for (int i = 0; i < 50; i++) {
    lastDetectedAngles[i] = 0;
  }
}

// Function to update the 4-digit display
void updateDisplay(int count) {
  // Show number on 4-digit display
  display.showNumberDec(count, true);
  
  // Optional: Add special effect when count increases
  if (count > 0 && count % 10 == 0) {
    // Blink display for every 10th detection
    delay(100);
    display.clear();
    delay(100);
    display.showNumberDec(count, true);
  }
}