#include <TFT_eSPI.h> // Graphics and font library for ILI9341 driver chip
#include <SPI.h>

TFT_eSPI tft = TFT_eSPI();  // Invoke library

#define SCREEN_WIDTH 320
#define SCREEN_HEIGHT 240
#define FONT_SIZE 2

// int animationIndex = 0;         // Current animation index

struct Arc {
  int x, y;     // Center of the arc
  int r;        // Radius of the arc
  bool active;  // Is the arc active?
};

const byte interruptPin = 27;
const int trigPin = 4;
const int echoPin = 5;

#define SOUND_SPEED 0.034

long duration;
float distanceFiltered;

unsigned long lastRadarUpdate = 0;
const unsigned long radarInterval = 10;

bool blipVisible = true;
unsigned long lastBlinkTime = 0;

const unsigned long blinkInterval = 50;
int previousBlipX = 0;
int previousBlipY = 0;

const int filterSize = 16;
float distanceBuffer[filterSize] = { 0 };
int bufferIndex = 0;

const int maxRadius = 240;

Arc arcs[3];
int activeArcCount = 2;

uint16_t del = 20;
uint16_t delSingle = 0;
unsigned long last_run = 0;
unsigned long last_button = 0;

//CHANGE THIS TO affect the speed of the updates for numbers. Lower the number the faster it updates.
int updateTime = 40;

int InitY[] = { 320, 300, 280 };

int cornerBarY[] = { 260, 240, 220, 200, 180, 160, 140 };
int centreBarY[] = { 140, 160, 180, 200, 220, 240, 260 };

int cornerBarX1 = 20;
int cornerBarX2 = 200;
int centreBarX1 = 80;
int centreBarX2 = 140;

int cornerSingleEndY1[] = { 280, 260, 240, 220, 200, 180, 160, 140, 160, 180, 200, 220, 240, 260, 280 };
int centreSingleEndY1[] = { 200, 180, 160, 140, 160, 180, 200, 220, 240, 260, 280, 260, 240, 220, 200 };
int centreSingleEndY2[] = { 260, 240, 220, 200, 180, 160, 140, 160, 180, 200, 220, 240, 260, 280, 260 };
int cornerSingleEndY2[] = { 220, 200, 180, 160, 140, 160, 180, 200, 220, 240, 260, 280, 260, 240, 220 };

int lineX = 0;
int lineY = 120;
int lineY2 = 121;

int topX[] = { 60, 180 };
int topY[] = { 80, 80 };
int bottomX[] = { 60, 180 };
int bottomY[] = { 240, 240 };

int circleX[] = { 15, 35, 55, 75, 95, 114, 131, 146, 160, 169, 176, 182, 187, 192 };
int circleY[] = { 80, 80, 80, 80, 85, 94, 106, 122, 139, 158, 178, 198, 218, 239 };

int prevTopSize = 0;
int prevBtmSize = 0;

// uint8_t counter = 0;
// String dir = "";

volatile byte state = 0;

boolean buttonPressed = false;

boolean topCapSize = false;
boolean btmCapSize = false;

boolean topClrSize = false;
boolean btmClrSize = false;

volatile boolean intrptEnable = true;

// void IRAM_ATTR handleTouchInterrupt();
void IRAM_ATTR buttonState();

void setup() {
  tft.begin();
  tft.setRotation(0);
  tft.fillScreen(TFT_BLACK);

  Serial.begin(115200);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  float initialMeasurement = getDistance();
  applyFilter(initialMeasurement);

  arcs[0] = { 120, 305, 10, true };
  arcs[1] = { 120, 305, 0, false };
  arcs[2] = { 120, 305, 0, false };

  pinMode(interruptPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(interruptPin), buttonState, FALLING);
}

void loop() {
    if (state == 0)
    drawBars();

  if (state == 1)
    drawSingleEndBars();

  if (state == 2)
    drawMovingCircles();

  if (state == 3) {
    drawGrid();
    drawLine();
  }

  if (state == 4)
    drawCircle();

  if (state == 5) {
    drawGrid();
    updateArcs();
  }
}

void IRAM_ATTR buttonState() {
  tft.fillScreen(TFT_BLACK);
  detachInterrupt(digitalPinToInterrupt(interruptPin));
  if (millis() - last_button > 200) {
    buttonPressed = true;
    intrptEnable = false;
    state = (state + 1) % 6;
    // delay(75);
    last_button = millis();
  }
  attachInterrupt(digitalPinToInterrupt(interruptPin), buttonState, FALLING);
}

