#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <WiFi.h>
#include <ThingsBoard.h>
#include <Arduino_MQTT_Client.h>
#include <TBPubSubClient.h>
#include <ArduinoJson.h>

Adafruit_MPU6050 mpu;

const int echoPin = 12;
const int trigPin = 14;
float distance, duration;

#define WIFI_AP "Wokwi-GUEST"
#define WIFI_PASS ""

#define TB_SERVER "thingsboard.cloud"
#define TOKEN "of5sPflyQZd3LM6CnwG0"

WiFiClient wifiClient;
Arduino_MQTT_Client mqttClient(wifiClient);
ThingsBoard tb(mqttClient);

void setup() {
  Serial.begin(115200);
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);
  while(!Serial)
    delay(10);

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

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }

  Serial.println("");
  delay(100);

  initWifi();
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration * 0.0343) / 2;
  Serial.print("Jarak : ");
  Serial.print(distance);
  Serial.println(" cm");

  if(!tb.connected()) {
    Serial.println("Connecting to server...");
    if(!tb.connect(TB_SERVER, TOKEN)) {
      Serial.println("Failed connecting to server!");
      return;
    }
  } else {
    Serial.println("Connected to server!");
  }

  tb.sendTelemetryData("Acc X", a.acceleration.x);
  tb.sendTelemetryData("Acc Y", a.acceleration.y);
  tb.sendTelemetryData("Acc Z", a.acceleration.z);
  tb.sendTelemetryData("Dist", distance);
  
  Serial.println("Sending data...");

  delay(1000);
  tb.loop();
}

void initWifi() {
  Serial.println("Connecting WiFi...");
  while(WiFi.status() != WL_CONNECTED) {
    WiFi.begin(WIFI_AP, WIFI_PASS, 6);
    Serial.print(".");
    delay(500);
  }
  Serial.println("");
  Serial.println("WiFi Connected!");
}