#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <WiFi.h>
#include <HTTPClient.h>

// WiFi credentials
const char* ssid = "Wokwi-GUEST";
const char* password = "";

// Google Apps Script URL
const char* scriptURL = "https://script.google.com/macros/s/AKfycbzhkYPVXR5_wmXTmQKiB6Fuh2C_incxCRq6J4J6mqvOHzO3TwukDhAmQOov5jmnQ8BKUQ/exec";

// Create MPU6050 objects for five sensors
Adafruit_MPU6050 mpu1, mpu2, mpu3, mpu4, mpu5;

// Define AD0 control pins for dynamically switching I2C addresses
#define AD0_MPU3  15  
#define AD0_MPU4  2   
#define AD0_MPU5  4   

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22); // SDA on GPIO 21, SCL on GPIO 22

  // Initialize MPU1 (0x68) and MPU2 (0x69)
  if (!mpu1.begin(0x68)) {
    Serial.println("MPU6050 #1 not found!");
    while (1);
  }
  Serial.println("MPU6050 #1 initialized.");

  if (!mpu2.begin(0x69)) {
    Serial.println("MPU6050 #2 not found!");
    while (1);
  }
  Serial.println("MPU6050 #2 initialized.");

  // Initialize AD0 pins for MPU3, MPU4, and MPU5
  pinMode(AD0_MPU3, OUTPUT);
  pinMode(AD0_MPU4, OUTPUT);
  pinMode(AD0_MPU5, OUTPUT);

  digitalWrite(AD0_MPU3, HIGH);
  digitalWrite(AD0_MPU4, HIGH);
  digitalWrite(AD0_MPU5, HIGH);
  delay(100);

  // Initialize the additional sensors one by one
  digitalWrite(AD0_MPU3, LOW);
  if (!mpu3.begin(0x68)) {
    Serial.println("MPU6050 #3 not found!");
    while (1);
  }
  Serial.println("MPU6050 #3 initialized.");
  digitalWrite(AD0_MPU3, HIGH);

  digitalWrite(AD0_MPU4, LOW);
  if (!mpu4.begin(0x68)) {
    Serial.println("MPU6050 #4 not found!");
    while (1);
  }
  Serial.println("MPU6050 #4 initialized.");
  digitalWrite(AD0_MPU4, HIGH);

  digitalWrite(AD0_MPU5, LOW);
  if (!mpu5.begin(0x68)) {
    Serial.println("MPU6050 #5 not found!");
    while (1);
  }
  Serial.println("MPU6050 #5 initialized.");
  digitalWrite(AD0_MPU5, HIGH);

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

void loop() {
  sensors_event_t accel1, gyro1, temp1;
  sensors_event_t accel2, gyro2, temp2;
  sensors_event_t accel3, gyro3, temp3;
  sensors_event_t accel4, gyro4, temp4;
  sensors_event_t accel5, gyro5, temp5;

  // Read from MPU1 and MPU2
  mpu1.getEvent(&accel1, &gyro1, &temp1);
  mpu2.getEvent(&accel2, &gyro2, &temp2);

  // Read MPU3 (toggle AD0)
  digitalWrite(AD0_MPU3, LOW);
  delay(10);
  mpu3.getEvent(&accel3, &gyro3, &temp3);
  digitalWrite(AD0_MPU3, HIGH);

  // Read MPU4 (toggle AD0)
  digitalWrite(AD0_MPU4, LOW);
  delay(10);
  mpu4.getEvent(&accel4, &gyro4, &temp4);
  digitalWrite(AD0_MPU4, HIGH);

  // Read MPU5 (toggle AD0)
  digitalWrite(AD0_MPU5, LOW);
  delay(10);
  mpu5.getEvent(&accel5, &gyro5, &temp5);
  digitalWrite(AD0_MPU5, HIGH);

  // Prepare data for Google Sheets
  String postData = 
      "mpu1_accel_x=" + String(accel1.acceleration.x) +
      "&mpu1_accel_y=" + String(accel1.acceleration.y) +
      "&mpu1_accel_z=" + String(accel1.acceleration.z) +
      "&mpu1_gyro_x=" + String(gyro1.gyro.x) +
      "&mpu1_gyro_y=" + String(gyro1.gyro.y) +
      "&mpu1_gyro_z=" + String(gyro1.gyro.z) +
      "&mpu2_accel_x=" + String(accel2.acceleration.x) +
      "&mpu2_accel_y=" + String(accel2.acceleration.y) +
      "&mpu2_accel_z=" + String(accel2.acceleration.z) +
      "&mpu2_gyro_x=" + String(gyro2.gyro.x) +
      "&mpu2_gyro_y=" + String(gyro2.gyro.y) +
      "&mpu2_gyro_z=" + String(gyro2.gyro.z) +
      "&mpu3_accel_x=" + String(accel3.acceleration.x) +
      "&mpu3_accel_y=" + String(accel3.acceleration.y) +
      "&mpu3_accel_z=" + String(accel3.acceleration.z) +
      "&mpu3_gyro_x=" + String(gyro3.gyro.x) +
      "&mpu3_gyro_y=" + String(gyro3.gyro.y) +
      "&mpu3_gyro_z=" + String(gyro3.gyro.z) +
      "&mpu4_accel_x=" + String(accel4.acceleration.x) +
      "&mpu4_accel_y=" + String(accel4.acceleration.y) +
      "&mpu4_accel_z=" + String(accel4.acceleration.z) +
      "&mpu4_gyro_x=" + String(gyro4.gyro.x) +
      "&mpu4_gyro_y=" + String(gyro4.gyro.y) +
      "&mpu4_gyro_z=" + String(gyro4.gyro.z) +
      "&mpu5_accel_x=" + String(accel5.acceleration.x) +
      "&mpu5_accel_y=" + String(accel5.acceleration.y) +
      "&mpu5_accel_z=" + String(accel5.acceleration.z) +
      "&mpu5_gyro_x=" + String(gyro5.gyro.x) +
      "&mpu5_gyro_y=" + String(gyro5.gyro.y) +
      "&mpu5_gyro_z=" + String(gyro5.gyro.z);

  // Send data to Google Sheets
  if (WiFi.status() == WL_CONNECTED) {
    HTTPClient http;
    http.begin(scriptURL);
    http.addHeader("Content-Type", "application/x-www-form-urlencoded");
    int httpResponseCode = http.POST(postData);

    if (httpResponseCode > 0) {
      Serial.println("Data sent successfully!");
    } else {
      Serial.println("Error sending data: " + String(httpResponseCode));
    }
    http.end();
  }

  delay(100);
}