#include <WiFi.h>
#include <PubSubClient.h>
#include <Wire.h>
#include <esp_wifi.h>
#include <esp_bt.h>


// Replace the next variables with your SSID/Password combination
const char* ssid = "***";
const char* password = "****";

// Add your MQTT Broker IP address, example:
//const char* mqtt_server = "192.168.1.144";
const char* mqtt_server = "192.168.1.26";

// Set your Static IP address
IPAddress local_IP(192, 168, 1, 44);
// Set your Gateway IP address
IPAddress gateway(192, 168, 1, 1);
IPAddress subnet(255, 255, 255, 0);

WiFiClient espClient;
PubSubClient client(espClient);
long lastMsg = 0;

#define uS_TO_S_FACTOR 1000000  /* Conversion factor for micro seconds to seconds */
#define TIME_TO_SLEEP  20        /* Time ESP32 will go to sleep (in seconds) */
#define WIFI_TIMEOUT 10000       /* 10 secondes in milliseconds */
#define TIMEOUT 20000

unsigned long chrono = millis();

float bat = 0;
float bat_pourcentage = 0;
float bat_volt = 0;

RTC_DATA_ATTR int bootCount = 0;

unsigned long Tempo = 3;                 /* Tempo Volet ON/OFF (secondes) */
unsigned long TempoMS = Tempo * 1000;    /* Tempo Volet en ms) */

// Volet Pin
const int MB = 16;
const int DB = 17;
const int MH = 5;
const int DH = 21;

void setup() {
  setCpuFrequencyMhz(80);
  Serial.begin(115200);
  // default settings
  // (you can also pass in a Wire library object like &Wire2)
  //status = bme.begin(); 
  setup_wifi();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);

  pinMode(MB, OUTPUT);
  pinMode(DB, OUTPUT);
  pinMode(MH, OUTPUT);
  pinMode(DH, OUTPUT);
}

void setup_wifi() {
  delay(10);
  // We start by connecting to a WiFi network
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

 if (!WiFi.config(local_IP, gateway, subnet)) {
    Serial.println("STATIC IP Failed to configure");
  } 
  WiFi.begin(ssid, password);
  unsigned long startAttemptTime = millis(); // Chrono 1er connection
  while (WiFi.status() != WL_CONNECTED && 
          millis() - startAttemptTime < WIFI_TIMEOUT) {
    delay(10);
    //Serial.print(".");
  }

  if(WiFi.status() != WL_CONNECTED){
    Serial.println("FAILED");
    goToDeepSleep();
  }  
  //Serial.println("");
  //Serial.println("WiFi connected");
  //Serial.println("IP address: ");
  //Serial.println(WiFi.localIP());
}

void goToDeepSleep()
{
  //Serial.println("Going to sleep...");
  WiFi.disconnect(true);
  WiFi.mode(WIFI_OFF);
  btStop();
  esp_wifi_stop();
  esp_bt_controller_disable();
  esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP * uS_TO_S_FACTOR);
  esp_deep_sleep_start();
}

void callback(char* topic, byte* message, unsigned int length) {
  Serial.print("Message arrived on topic: ");
  Serial.print(topic);
  Serial.print(". Message: ");
  String messageTemp;
  
  for (int i = 0; i < length; i++) {
    Serial.print((char)message[i]);
    messageTemp += (char)message[i];
  }
  Serial.println();

  // Feel free to add more if statements to control more GPIOs with MQTT

  // If a message is received on the topic esp32/output, you check if the message is either "on" or "off". 
  // Changes the output state according to the message
  if (String(topic) == "esp32/output") {
    //Serial.print("Changing output to ");
   if (messageTemp == "Pause"){
     ++bootCount ;
    char countString[8];
    dtostrf(bootCount, 1, 2, countString);
    //Serial.println("Sleep");
    //Serial.println(countString);
    client.publish("esp32/Sleep_compteur", countString);
    delay(100);
    client.publish("esp32/Etatvolet", "Pause");
    Serial.println("Sleep - Pause");
    goToDeepSleep();
      }
   else {
      bootCount = 0;
      if(messageTemp == "MBon"){
        //Serial.println("MBas on");
        digitalWrite(MB, HIGH);
        delay(TempoMS);
        digitalWrite(MB, LOW);
        client.publish("esp32/Etatvolet", "1");
      }
      else if(messageTemp == "DBon"){
        //Serial.println("DBas on");
        digitalWrite(DB, HIGH);
        delay(TempoMS);
        digitalWrite(DB, LOW);
        client.publish("esp32/Etatvolet", "1");
      }
      else if(messageTemp == "MHon"){
        //Serial.println("MHaut on");
        digitalWrite(MH, HIGH);
        delay(TempoMS);
        digitalWrite(MH, LOW);
        client.publish("esp32/Etatvolet", "1");
      }
      else if(messageTemp == "DHon"){
        //Serial.println("DHaut on");
        digitalWrite(DH, HIGH);
        delay(TempoMS);
        digitalWrite(DH, LOW);
        client.publish("esp32/Etatvolet", "1");
      }
      else if(messageTemp == "BAT"){
        bat = analogRead(35);
        bat_volt = (bat * 7.65)/(4096); //7.445
        bat_pourcentage =((-0.9759)*pow(bat_volt,3)+11.58*pow(bat_volt,2)-(44.232*bat_volt)+54.861)*100;
          if (bat_pourcentage > 100){
           bat_pourcentage = 100;
          }
        char batVString[8];
        char batPString[8];
        dtostrf(bat_volt, 1, 2, batVString);
        dtostrf(bat_pourcentage, 1, 2, batPString);
        //Serial.print("Voltage batterie ");
        //Serial.println(batVString);
        client.publish("esp32/V_battery", batVString);
        //Serial.print("Poucentage batterie ");
        //Serial.println(batPString);
        delay(500);
        client.publish("esp32/p_battery", batPString);
        client.publish("esp32/Etatvolet", "1");
      }
      else {
       Serial.print("Sleep - NO elseif");
       goToDeepSleep();
      }
    }
    goToDeepSleep();
  }
  else {
    Serial.print("Sleep - NO topic");
    goToDeepSleep();
  }
}

void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("ESP32Client")) {
      Serial.println("connected");
      // Subscribe
      //client.subscribe("esp32/output");
      if (!client.subscribe("esp32/output")) {
      Serial.println("Sleep Subscribe failed");
      goToDeepSleep();
      }
    } 
    else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}
void loop() {
  while (millis() - chrono < TIMEOUT) {
    if (!client.connected()) {
   reconnect();
  }
  client.loop();
  delay(10);
  }
  Serial.print("Global Timeout");
  goToDeepSleep();
}