// Function to get distance from ultrasonic sensor
float getDistance() {
  // Trigger ultrasonic pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure the echo pulse duration
  duration = pulseIn(echoPin, HIGH);

  // Calculate distance in cm
  return duration * SOUND_SPEED / 2;
}

float applyFilter(float newMeasurement) {
  static bool bufferInitialized = false;

  if (!bufferInitialized) {
    // Pre-fill the buffer with the first valid measurement
    for (int i = 0; i < filterSize; i++) {
      distanceBuffer[i] = newMeasurement;
    }
    bufferInitialized = true;
  }

  // Update the buffer with the new measurement
  distanceBuffer[bufferIndex] = newMeasurement;
  bufferIndex = (bufferIndex + 1) % filterSize;

  // Calculate the average of the buffer
  float sum = 0;
  for (int i = 0; i < filterSize; i++) {
    sum += distanceBuffer[i];
  }

  return sum / filterSize;
}

// Function to draw a blip on the radar
void drawBlip(float distance, uint16_t y) {
  int screenWidth = tft.width();
  int screenHeight = tft.height();
  int centerX = screenWidth / 2;
  int bottomY = screenHeight;

  // Scale distance to screen height (assuming max distance 300 cm)
  int blipX = centerX - map(distance, 0, 320, 22, screenWidth - 22);
  int blipY = bottomY - map(y, 0, 500, 20, screenHeight - 18);

  if (blipX < 22)
    blipX = 22;

  if (blipY < 20)
    blipY = 20;

  // Blip blink logic
  unsigned long currentTime = millis();
  if (currentTime - lastBlinkTime >= blinkInterval) {
    lastBlinkTime = currentTime;
    blipVisible = !blipVisible;
  }

  // Draw the blip if visible
  if (blipVisible) {
    if (previousBlipY != blipY) {
      tft.fillCircle(blipX, previousBlipY, 5, TFT_BLACK);
    }
    if (previousBlipX != blipX) {
      tft.fillCircle(previousBlipX, blipY, 5, TFT_BLACK);
    }
    tft.fillCircle(blipX, blipY, 5, TFT_YELLOW);
  } else {
    tft.fillCircle(previousBlipX, previousBlipY, 5, TFT_BLACK);
    drawGrid();
  }
  previousBlipX = blipX;
  previousBlipY = blipY;
}

void drawArc(int x, int y, int r, int startAngle, int endAngle, uint16_t color) {
  int screenWidth = tft.width();
  int gridOffsetX = 20;

  for (int angle = startAngle; angle <= endAngle; angle++) {
    float rad = angle * DEG_TO_RAD;
    int xPos = x + r * cos(rad);
    int yPos = y + r * sin(rad);

    if (xPos >= gridOffsetX && xPos < screenWidth - gridOffsetX) {
      tft.drawPixel(xPos, yPos, color);
    }
  }
}

void updateArcs() {
  if (!intrptEnable) {
    attachInterrupt(digitalPinToInterrupt(interruptPin), buttonState, FALLING);
    intrptEnable = true;
  }
  float rawDistance = getDistance();
  distanceFiltered = applyFilter(rawDistance);

  uint16_t dist_y = analogRead(13);

  uint16_t y = map(dist_y, 0, 4095, 0, 1200);

  int16_t y_mapped = y - 600;

  uint16_t y_mod = (int16_t)y_mapped + 120;

  // Draw the blip if an object is detected
  if (distanceFiltered > 0 && distanceFiltered < 400) {  // Valid range for ultrasonic sensor
    drawBlip(distanceFiltered, y_mod);
  }

  unsigned long currentTime = millis();
  if (currentTime - lastRadarUpdate >= radarInterval) {
    lastRadarUpdate = currentTime;

    for (int i = 0; i < 3; i++) {
      if (buttonPressed) {
        tft.fillScreen(TFT_BLACK);
        break;
      }
      if (arcs[i].active) {
        drawArc(arcs[i].x, arcs[i].y, arcs[i].r, 180, 360, TFT_BLACK);
        drawArc((arcs[i].x) + 1, (arcs[i].y) + 1, arcs[i].r, 180, 360, TFT_BLACK);
        drawArc((arcs[i].x) + 2, (arcs[i].y) + 2, arcs[i].r, 180, 360, TFT_BLACK);
        drawGrid();

        arcs[i].y -= 2;  // Move upward
        arcs[i].r += 8;  // Expand

        drawArc(arcs[i].x, arcs[i].y, arcs[i].r, 180, 360, TFT_YELLOW);
        drawArc((arcs[i].x) + 1, (arcs[i].y) + 1, arcs[i].r, 180, 360, TFT_YELLOW);
        drawArc((arcs[i].x) + 2, (arcs[i].y) + 2, arcs[i].r, 180, 360, TFT_YELLOW);

        if (arcs[i].y - arcs[i].r < 20 || arcs[i].r > maxRadius) {
          drawArc(arcs[i].x, arcs[i].y, arcs[i].r, 180, 360, TFT_BLACK);
          drawArc((arcs[i].x) + 1, (arcs[i].y) + 1, arcs[i].r, 180, 360, TFT_BLACK);
          drawArc((arcs[i].x) + 2, (arcs[i].y) + 2, arcs[i].r, 180, 360, TFT_BLACK);
          arcs[i].active = false;
        }
      }
    }

    if (!arcs[0].active && arcs[1].y - arcs[1].r < (305 / 2 + 20) && !arcs[2].active) {
      arcs[2] = { 120, 305, 10, true };
    }
    if (!arcs[1].active && arcs[2].y - arcs[2].r < (305 / 2 + 20) && !arcs[0].active) {
      arcs[0] = { 120, 305, 10, true };
    }
    if (!arcs[2].active && arcs[0].y - arcs[0].r < (305 / 2 + 20) && !arcs[1].active) {
      arcs[1] = { 120, 305, 10, true };
    }
  }
}

