#include <Wire.h>
#include <WiFi.h>
#include <PubSubClient.h>  // MQTT library
#include <MPU6050.h>

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

// MQTT Broker (Public Mosquitto)
const char* mqtt_server = "test.mosquitto.org";
const char* mqtt_topic = "wokwi/sensor_data";
const char* mqtt_client_id = "esp32_wokwi";

WiFiClient espClient;
PubSubClient mqttClient(espClient);

// MPU6050 setup
MPU6050 mpu;
int16_t ax, ay, az;
float ax_g, ay_g, az_g;
float ax_ms2, ay_ms2, az_ms2;

// Displacement variables
unsigned long lastTime = 0;
float velocity_x = 0, velocity_y = 0;
float displacement_x = 0, displacement_y = 0;
float vibrationX = 0, vibrationY = 0;

// Fixed values
const float loadValue = 80;
const float speedSet = 25;
int machineId = 1;
int isActive = 1;

// ==================== WiFi Setup ====================
void setup_wifi() {
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("WiFi connected");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
}

// ==================== MQTT Reconnect ====================
void reconnect() {
  while (!mqttClient.connected()) {
    Serial.print("Attempting MQTT connection...");
    if (mqttClient.connect(mqtt_client_id)) {
      Serial.println("connected");
    } else {
      Serial.print("failed, rc=");
      Serial.print(mqttClient.state());
      Serial.println(" retrying in 5 seconds");
      delay(5000);
    }
  }
}

// ==================== Setup ====================
void setup() {
  Serial.begin(115200);
  setup_wifi();
  
  // Initialize MQTT
  mqttClient.setServer(mqtt_server, 1883);
  
  // Initialize MPU6050
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }
  Serial.println("MPU6050 connected");
}

// ==================== Main Loop ====================
void loop() {
  if (!mqttClient.connected()) {
    reconnect();
  }
  mqttClient.loop();

  // Calculate dt (time difference)
  unsigned long currentTime = millis();
  float dt = (currentTime - lastTime) / 1000.0;
  lastTime = currentTime;

  // Read accelerometer data
  mpu.getAcceleration(&ax, &ay, &az);
  
  // Convert to g and m/s²
  ax_g = ax / 16384.0;
  ay_g = ay / 16384.0;
  az_g = az / 16384.0;
  
  ax_ms2 = ax_g * 9.81;
  ay_ms2 = ay_g * 9.81;
  az_ms2 = az_g * 9.81;

  // Calculate displacement
  velocity_x += ax_ms2 * dt;
  velocity_y += ay_ms2 * dt;
  displacement_x += velocity_x * dt;
  displacement_y += velocity_y * dt;

  vibrationX = displacement_x * 1000;  // mm
  vibrationY = displacement_y * 1000;  // mm

  // Print to Serial (for debugging)
  Serial.print("Vibration X: ");
  Serial.print(vibrationX);
  Serial.print(" mm, Y: ");
  Serial.println(vibrationY);

  // ===== MQTT Publish =====
  String payload = 
    "{"
    "\"data\":{"
        "\"load_value\":" + String(loadValue) + "," +
        "\"speed_set\":" + String(speedSet) + "," +
        "\"vibration_x\":" + String(vibrationX) + "," +
        "\"vibration_y\":" + String(vibrationY) + "," +
        "\"machine_id\":" + String(machineId) + "," +
        "\"is_active\":" + String(isActive) +
    "}"
    "}";

  if (mqttClient.publish(mqtt_topic, payload.c_str())) {
    Serial.println("MQTT publish success");
  } else {
    Serial.println("MQTT publish failed");
  }

  delay(200);  // Adjust as needed
}