#include <WiFi.h>
#include <Adafruit_MQTT.h>
#include <Adafruit_MQTT_Client.h>

// Replace with your Wi-Fi credentials
const char* ssid = "Wokwi-GUEST";
const char* password = "";

// Adafruit IO setup
#define AIO_SERVER      "io.adafruit.com"
#define AIO_SERVERPORT  1883
#define AIO_USERNAME    "your_AIO_username"
#define AIO_KEY         "your_AIO_key"

// Define MQTT topics
WiFiClient client;
Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT, AIO_USERNAME, AIO_KEY);

Adafruit_MQTT_Publish locationPublish = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/location");
Adafruit_MQTT_Publish gyroPublish = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/gyro");
Adafruit_MQTT_Publish speedPublish = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/speed");

// Initial GPS coordinates
float latitude = -7.780846;
float longitude = 110.383479;

// Dummy gyro data
float gyroX = 0.5;  
float gyroY = 0.3;
float gyroZ = -0.2;

// Speed variables
float speed = 10.0; 

// Timing variables
unsigned long previousUpdateTime = 0;
const unsigned long updateInterval = 3000; 

// Turning variables
float totalDistance = 0.0;
float turnThreshold = random(50.0, 125.0);
int consecutiveTurns = 0;
bool turnRight = true;  

void connectToWiFi() {
  Serial.print("Connecting to WiFi");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.print(".");
  }
  Serial.println("\nConnected to WiFi");
}

void connectToAdafruitIO() {
  int8_t retries = 3;
  while ((mqtt.connected() == false) && (--retries > 0)) {
    Serial.print(".");
    if (mqtt.connect()) {
      Serial.println("\nConnected to Adafruit IO");
    } else {
      Serial.println("\nConnection to Adafruit IO failed. Retrying...");
      delay(500);
    }
  }
  if (!mqtt.connected()) {
    Serial.println("Unable to connect to Adafruit IO. Please check your AIO settings.");
    while (1);
  }
}

void updateLocation() {
  float movement = speed * 3;
  latitude += 0.00001 * movement;
  longitude += 0.00001 * movement;
  totalDistance += movement;

  if (totalDistance > turnThreshold) {
    totalDistance = 0.0;
    determineNextTurn();
  }

  char locationPayload[64];
  snprintf(locationPayload, sizeof(locationPayload), "{\"lat\": %.6f, \"lon\": %.6f}", latitude, longitude);

  if (locationPublish.publish(locationPayload)) {
    Serial.println("Location data published to Adafruit IO");
  } else {
    Serial.println("Failed to publish location data");
  }
}

void determineNextTurn() {
  consecutiveTurns = random(1, 4);

  for (int i = 0; i < consecutiveTurns; i++) {
    if (turnRight) {
      latitude += random(0.0004, 0.0015);
      Serial.println("Turning right");
    } else {
      longitude += random(0.0004, 0.0015);
      Serial.println("Turning left");
    }
    turnRight = !turnRight;
  }
}

void updateGyro() {
  gyroX += 0.1;
  gyroY -= 0.05;
  gyroZ += 0.2;

  char gyroPayload[64];
  snprintf(gyroPayload, sizeof(gyroPayload), "{\"x\": %.2f, \"y\": %.2f, \"z\": %.2f}", gyroX, gyroY, gyroZ);

  if (gyroPublish.publish(gyroPayload)) {
    Serial.println("Gyro data published to Adafruit IO");
  } else {
    Serial.println("Failed to publish gyro data");
  }
}

void updateSpeed() {
  speed = random(10, 66);

  char speedPayload[16];
  snprintf(speedPayload, sizeof(speedPayload), "%.2f", speed);

  if (speedPublish.publish(speedPayload)) {
    Serial.println("Speed data published to Adafruit IO");
  } else {
    Serial.println("Failed to publish speed data");
  }
}

void setup() {
  Serial.begin(115200);
  connectToWiFi();
}

void loop() {
  connectToAdafruitIO();

  unsigned long currentMillis = millis();

  if (currentMillis - previousUpdateTime >= updateInterval) {
    updateLocation();
    updateGyro();
    updateSpeed();
    previousUpdateTime = currentMillis;
  }
}