void drawGrid() {
  int screenWidth = tft.width();
  int screenHeight = tft.height();

  int gridOffsetX = 20;  // Offset from the left and right edges
  int gridOffsetY = 20;  // Offset from the top and bottom edges

  for (int x = gridOffsetX; x < screenWidth - gridOffsetX; x += 40) {
    tft.drawLine(x, gridOffsetY, x, (screenHeight - gridOffsetY) - 1, (x == 20 || x == 220) ? TFT_ORANGE : TFT_WHITE);
  }

  //for (int y = gridOffsetY; y < screenHeight - gridOffsetY; y += 20) {
  for (int y = gridOffsetY; y < screenHeight - gridOffsetY; y += 35) {
    tft.drawLine(gridOffsetX, y, (screenWidth - gridOffsetX) - 1, y, (y == 20 || y == 300) ? TFT_ORANGE : TFT_WHITE);

    tft.drawRect(gridOffsetX, gridOffsetY, screenWidth - 2 * gridOffsetX, screenHeight - 2 * gridOffsetY, TFT_ORANGE);
    tft.drawRect(gridOffsetX - 1, gridOffsetY - 1, (screenWidth - 2 * gridOffsetX) + 2, (screenHeight - 2 * gridOffsetY) + 2, TFT_ORANGE);

    // Draw distance markers
    tft.setTextSize(1);
    tft.setCursor(0, gridOffsetY);
    tft.print("400");
    tft.setCursor(0, gridOffsetY + (screenHeight - 40) / 2);
    tft.print("200");
    tft.setCursor(0, screenHeight - gridOffsetY - 10);
    tft.print("  0");
    tft.setCursor(0, screenHeight - gridOffsetY);
    tft.print(" cm");

    tft.setCursor(screenWidth - 18, gridOffsetY);
    tft.print("157");
    tft.setCursor(screenWidth - 18, gridOffsetY + (screenHeight - 40) / 2);
    tft.print("79");
    tft.setCursor(screenWidth - 18, screenHeight - gridOffsetY - 10);
    tft.print(" 0");
    tft.setCursor(screenWidth - 18, screenHeight - gridOffsetY);
    tft.print("in");
  }
}

void drawBars() {
  if (!intrptEnable) {
    attachInterrupt(digitalPinToInterrupt(interruptPin), buttonState, FALLING);
    intrptEnable = true;
  }
  for (uint8_t i = 0; i < 7; i++) {
    if (buttonPressed) {
      tft.fillScreen(TFT_BLACK);
      break;
    }
    tft.fillRect(cornerBarX1, cornerBarY[i], 20, 40, ILI9341_GREEN);
    tft.fillRect(cornerBarX2, cornerBarY[i], 20, 40, ILI9341_GREEN);
    tft.fillRect(centreBarX1, cornerBarY[i], 20, 40, TFT_BLACK);
    tft.fillRect(centreBarX2, cornerBarY[i], 20, 40, TFT_BLACK);
    delay(del);
  }
  for (uint8_t i = 6; i > 0; i--) {
    if (buttonPressed) {
      buttonPressed = false;
      tft.fillScreen(TFT_BLACK);
      break;
    }
    tft.fillRect(centreBarX1, centreBarY[i], 20, 40, ILI9341_RED);
    tft.fillRect(centreBarX2, centreBarY[i], 20, 40, ILI9341_RED);
    tft.fillRect(cornerBarX1, cornerBarY[i], 20, 40, TFT_BLACK);
    tft.fillRect(cornerBarX2, cornerBarY[i], 20, 40, TFT_BLACK);
    delay(del);
  }
}

