#include <WiFi.h>
#include <TinyGPS++.h>
#include <ESP32Servo.h>

// WiFi credentials
const char* ssid = "TP-Link_73AE";
const char* password = "45092121";

// GPS setup
TinyGPSPlus gps;
HardwareSerial gpsSerial(1); // Use UART1

// Servo setup
Servo myservo;
int servoPin = 12;

// Motor setup (using H-Bridge)
int motorPin1 = 27;
int motorPin2 = 26;
int speedPin = 25; // PWM pin for speed control

void setup() {
    Serial.begin(115200);
    gpsSerial.begin(9600, SERIAL_8N1, 16, 17); // RX, TX

    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
        delay(1000);
        Serial.println("Connecting to WiFi...");
    }
    Serial.println("Connected to WiFi");

    myservo.attach(servoPin);
    pinMode(motorPin1, OUTPUT);
    pinMode(motorPin2, OUTPUT);
    pinMode(speedPin, OUTPUT);
}

void loop() {
    while (gpsSerial.available() > 0) {
        if (gps.encode(gpsSerial.read())) {
            double latitude = gps.location.lat();
            double longitude = gps.location.lng();
            Serial.print("Latitude: "); Serial.println(latitude, 6);
            Serial.print("Longitude: "); Serial.println(longitude, 6);

            // Implement navigation logic here
            navigateTo(latitude, longitude);
        }
    }

    // Add code to receive new coordinates from the server
}

void navigateTo(double targetLat, double targetLng) {
    // Placeholder logic to navigate to target coordinates
    // You would compare current GPS coordinates with target and adjust motor/servo accordingly

    // Example: Move forward
    digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);
    analogWrite(speedPin, 255); // Full speed
    myservo.write(30); // Straight
}