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


Adafruit_MPU6050 mpu;
sensors_event_t acc, gcc, temp;

float AcX, AcY,AcZ, GoX, GoY, GoZ;

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

//MQTT Broker Config
const char *mqtt_broker = "broker.emqx.io";
const char *topic = "esp/test";
const char *mqtt_username = "esp32";
const char *mqtt_password = "symbotic";
const int   mqtt_port = 1883;

WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient);

// Init MPU6050
void initMPU(){
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");
}

// Initialize WiFi
void initWiFi() {
  Serial.println("Initializing WiFi...");
  WiFi.mode(WIFI_STA);  
  Serial.print("Connecting to WiFi: ");
  Serial.println(ssid);
  WiFi.begin(ssid, password, 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
}

void mqttConnect() 
{
  mqttClient.setServer(mqtt_broker, mqtt_port);
    while (!mqttClient.connected()) 
    {
      String client_id = "esp32-client";
      client_id += String(WiFi.macAddress());
      Serial.printf("The client %s connects to the public MQTT broker\n", client_id.c_str());
      if (mqttClient.connect(client_id.c_str(), mqtt_username, mqtt_password))
      {
          Serial.println("Public EMQX MQTT broker connected");
      } 
      else 
      {
          Serial.print("failed with state ");
          Serial.println(mqttClient.state());
          delay(5000);
      }
    }
}

void setup(void) {
  Serial.begin(115200);
  initWiFi();
  initMPU();
}


void loop(){

  if(!mqttClient.connected()) 
  {
    mqttConnect();
  }
  
  mqttClient.loop();
  mpu.getEvent(&acc,&gcc,&temp);
  AcX = acc.acceleration.x;
  AcY = acc.acceleration.y;
  AcZ = acc.acceleration.z;
  GoX = gcc.gyro.x*180/3.14;
  GoY = gcc.gyro.y*180/3.14;
  GoZ = gcc.gyro.z*180/3.14;

  //Serial Monitor
  String space = "           ";
  Serial.println("Accel X-Axis, Accel Y-Axis, Accel Z-Axis");
  Serial.println(AcX + space + AcY + space + AcZ);
  delay(2000);
  Serial.println("Gyro X-Axis, Gyro Y-Axis, Gyro Z-Axis");
  Serial.println(GoX + space + GoY + space + GoZ);
  delay(2000);

  //MQTT Server 
  mqttClient.publish(topic, String(AcX).c_str(),true);
  mqttClient.publish(topic, String(AcY).c_str(),true);
  mqttClient.publish(topic, String(AcZ).c_str(),true);
  mqttClient.publish(topic, String(GoX).c_str(),true);
  mqttClient.publish(topic, String(GoY).c_str(),true);
  mqttClient.publish(topic, String(GoZ).c_str(),true);
  delay(1000);
}