#include <WiFi.h>
#include <TinyGPS++.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <math.h>

// ------------ CONFIG -----------------
#define GPS_BAUDRATE 9600

// WiFi credentials
const char* WIFI_SSID     = "Wokwi-GUEST";
const char* WIFI_PASSWORD = "";


// ThingSpeak
const char* THINGSPEAK_HOST = "api.thingspeak.com";
const char* THINGSPEAK_API_KEY = "1PF2TGAO8KOUL1WZ";  // put your Write API key here

// ThingSpeak minimum interval (free account): 15s
const unsigned long THINGSPEAK_INTERVAL_MS = 20000; // 20s to be safe

// -------------------------------------

TinyGPSPlus gps;
Adafruit_MPU6050 mpu;

WiFiClient client;

unsigned long lastThingSpeakUpdate = 0;
unsigned long lastPrint = 0;

// Latest sensor values
double gpsLat = 0.0;
double gpsLon = 0.0;
double gpsSpeedKmph = 0.0;
bool gpsHasFix = false;

float accX = 0.0f, accY = 0.0f, accZ = 0.0f;
float gyroX = 0.0f, gyroY = 0.0f, gyroZ = 0.0f;
float accMag = 0.0f;

void connectWiFi() {
  Serial.print("Connecting to WiFi: ");
  Serial.println(WIFI_SSID);

  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);

  int retries = 0;
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
    retries++;
    if (retries > 60) { // 30s timeout
      Serial.println("\nWiFi connection failed.");
      return;
    }
  }

  Serial.println("\nWiFi connected.");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
}

void setupMPU() {
  Wire.begin(); // SDA=21, SCL=22 on ESP32 by default

  if (!mpu.begin()) {
    Serial.println("ERROR: Failed to find MPU6050!");
    while (1) {
      delay(1000);
    }
  }

  // Configure ranges and filter
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  Serial.println("MPU6050 initialized.");
}

void setupGPS() {
  // Serial2: RX=16, TX=17 for ESP32
  Serial2.begin(GPS_BAUDRATE, SERIAL_8N1, 16, 17);
  Serial.println("GPS (TinyGPS++) initialized on Serial2 (RX=16, TX=17).");
}

void setup() {
  Serial.begin(9600);
  delay(1000);
  Serial.println("ESP32 - GPS + MPU6050 + ThingSpeak");

  setupMPU();
  setupGPS();
  connectWiFi();
}

// Read GPS bytes and update TinyGPS++
void updateGPS() {
  while (Serial2.available() > 0) {
    char c = Serial2.read();
    if (gps.encode(c)) {
      // New sentence fully parsed -> update latest values
      if (gps.location.isValid()) {
        gpsLat = gps.location.lat();
        gpsLon = gps.location.lng();
        gpsHasFix = true;
      } else {
        gpsHasFix = false;
      }

      if (gps.speed.isValid()) {
        gpsSpeedKmph = gps.speed.kmph();
      } else {
        gpsSpeedKmph = 0.0;
      }
    }
  }

  // Check if no GPS chars at all (wiring or sim problem)
  if (millis() > 5000 && gps.charsProcessed() < 10) {
    Serial.println("No GPS data received: check wiring or simulation input.");
  }
}

// Read IMU (MPU6050) and compute acceleration magnitude
void updateIMU() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  accX = a.acceleration.x;
  accY = a.acceleration.y;
  accZ = a.acceleration.z;

  gyroX = g.gyro.x;
  gyroY = g.gyro.y;
  gyroZ = g.gyro.z;

  accMag = sqrtf(accX * accX + accY * accY + accZ * accZ);
}

// Print sensors to Serial for debug
void printSensors() {
  Serial.println("========== SENSOR DATA ==========");

  if (gpsHasFix) {
    Serial.print("Latitude : "); Serial.println(gpsLat, 6);
    Serial.print("Longitude: "); Serial.println(gpsLon, 6);
  } else {
    Serial.println("GPS Location: INVALID");
  }

  Serial.print("GPS Speed: ");
  Serial.print(gpsSpeedKmph);
  Serial.println(" km/h");

  Serial.print("GPS Date&Time: ");
  if (gps.date.isValid() && gps.time.isValid()) {
    Serial.printf("%04d-%02d-%02d %02d:%02d:%02d\n",
      gps.date.year(),
      gps.date.month(),
      gps.date.day(),
      gps.time.hour(),
      gps.time.minute(),
      gps.time.second()
    );
  } else {
    Serial.println("INVALID");
  }

  Serial.println("---- IMU (MPU6050) ----");
  Serial.printf("Accel (m/s^2): X=%.2f  Y=%.2f  Z=%.2f\n", accX, accY, accZ);
  Serial.printf("Gyro  (deg/s): X=%.2f  Y=%.2f  Z=%.2f\n", gyroX, gyroY, gyroZ);
  Serial.printf("Accel Magnitude: %.2f m/s^2\n", accMag);

  Serial.println("=================================\n");
}

// Send data to ThingSpeak
void sendToThingSpeak() {
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("WiFi not connected, skipping ThingSpeak update.");
    return;
  }

  if (!client.connect(THINGSPEAK_HOST, 80)) {
    Serial.println("Connection to ThingSpeak failed.");
    return;
  }

  // Map your data to fields:
  // field1: latitude
  // field2: longitude
  // field3: GPS speed km/h
  // field4: accel magnitude
  // field5: accel X
  // field6: accel Y
  // field7: accel Z
  // field8: gyro Z (for example)

  String url = "/update?api_key=" + String(THINGSPEAK_API_KEY);

  if (gpsHasFix) {
    url += "&field1=" + String(gpsLat, 6);
    url += "&field2=" + String(gpsLon, 6);
  }

  url += "&field3=" + String(gpsSpeedKmph, 2);
  url += "&field4=" + String(accMag, 2);
  url += "&field5=" + String(accX, 2);
  url += "&field6=" + String(accY, 2);
  url += "&field7=" + String(accZ, 2);
  url += "&field8=" + String(gyroZ, 2);

  Serial.println("Sending to ThingSpeak:");
  Serial.println(url);

  client.print(String("GET ") + url + " HTTP/1.1\r\n" +
               "Host: " + THINGSPEAK_HOST + "\r\n" +
               "Connection: close\r\n\r\n");

  // Just read and ignore response
  unsigned long timeout = millis();
  while (client.connected() && millis() - timeout < 2000) {
    while (client.available()) {
      client.read(); // discard
    }
  }

  client.stop();
  Serial.println("ThingSpeak update complete.\n");
}

void loop() {
  // Continuously update sensors
  updateGPS();
  updateIMU();

  unsigned long now = millis();

  // Print to Serial every 2 seconds for debugging
  if (now - lastPrint >= 2000) {
    lastPrint = now;
    printSensors();
  }

  // Send to ThingSpeak every THINGSPEAK_INTERVAL_MS
  if (now - lastThingSpeakUpdate >= THINGSPEAK_INTERVAL_MS) {
    lastThingSpeakUpdate = now;
    sendToThingSpeak();
  }
}