#include <WiFi.h>
#include <PubSubClient.h> 

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>


#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

Adafruit_MPU6050 mpu;

char* ssid = "Wokwi-GUEST"; //請修改為您連線的網路名稱
char* password = ""; //請修改為您連線的網路密碼
// ------ 以下修改成你MQTT設定 ------
char* MQTTServer = "mqttgo.io";//免註冊MQTT伺服器
int MQTTPort = 1883;//MQTT Port 除非加密，否則不用更改port
char* MQTTUser = "";//不須帳密
char* MQTTPassword = "";//不須帳密

//推播主題1:推播訊息(記得改Topic)
char* MQTTPubTopic1 = "eric/msg";
//推播主題2:推播數字(記得改Topic)
char* MQTTPubTopic2 = "mpu6050/num";

long MQTTLastPublishTime;//此變數用來記錄推播時間
long MQTTPublishInterval = 2000;//每10秒推撥一次
//建立MQTT與聯網物件
WiFiClient WifiClient;
PubSubClient MQTTClient(WifiClient);

int num=1000;
char buffer[5];
void setup(void) {
  Serial.begin(115200);

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE,0);
  display.setCursor(35,10);
  display.print("ESP32");
  display.setCursor(25,40);
  display.print("MPU6050");
  display.display();

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

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("");
  delay(1000);

  WifiConnecte(); 
  MQTTConnecte();
}

void loop() {

  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE,0);
  display.setCursor(0,0);
  display.println("Acelerometro:");
  display.print(a.acceleration.x);
  display.print(",");
  display.print(a.acceleration.y);
  display.print(",");
  display.println(a.acceleration.z);
  display.println("");

  display.println("Giroscopio:");
  display.print(g.gyro.x);
  display.print(",");
  display.print(g.gyro.y);
  display.print(",");
  display.print(g.gyro.z);
  display.display();

  if (WiFi.status() != WL_CONNECTED) { WifiConnecte(); }
  //如果MQTT連線中斷，則重啟MQTT連線
  if (!MQTTClient.connected()) {  MQTTConnecte(); }
  //如果距離上次傳輸已經超過10秒，則Publish溫溼度
  if ((millis() - MQTTLastPublishTime) >= MQTTPublishInterval ) {
    num += 1;
    mpu.getEvent(&a, &g, &temp);
    // MQTTClient.publish(MQTTPubTopic1, "這是msg");//將訊息推播到MQTTPubTopic1
    int acc = sqrt(a.acceleration.x*a.acceleration.x+a.acceleration.y*a.acceleration.y+a.acceleration.z*a.acceleration.z);
    MQTTClient.publish(MQTTPubTopic2, itoa(acc*100 ,buffer,10));//將訊息推播到MQTTPubTopic2
    Serial.print("訊息已推播到MQTT Broker ");
    Serial.println(acc*100);
    MQTTLastPublishTime = millis(); //更新最後傳輸時間
  }
  MQTTClient.loop();//更新訂閱狀態
  delay(50);

  delay(10);
}

void WifiConnecte() {
  //開始WiFi連線
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("WiFi連線成功");
  Serial.print("IP Address:");
  Serial.println(WiFi.localIP());
}

//開始MQTT連線
void MQTTConnecte() {
  MQTTClient.setServer(MQTTServer, MQTTPort);
  while (!MQTTClient.connected()) {
    //以亂數為ClietID
    String  MQTTClientid = "esp32-" + String(random(1000000, 9999999));//建立一個id，MQTT要求id要不一樣，所以使用亂數
    if (MQTTClient.connect(MQTTClientid.c_str(), MQTTUser, MQTTPassword)) {
      //連結成功，顯示「已連線」。
      Serial.println("MQTT已連線");
    } else {
      //若連線不成功，則顯示錯誤訊息，並重新連線
      Serial.print("MQTT連線失敗,狀態碼=");
      Serial.println(MQTTClient.state());
      Serial.println("五秒後重新連線");
      delay(5000);
    }
  }
}