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

// Dummy sensor data
struct SensorData {
  float ax, ay, az; // Accelerometer data
  float gx, gy, gz; // Gyroscope data
};

SensorData sensorData;

float accelWeight = 0.98;
float gyroWeight = 0.02;
float elapsedTime, currentTime, previousTime;
float pitch, roll;

float kalmanGain;
float pitchEstimate;
float pitchVariance = 0.1;  // Adjust as needed for your specific scenario

#define AIO_USERNAME    "username"
#define AIO_KEY         "key"

double speed;  // Declare speed as a global variable

WiFiClient wifiClient;
Adafruit_MQTT_Client mqtt(&wifiClient, "io.adafruit.com", 1883, AIO_USERNAME, AIO_KEY);

Adafruit_MQTT_Publish latitudeFeed = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/latitude");
Adafruit_MQTT_Publish longitudeFeed = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/longitude");
Adafruit_MQTT_Publish horizontalSpeedFeed = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/horizontal_speed");
Adafruit_MQTT_Publish verticalSpeedFeed = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/vertical_speed");
Adafruit_MQTT_Publish speedFeed = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/speed");

void MQTT_connect() {
  int8_t ret;
  while ((ret = mqtt.connect()) != 0) {
    Serial.println(mqtt.connectErrorString(ret));
    Serial.println("Retrying MQTT connection in 5 seconds...");
    delay(5000);
  }
  Serial.println("MQTT Connected!");
}

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // Dummy initialization (no real sensor)

  // Connect to Wi-Fi
  WiFi.begin("Wokwi-GUEST", "");
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }
  Serial.println("Connected to WiFi");

  // Adafruit MQTT Setup
  MQTT_connect();
}

void loop() {
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000.0;

  // Dummy sensor data (replace with actual dummy data as needed)
  updateDummySensorData();

  // Print raw sensor data
  printRawSensorData();

  // Calculate pitch and roll using complementary filter
  computeIMU();

  // Apply Kalman filter to pitch
  kalmanFilter();

  // Dummy GPS data (replace with actual dummy data as needed)
  double latitude = 37.7749;
  double longitude = -122.4194;
  // Generate random speed between 10.8 km/h and 55 km/h
  speed = random(108, 550) / 10.0;

  double horizontalSpeed = speed * cos(pitchEstimate);
  double verticalSpeed = speed * sin(pitchEstimate);

  // Print or process the data as needed
  printSensorData(latitude, longitude, horizontalSpeed, verticalSpeed);

  // Publish data to MQTT
  if (!publishToMQTT(latitude, longitude, horizontalSpeed, verticalSpeed)) {
    Serial.println("Failed to publish data to MQTT");
  }

  // Ensure MQTT connection and handle subscriptions
  if (!mqtt.connected()) {
    MQTT_connect();
  }
  mqtt.processPackets(10000);
  mqtt.ping();

  previousTime = currentTime;
}

void updateDummySensorData() {
  // Increment dummy sensor data (replace with actual logic as needed)
  sensorData.ax += 1;
  sensorData.ay += 2;
  sensorData.az += 3;
  sensorData.gx += 0.1;
  sensorData.gy += 0.2;
  sensorData.gz += 0.3;
}

void computeIMU() {
  float accX = sensorData.ax;
  float accY = sensorData.ay;
  float accZ = sensorData.az;
  float gyroX = sensorData.gx;
  float gyroY = sensorData.gy;
  float gyroZ = sensorData.gz;

  roll = accelWeight * (roll + gyroX * elapsedTime) + gyroWeight * atan2(-accY, accZ) * RAD_TO_DEG;
  pitch = accelWeight * (pitch + gyroY * elapsedTime) + gyroWeight * atan2(accX, sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
}

void kalmanFilter() {
  // Kalman filter update for pitch
  kalmanGain = pitchVariance / (pitchVariance + sensorData.ax * sensorData.ax);
  pitchEstimate = pitch + kalmanGain * (sensorData.ax - pitch);
  pitchVariance = (1 - kalmanGain) * pitchVariance;
}

bool publishToMQTT(double latitude, double longitude, double horizontalSpeed, double verticalSpeed) {
  if (!latitudeFeed.publish(latitude)) {
    Serial.println("Failed to publish latitude");
    return false;
  }
  if (!longitudeFeed.publish(longitude)) {
    Serial.println("Failed to publish longitude");
    return false;
  }
  if (!horizontalSpeedFeed.publish(horizontalSpeed)) {
    Serial.println("Failed to publish horizontal speed");
    return false;
  }
  if (!verticalSpeedFeed.publish(verticalSpeed)) {
    Serial.println("Failed to publish vertical speed");
    return false;
  }
  if (!speedFeed.publish(speed)) {
    Serial.println("Failed to publish speed");
    return false;
  }
  return true;
}

void printRawSensorData() {
  Serial.print("Raw Accelerometer Data - X: "); Serial.print(sensorData.ax); Serial.print(", Y: "); Serial.print(sensorData.ay); Serial.print(", Z: "); Serial.println(sensorData.az);
  Serial.print("Raw Gyroscope Data - X: "); Serial.print(sensorData.gx); Serial.print(", Y: "); Serial.print(sensorData.gy); Serial.print(", Z: "); Serial.println(sensorData.gz);
}

void printSensorData(double latitude, double longitude, double horizontalSpeed, double verticalSpeed) {
  Serial.print("Latitude: "); Serial.println(latitude, 7);
  Serial.print("Longitude: "); Serial.println(longitude, 7);
  Serial.print("Horizontal Speed: "); Serial.println(horizontalSpeed, 2);
  Serial.print("Vertical Speed: "); Serial.println(verticalSpeed, 2);
  // Assuming this is the speed you want to print
  Serial.print("Speed: "); Serial.println(speed, 2);
}