void drawSingleEndBars() {
  if (!intrptEnable) {
    attachInterrupt(digitalPinToInterrupt(interruptPin), buttonState, FALLING);
    intrptEnable = true;
  }
  for (uint8_t i = 0; i < 15; i++) {
    if (buttonPressed) {
      buttonPressed = false;
      tft.fillScreen(TFT_BLACK);
      break;
    }
    if (del < 5)
      delSingle = 4;
    else
      delSingle = del;

    tft.fillRect(cornerBarX1, cornerSingleEndY2[i], 20, 10, ILI9341_WHITE);
    delay(delSingle / 4);
    tft.fillRect(cornerBarX1, cornerSingleEndY2[i - 1], 20, 10, TFT_BLACK);
    tft.fillRect(cornerBarX2, cornerSingleEndY1[i], 20, 10, ILI9341_WHITE);
    delay(delSingle / 4);
    tft.fillRect(cornerBarX2, cornerSingleEndY1[i - 1], 20, 10, TFT_BLACK);
    tft.fillRect(centreBarX1, centreSingleEndY2[i], 20, 10, ILI9341_WHITE);
    delay(delSingle / 4);
    tft.fillRect(centreBarX1, centreSingleEndY2[i - 1], 20, 10, TFT_BLACK);
    tft.fillRect(centreBarX2, centreSingleEndY1[i], 20, 10, ILI9341_WHITE);
    delay(delSingle / 4);
    tft.fillRect(centreBarX2, centreSingleEndY1[i - 1], 20, 10, TFT_BLACK);
  }
}

void drawMovingCircles() {
  if (!intrptEnable) {
    attachInterrupt(digitalPinToInterrupt(interruptPin), buttonState, FALLING);
    intrptEnable = true;
  }
  for (uint8_t i = 0; i < 5; i++) {
    if (buttonPressed) {
      tft.fillScreen(TFT_BLACK);
      break;
    }
    tft.fillCircle(circleX[i], circleY[i], 10, ILI9341_GREEN);
    delay(del);
  }
  for (uint8_t i = 5; i < 14; i++) {
    if (buttonPressed) {
      tft.fillScreen(TFT_BLACK);
      break;
    }
    tft.fillCircle(circleX[i], circleY[i], 10, ILI9341_GREEN);
    delay(del);
    tft.fillCircle(circleX[i - 5], circleY[i - 5], 10, TFT_BLACK);
    tft.fillCircle(circleX[i - 4], circleY[i - 4], 10, TFT_BLACK);
  }
  for (uint8_t i = 9; i < 14; i++) {
    if (buttonPressed) {
      tft.fillScreen(TFT_BLACK);
      buttonPressed = false;
      break;
    }
    tft.fillCircle(circleX[i], circleY[i], 10, TFT_BLACK);
    if (i != 13)
      tft.fillCircle(circleX[i + 1], circleY[i + 1], 10, ILI9341_GREEN);
    delay(del);
  }
}

