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

Adafruit_MPU6050 mpu;
WiFiClient wifiClient;
PubSubClient client(wifiClient);

const char* ssid = "Your_WiFi_SSID";
const char* password = "Your_WiFi_Password";

const char* org = "7q6x30";
const char* deviceType = "abcd";
const char* deviceId = "1234";
const char* token = "12345678";

const char* server = "7q6x30.messaging.internetofthings.ibmcloud.com";
const int port = 1883;
const char* publishTopic = "iot-2/evt/Data/fmt/json";
const char* clientId = "d:7q6x30:abcd:1234";

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

  Serial.println("Initializing MPU6050...");
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 sensor!");
    while (1);
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("MPU6050 initialization complete");

  connectToWiFi();
  connectToMQTT();
}

void loop() {
  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);

  Serial.print("Accelerometer (m/s^2): ");
  Serial.print(accel.acceleration.x);
  Serial.print(", ");
  Serial.print(accel.acceleration.y);
  Serial.print(", ");
  Serial.println(accel.acceleration.z);

  Serial.print("Gyroscope (deg/s): ");
  Serial.print(gyro.gyro.x);
  Serial.print(", ");
  Serial.print(gyro.gyro.y);
  Serial.print(", ");
  Serial.println(gyro.gyro.z);

  Serial.print("Temperature (°C): ");
  Serial.println(temp.temperature);

  publishData(accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
              gyro.gyro.x, gyro.gyro.y, gyro.gyro.z,
              temp.temperature);

  delay(500);
}

void connectToWiFi() {
  Serial.println();
  Serial.print("Connecting to WiFi...");

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

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

void connectToMQTT() {
  client.setServer(server, port);

  Serial.println();
  Serial.print("Connecting to MQTT broker...");

  while (!client.connected()) {
    if (client.connect(clientId, "use-token-auth", token)) {
      Serial.println();
      Serial.println("Connected to MQTT broker");
    } else {
      Serial.print(".");
      delay(500);
    }
  }
}

void publishData(float accelX, float accelY, float accelZ, float gyroX, float gyroY, float gyroZ, float temperature) {
  const int bufferSize = JSON_OBJECT_SIZE(7);
  StaticJsonDocument<bufferSize> jsonDoc;

  jsonDoc["AccelerometerX"] = accelX;
  jsonDoc["AccelerometerY"] = accelY;
  jsonDoc["AccelerometerZ"] = accelZ;
  jsonDoc["GyroscopeX"] = gyroX;
  jsonDoc["GyroscopeY"] = gyroY;
  jsonDoc["GyroscopeZ"] = gyroZ;
  jsonDoc["Temperature"] = temperature;

  char payload[256];
  serializeJson(jsonDoc, payload, sizeof(payload));

  if (client.connected()) {
    Serial.println();
    Serial.print("Publishing data to MQTT broker... ");
    Serial.println(payload);

    client.publish(publishTopic, payload);
  }
}