#ifdef CORE_DEBUG_LEVEL
#undef CORE_DEBUG_LEVEL
#endif

#define CORE_DEBUG_LEVEL 3
#define LOG_LOCAL_LEVEL ESP_LOG_DEBUG

#include <WiFi.h>
#include <ArduinoJson.h>
#include <HTTPClient.h>
#include <base64.h>
#include <ESPmDNS.h>
#include <Servo.h>
#include <PubSubClient.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>

// WiFi Info
const char* ssid = "Unknown Network";
const char* password = "HDUUpdcbEc3w";

// MQTT 
const char *mqtt_broker = "192.168.0.42";
const char *topic = "esp32/switch";
const int mqtt_port = 1883;

WiFiClient espClient;
PubSubClient mqttClient(espClient);
long lastMsg = 0;
char msg[50];
int value = 0;

// Servo Vars
int lightPin = 13;
int pos = 0;

Servo leftServo;
Servo rightServo;

/* Returns a semi-unique id for the device. The id is based
   on part of a MAC address or chip ID so it won't be
   globally unique. */
uint16_t GetDeviceId()
{
#if defined(ARDUINO_ARCH_ESP32)
  return ESP.getEfuseMac();
#else
  return ESP.getChipId();
#endif
}

/* Append a semi-unique id to the name template */
String MakeMine(const char *NameTemplate)
{
  uint16_t uChipId = GetDeviceId();
  String Result = String(NameTemplate) + String(uChipId, HEX);
  return Result;
}

void AdvertiseServices(const char *MyName)
{
  if (MDNS.begin(MyName))
  {
    Serial.println(F("mDNS responder started"));
    Serial.print(F("I am: "));
    Serial.println(MyName);

    // Add service to MDNS-SD
    //MDNS.addService("sensor", "tcp", 8080);
  }
  else
  {
    while (1)
    {
      Serial.println(F("Error setting up MDNS responder"));
      delay(1000);
    }
  }
}

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/switch, you check if the message is either "on" or "off". 
  // Changes the output state according to the message
  if (String(topic) == "esp32/switch") {
    Serial.print("Changing switch to ");
    if(messageTemp == "on"){
      Serial.println("lights on");
      leftServo.write(180);
      rightServo.write(135);
    }
    else if(messageTemp == "off"){
      Serial.println("lights off");
      leftServo.write(135);
      rightServo.write(180);
    }
  }
}

void setupWifi() {
  delay(10);
  // We start by connecting to a WiFi network
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  
  WiFi.begin(ssid, password);
  Serial.println("Connecting");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.print("Connected to WiFi network with IP Address: ");
  Serial.println(WiFi.localIP());
}

void setupOTA(){
  // Arduino OTA Info
  // Port defaults to 3232
  // Hostname defaults to esp3232-[MAC]
  ArduinoOTA.setPasswordHash("b2cb7bf46d7afe5ad2ed16d87093d342");
  ArduinoOTA
      .onStart([]() {
        String type;
        if (ArduinoOTA.getCommand() == U_FLASH)
          type = "sketch";
        else // U_SPIFFS
          type = "filesystem";
  
        // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end()
        Serial.println("Start updating " + type);
      })
      .onEnd([]() {
        Serial.println("\nEnd");
      })
      .onProgress([](unsigned int progress, unsigned int total) {
        Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
      })
      .onError([](ota_error_t error) {
        Serial.printf("Error[%u]: ", error);
        if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed");
        else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed");
        else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed");
        else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed");
        else if (error == OTA_END_ERROR) Serial.println("End Failed");
      });
  
  ArduinoOTA.begin();
}

void setup() {
  Serial.begin(115200);
  Serial.println("Starting up");

  setupWifi();
  setupOTA();

  Serial.print("Subscribing to ");
  Serial.println(mqtt_broker);
  mqttClient.setServer(mqtt_broker, 1883);
  mqttClient.subscribe(topic);
  mqttClient.setCallback(callback);

  Serial.println("Attaching servo at pin 13");
  leftServo.attach(13);
  Serial.println("Attaching servo at pin 12");
  rightServo.attach(12);

  leftServo.write(135);
  rightServo.write(180);
  
  String MyName = MakeMine("Door monitor");
  //AdvertiseServices(MyName.c_str());
}

void reconnect() {
  // Loop until we're reconnected
  while (!mqttClient.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    String MyName = MakeMine("Light switch");
    if (mqttClient.connect(MyName.c_str())) {
      Serial.println("connected");
      // Subscribe
      mqttClient.subscribe(topic);
    } else {
      Serial.print("failed, rc=");
      Serial.print(mqttClient.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

void loop() {
  if (!mqttClient.connected()) {
    reconnect();
  }
  
  mqttClient.loop();
  ArduinoOTA.handle();
}