void drawLine() {
  if (!intrptEnable) {
    attachInterrupt(digitalPinToInterrupt(interruptPin), buttonState, FALLING);
    intrptEnable = true;
  }
  for (uint16_t i = 0; i < tft.width(); i++) {
    if (buttonPressed) {
      tft.fillScreen(TFT_BLACK);
      buttonPressed = false;
      break;
    }
    delay(del);
    if (i < 100) {
      tft.drawLine(lineX, lineY, lineX, lineY, ILI9341_GREEN);
      tft.drawLine(lineX, lineY2, lineX, lineY2, ILI9341_GREEN);
    }
    if (i >= 80 && i < 110) {
      // lineY--;
      // lineY2--;
      int8_t randomStep = (random(0, 2) * 2) - 1; // 0 -> -1, 1 -> +1
      lineY += randomStep;
      lineY2 += randomStep;
      tft.drawLine(lineX, lineY, lineX, lineY, ILI9341_GREEN);
      tft.drawLine(lineX, lineY2, lineX, lineY2, ILI9341_GREEN);
    }
    if (i >= 110 && i < 150) {
      int8_t randomStep = (random(0, 2) * 2) - 1; // 0 -> -1, 1 -> +1
      lineY += randomStep;
      lineY2 += randomStep;
      tft.drawLine(lineX, lineY, lineX, lineY, ILI9341_GREEN);
      tft.drawLine(lineX, lineY2, lineX, lineY2, ILI9341_GREEN);
    }
    if (i >= 150 && i < 170) {
      // lineY++;
      // lineY2++;
      int8_t randomStep = (random(0, 2) * 2) - 1; // 0 -> -1, 1 -> +1
      lineY += randomStep;
      lineY2 += randomStep;
      tft.drawLine(lineX, lineY, lineX, lineY, ILI9341_GREEN);
      tft.drawLine(lineX, lineY2, lineX, lineY2, ILI9341_GREEN);
    }
    if (i >= 170 && i < 190) {
      int8_t randomStep = (random(0, 2) * 2) - 1; // 0 -> -1, 1 -> +1
      lineY += randomStep;
      lineY2 += randomStep;
      tft.drawLine(lineX, lineY, lineX, lineY, ILI9341_GREEN);
      tft.drawLine(lineX, lineY2, lineX, lineY2, ILI9341_GREEN);
    }
    if (i >= 190 && i < 210) {
      int8_t randomStep = (random(0, 2) * 2) - 1; // 0 -> -1, 1 -> +1
      lineY += randomStep;
      lineY2 += randomStep;
      // lineY++;
      // lineY2++;
      tft.drawLine(lineX, lineY, lineX, lineY, ILI9341_GREEN);
      tft.drawLine(lineX, lineY2, lineX, lineY2, ILI9341_GREEN);
    }
    if (i >= 210) {
      tft.drawLine(lineX, lineY, lineX, lineY, ILI9341_GREEN);
      tft.drawLine(lineX, lineY2, lineX, lineY2, ILI9341_GREEN);
    }
    lineX++;
  }
  lineX = 0;
  lineY = 120;
  lineY2 = 121;
  tft.fillScreen(TFT_BLACK);
}

void drawCircle() {
  if (!intrptEnable) {
    attachInterrupt(digitalPinToInterrupt(interruptPin), buttonState, FALLING);
    intrptEnable = true;
  }
  // Draw top circles with random sizes
  for (int i = 0; i < 2; ++i) {
    if (buttonPressed) {
      tft.fillScreen(TFT_BLACK);
      buttonPressed = false;
      break;
    }
    drawRandomShape(topX[i], topY[i], ILI9341_GREEN);
    topCapSize = true;
    btmCapSize = false;
    clearPreviousShape(topX[i], topY[i], prevTopSize);  // Use the maximum size for clearing
    topClrSize = false;
    delay(del);
    drawRandomShape(bottomX[i], bottomY[i], ILI9341_GREEN);
    topCapSize = false;
    btmCapSize = true;
    clearPreviousShape(bottomX[i], bottomY[i], prevBtmSize);  // Use the maximum size for clearing
    btmClrSize = false;
  }
  // Draw top circles with random sizes
  for (int i = 0; i < 2; ++i) {
    if (buttonPressed) {
      tft.fillScreen(TFT_BLACK);
      buttonPressed = false;
      break;
    }
    drawRandomShape(topX[i], topY[i], ILI9341_RED);
    topCapSize = true;
    btmCapSize = false;
    clearPreviousShape(topX[i], topY[i], prevTopSize);  // Use the maximum size for clearing
    topClrSize = false;
    delay(del);
    drawRandomShape(bottomX[i], bottomY[i], ILI9341_RED);
    topCapSize = false;
    btmCapSize = true;
    clearPreviousShape(bottomX[i], bottomY[i], prevBtmSize);  // Use the maximum size for clearing
    btmClrSize = false;
  }
}

int getRandomSize() {
  int sizes[] = { 5, 10, 20, 40 };
  return sizes[random(4)];
}

void drawRandomShape(int x, int y, uint16_t color) {
  int size = getRandomSize();

  if (topCapSize) {
    btmClrSize = true;
    prevTopSize = size;
  }
  if (btmCapSize) {
    topClrSize = true;
    prevBtmSize = size;
  }

  tft.drawCircle(x, y, size, color);
}

void clearPreviousShape(int x, int y, int size) {
  tft.drawCircle(x, y, size, TFT_BLACK);
}