//libraries ko
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <ESP32Servo.h>
#include <NewPing.h>

//Display Definitions
#define TFT_MISO  25
#define TFT_LED 5
#define TFT_SCK 19
#define TFT_MOSI  23
#define TFT_DC  21
#define TFT_RESET 18
#define TFT_CS  22
//Sensor Definitions
const int TRIGGER_PIN = 32;  // Use appropriate pins for the ESP32
const int ECHO_PIN = 33; // Use appropriate pins for the ESP32
const int MAX_DISTANCE = 200; // Maximum distance we want to ping for (in centimeters)

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCK, TFT_RESET, TFT_MISO);
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

// Variables for the timing
unsigned long previousMillisServo = 0;  // stores last time the servo was moved
unsigned long previousMillisSensor = 0;  // stores last time the HC-SR04 was activated
const long intervalServo = 100;          // interval at which to move servo (milliseconds)
const long intervalSensor = 150;         // interval at which to ping sensor (milliseconds)

Servo myServo;  // create servo object to control a servo
int servoPin = 15;
int servoPos = 0;

void setup() {
   
  //for the ultrasonic sprikitik
  Serial.begin(9600);
  
  //servo
  myServo.attach(servoPin);
  
  //turns backlight lol
  pinMode(TFT_LED, OUTPUT);
  digitalWrite(TFT_LED, HIGH);
  
  //Initializes display
  tft.begin();  // Initialize the display
  tft.setRotation(3); // Set orientation
  tft.fillScreen(ILI9341_BLACK); // Clear the screen
  pinMode(TFT_LED, OUTPUT); // Set TFT_LED as output if connected
  digitalWrite(TFT_LED, HIGH); // Turn on the backlight if connected
  tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK); // Set the font color and background color
  tft.setTextSize(2); // Set the text size
}

void loop() {
  unsigned long currentMillis = millis();

  // Move the servo every 100ms
  if (currentMillis - previousMillisServo >= intervalServo) {
    previousMillisServo = currentMillis;
    
    // Increment the servo position by 1 degree
    servoPos++;
    if (servoPos > 180) {
      servoPos = 0;  // reset position if it goes over 180 degrees
    }
    myServo.write(servoPos);  // move servo to the new position
  }

  // Ping the HC-SR04 every 150ms using NewPing library
  if (currentMillis - previousMillisSensor >= intervalSensor) {
    previousMillisSensor = currentMillis;
    
    // Use the NewPing library to ping the sensor
    unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
    unsigned int distance = uS / US_ROUNDTRIP_CM; // Convert time into distance
    
    // Print the distance to the Serial Monitor
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    // Update the distance on the ILI9341 TFT display without clearing the screen
    tft.fillRect(0, 0, 240, 40, ILI9341_BLACK); // Clear the previous distance value
    tft.setCursor(0, 0); // Set the cursor position
    tft.print("Distance: ");
    tft.print(distance);
    tft.println(" cm");
  }
}