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

#define TFT_CS 10
#define TFT_RST 9
#define TFT_DC 8

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

const int trigPin = 7;
const int echoPin = 6;

unsigned long previousMillis = 0;
const long interval = 50; // 0.25 seconds
int displayx = 50; // Initialize displayx
int previousDepth = 0; // Variable to store previous depth
bool isFish = false; // Flag to indicate if a fish is detected

void setup() {
  Serial.begin(9600);
  tft.begin();
  tft.setRotation(3); // Rotate display if needed

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

void loop() {
  // Measure distance and draw pixel based on distance
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  float duration = pulseIn(echoPin, HIGH);
  float distance = (duration * 0.0343) / 2;

  int displayy = map(distance, 0, 400, tft.height(), 0); // Invert Y-axis direction
  int invertedDisplayy = tft.height() - displayy; // Invert displayy value
  int color = map(invertedDisplayy, 0, tft.height(), ILI9341_RED, ILI9341_BLUE);

  // Draw depth lines and numbers
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  for (int depth = 1; depth <= 3; depth++) {
    int yPos = map((4 - depth) * 100, 0, 400, tft.height(), 0);
    tft.setCursor(5, yPos - 10);
    tft.print(depth);
    tft.drawFastHLine(0, yPos, tft.width(), ILI9341_WHITE);
  }

  // Check if it's time to move the x-axis forward
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    // Save the current time for the next iteration
    previousMillis = currentMillis;

    // Move the x-axis forward
    displayx++;
    if (displayx >= tft.width()) {
      displayx = 0; // Reset x position when it reaches the end

      // Clear the screen
      tft.fillScreen(ILI9341_BLACK);
    }
  }

  // Check if a fish is detected
  if (abs(previousDepth - distance) > 20) {
    isFish = true;
  } else {
    isFish = false;
  }

  // Draw fish if detected, otherwise draw pillar
  if (isFish) {
    // Draw fish
    tft.fillCircle(displayx, invertedDisplayy, 5, ILI9341_GREEN); // Draw a green circle as a fish
  } else if (distance > 0) {
    // Draw pillar if not at the bottom
    tft.drawFastVLine(displayx, invertedDisplayy, tft.height() - invertedDisplayy, color);
  }

  // Store current depth for comparison in the next iteration
  previousDepth = distance;
}