#include <WiFi.h>
#include <Wire.h>
#include "ThingSpeak.h"
#include <PubSubClient.h>
#include <MPU6050.h>


const char* mqtt_acx_topic = "iot/acx";
const char* mqtt_acy_topic = "iot/acy";
const char* mqtt_acz_topic = "iot/acz";
const char* mqtt_gx_topic = "iot/gx";
const char* mqtt_gy_topic = "iot/gy";
const char* mqtt_gz_topic = "iot/gz";

const char *ssid = "Wokwi-GUEST";   // Your network SSID (name)
const char *pass = "";   // Your network password (fill this in)

const char* mqtt_server = "localhost";
const int mqtt_port = 1883;

const int LED_PIN_1 = 13;  // GPIO pin connected to the first LED
const int LED_PIN_2 = 12;  // GPIO pin connected to the second LED

WiFiClient client;  // Create a Wi-Fi client

unsigned long myTalkBackID = 50127;
const char *myTalkBackKey = "8Y8LH1XQY0ZXIDHJ";

unsigned long myChannelNumber = 2272854;  // Your channel number
const char *myWriteAPIKey = "HF2KUANDCAXE8PUZ";  // Your channel write API Key

unsigned long timer = 0;

float Cacc = 16384.0;
float Cgyro = 131.0; 
 
MPU6050 mpu;
WiFiClient espClient;
PubSubClient client(espClient);

void setup_wifi() {
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    
    if (client.connect("ESP32Client")) {
      Serial.println("connected");
      // Subscribe to MQTT topics here if needed
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

void setup() {
  
  pinMode(LED_PIN_1, OUTPUT);    // Set the first LED pin as an output
  pinMode(LED_PIN_2, OUTPUT);    // Set the second LED pin as an output
 
  Serial.begin(115200);  // Initialize serial
  Wire.begin();


    mpu.initialize();

  // Vérifiez si le MPU6050 est accessible
  if (!mpu.testConnection()) {
    Serial.println("Erreur de connexion MPU6050");
    while (1);
  }


  setup_wifi();
  WiFi.mode(WIFI_STA);

  ThingSpeak.begin(client);  // Initialize ThingSpeak

  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(callback);


}

void loop() {

    if (!client.connected()) {
    reconnect();
  }

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  float acx = ax / Cacc; 
  float acy = ay / Cacc;
  float acz = az / Cacc;
  float gx = gx / Cgyro;
  float gy = gy / Cgyro;
  float gz = gz / Cgyro;

  // Publish data to MQTT topics
  client.publish(mqtt_acx_topic, String(acx).c_str());
  client.publish(mqtt_acy_topic, String(acy).c_str());
  client.publish(mqtt_acz_topic, String(acz).c_str());
  client.publish(mqtt_gx_topic, String(gx).c_str());
  client.publish(mqtt_gy_topic, String(gy).c_str());
  client.publish(mqtt_gz_topic, String(gz).c_str());

  delay(100);

  mpu6050result();
  checkThingSpeak();
}

void mpu6050result() {
  mpu.update();
  if ((millis() - timer) > 100) { // print data every 0.1s
    ThingSpeak.setField(1, acx);
    ThingSpeak.setField(2, acy);
    ThingSpeak.setField(3, acz);
    ThingSpeak.setField(4, gx);
    ThingSpeak.setField(5, gy);
    ThingSpeak.setField(6, gz);
    ThingSpeak.writeFields(myChannelNumber, myWriteAPIKey);
    timer = millis();
  }
}

void checkThingSpeak() {
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("Wi-Fi not connected.");
    return;
  }
  String tbURI = "/talkbacks/" + String(myTalkBackID) + "/commands/execute";
  String postMessage = "api_key=" + String(myTalkBackKey);
  String newCommand = "";
  if (httpPOST(tbURI, postMessage, newCommand) == 200) {
    Serial.println("Checking queue...");
    if (newCommand.indexOf("led1 on") != -1) {
      Serial.println("New command: Turn LED 1 on");
      digitalWrite(LED_PIN_1, HIGH);
    } else if (newCommand.indexOf("led1 off") != -1) {
      Serial.println("New command: Turn LED 1 off");
      digitalWrite(LED_PIN_1, LOW);
    } else if (newCommand.indexOf("led2 on") != -1) {
      Serial.println("New command: Turn LED 2 on");
      digitalWrite(LED_PIN_2, HIGH);
    } else if (newCommand.indexOf("led2 off") != -1) {
      Serial.println("New command: Turn LED 2 off");
      digitalWrite(LED_PIN_2, LOW);
    } else {
      Serial.println("No command or syntax error!");
    }
  }
}

int httpPOST(String uri, String postMessage, String &response) {
  bool connectSuccess = false;
  connectSuccess = client.connect("api.thingspeak.com", 80);
  if (!connectSuccess) {
    return -301; // Connection error
  }
  postMessage += "&headers=false";
  String headers = "POST " + uri + " HTTP/1.1\r\n" +
                   "Host: api.thingspeak.com\r\n" +
                   "Content-Type: application/x-www-form-urlencoded\r\n" +
                   "Connection: close\r\n" +
                   "Content-Length: " + String(postMessage.length()) + "\r\n\r\n";
  client.print(headers);
  client.print(postMessage);
  unsigned long startWaitForResponseAt = millis();
  while (client.connected() && !client.available()) {
    if (millis() - startWaitForResponseAt > 5000) {
      client.stop();
      return -304; // Didn't get server response in time
    }
    delay(100);
  }
  if (!client.connected()) {
    return -305; // Connection lost
  }

  if (!client.find("HTTP/1.1")) {
    return -303; // Couldn't parse response (didn't find HTTP/1.1)
  }

  int status = client.parseInt();
  if (status != 200) {
    return status;
  }
  if (!client.find("\n\r\n")) {
    return -303;
  }
  response = client.readString();
  return status;
}