#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <math.h>
#include <Discord_WebHook.h>
#include "ThingsBoard.h"

#define WifiSSID "Wokwi-GUEST"
#define WifiPassword ""

#define CURRENT_FIRMWARE_TITLE    "DoorIntruderDetector-PintuDepan"
#define CURRENT_FIRMWARE_VERSION  "1.0.0"

#define TOKEN               "wLmc7ZfgYUNOi4zR3D6V"
#define THINGSBOARD_SERVER  "demo.thingsboard.io"

Adafruit_MPU6050 mpu;
unsigned long lastUpdate = 0;
unsigned long currentTime = 0;
double lastXDegree = 0;
double lastYDegree = 0;
double lastZDegree = 0;
int resultCode = -1;

Discord_Webhook discord; 
String DISCORD_WEBHOOK = "https://discord.com/api/webhooks/1104105694136905870/45bkvh6UMMg0yzhEKQvPaSCY4qqM9p-lxzVW6lWzpwAUufdCNlN7ddzpIlS3_cSUZpFp";

// Initialize ThingsBoard client
WiFiClient espClient;
// Initialize ThingsBoard instance
ThingsBoard tb(espClient);

void checkThingsboardConnection(){
  if (!tb.connected()) {
    // Connect to the ThingsBoard
    Serial.print("Connecting to: ");
    Serial.print(THINGSBOARD_SERVER);
    Serial.print(" with token ");
    Serial.println(TOKEN);
    if (!tb.connect(THINGSBOARD_SERVER, TOKEN)) {
      Serial.println("Failed to connect");
      return;
    }
  }
}

double radianToDegree(double rad){
  return rad * (180.0/3.141592653589793238463);
}


// Initialize ThingsBoard client
WiFiClient espClient;
// Initialize ThingsBoard instance
ThingsBoard tb(espClient);

void checkThingsboardConnection(){
  if (!tb.connected()) {
    // Connect to the ThingsBoard
    Serial.print("Connecting to: ");
    Serial.print(THINGSBOARD_SERVER);
    Serial.print(" with token ");
    Serial.println(TOKEN);
    if (!tb.connect(THINGSBOARD_SERVER, TOKEN)) {
      Serial.println("Failed to connect");
      return;
    }
  }
}

double radianToDegree(double rad){
  return rad * (180.0/3.141592653589793238463);
}

/*
  Result Code:
  1   : Ada maling
  -1  : Tidak ada maling
*/
void checkGyroDegreeChange(double x, double y, double z){
  
  if((fabs(x) > 7.0 && x != lastXDegree || fabs(y) > 7.0 && y != lastYDegree  || fabs(z) > 7.0 && z != lastZDegree) ){
    Serial.println("**INTRUDER DETECTED**");
    tb.sendTelemetryInt("resultCode", 1);
    resultCode = 1;  
    //discord.send("@here INTRUDER DETECTED!!!");
  }
  else if(resultCode != -1){
    tb.sendTelemetryInt("resultCode", -1);
    resultCode = -1;
  }
  tb.sendTelemetryInt("gyroX", (int)x);
  tb.sendTelemetryInt("gyroY", (int)y);
  tb.sendTelemetryInt("gyroZ", (int)z);
  lastXDegree = x;
  lastYDegree = y;
  lastZDegree = z;
}

void loopMPU6050Sensor(){
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  currentTime = millis();
  
  if(currentTime - lastUpdate > 3000){
    lastUpdate = currentTime;
    checkGyroDegreeChange(radianToDegree(g.gyro.x), radianToDegree(g.gyro.y), radianToDegree(g.gyro.z));
  }

  Serial.print(millis());
  Serial.println(" ms");
  /* Print out the values */

  Serial.print("Rotation X: ");
  Serial.print(radianToDegree(g.gyro.x));
  Serial.print(", Y: ");
  Serial.print(radianToDegree(g.gyro.y));
  Serial.print(", Z: ");
  Serial.print(radianToDegree(g.gyro.z));
  Serial.println(" deg/s");

  Serial.println("");
}

void setup(void) {
  Serial.begin(115200);
  discord.begin(DISCORD_WEBHOOK);
  discord.addWiFi(WifiSSID, WifiPassword);

  discord.connectWiFi();
  discord.send("Door Intruder Webhook Connected!");
  while (!Serial)
    delay(10);

  Serial.println("Door Intruder Detector");

  // 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);
  Serial.println("");
  delay(100);
}

void loop() {
  checkThingsboardConnection();
  loopMPU6050Sensor();
  delay(500);
  tb.loop();
}