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

// WiFi
constexpr char WIFI_SSID[] = "Wokwi-GUEST";
constexpr char WIFI_PASSWORD[] = "";

// ThingsBoard access token untuk koneksi ke Device Thingsboardnya
constexpr char TOKEN[] = "VKPgZojn19kQ6IEiFsEP";
constexpr char THINGSBOARD_SERVER[] = "thingsboard.cloud";

// default MQTT unencrypted port for ThingsBoard
constexpr uint16_t THINGSBOARD_PORT = 1883U;

// maximum package size
constexpr uint32_t MAX_MESSAGE_SIZE = 1024U;

// Inisialisasi client wifi, used to establish a connection
WiFiClient wifiClient;

// Initalize the Mqtt client instance
Arduino_MQTT_Client mqttClient(wifiClient);

// Initialize ThingsBoard instance with the maximum needed buffer size
ThingsBoard tb(mqttClient, MAX_MESSAGE_SIZE);

// initialize MPU
Adafruit_MPU6050 mpu;
sensors_event_t a, g, temp;

//initialize IMU
void initMPU(){
  Serial.println("Adafruit MPU6050 test!");

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

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
}

// Initialize wifi connection
void initWiFi() {
  Serial.println("Connecting to AP ...");
  // Attempting to establish a connection to the given WiFi network
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  while (WiFi.status() != WL_CONNECTED) {
    // Delay 500ms until a connection has been succesfully established
    delay(500);
    Serial.print(".");
  }
  Serial.println("Connected to AP");
}

// Check if Wifi is still connected
void reconnectWiFi(){
  if(WiFi.status() != WL_CONNECTED)
    initWiFi();
}

// initialize ThingsBoard
void initTB(){
  if (!tb.connected()) {
    if (tb.connect(THINGSBOARD_SERVER, TOKEN, THINGSBOARD_PORT)) {
      Serial.println("Connected to ThingsBoard");
    } 
    else {
      Serial.println("Failed to connect to ThingsBoard");
      delay(1000);
      return;
    }
  }
}

// check if ThingsBoard still connect
void reconnectTB(){
  if(!tb.connected())
    initTB();
}

void setup() {
  Serial.begin(9600);
  // Connect to WiFi
  initWiFi();
  // Connect to ThingsBoard
  initTB();
  // Check and initialize MPU
  initMPU();
}

void loop() {
  // used to reconnect wifi and ThingsBoard
  reconnectWiFi();
  reconnectTB();

  // get data from accelometer
  mpu.getEvent(&a, &g, &temp);

  // send data to thingsboard
  tb.sendTelemetryData("x", a.acceleration.x);
  tb.sendTelemetryData("y", a.acceleration.y);
  tb.sendTelemetryData("z", a.acceleration.z);
  
  delay(100);
}