#include <ESP8266WiFi.h>  //Wifi
#include <PubSubClient.h> //MQTT

#include <U8g2lib.h>      //OLED Library
#include <Wire.h>

//Wifi variables
const char* ssid = "FabAcademy";
const char* password = "fabacademy";
IPAddress wemosIP; //IP addres that wemos received

//MQTT server
const char* mqtt_server = "192.168.4.1";
const int mqtt_port = 1883;
WiFiClient espClient;
const char* mqttServerStatus = "mqttServerStatus";
PubSubClient MQTTclient(espClient);

String wemosListener; //message received from MQTT
char* mqttSensorTopic = "mqttSensorListener";
char* mqttOLEDTopic = "mqttOLEDListener";

//MQTT send message
unsigned long lastMsg = 0;
#define MSG_BUFFER_SIZE  (50)
char msgReceived[MSG_BUFFER_SIZE];

//OLED
const int8_t PIN_SDA = D1;
const int8_t PIN_SCL = D2;
U8G2_SSD1306_128X32_UNIVISION_F_SW_I2C u8g2(U8G2_R0, PIN_SCL, PIN_SDA, U8X8_PIN_NONE);
bool initialized = false;

void setup_wifi() {

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

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  randomSeed(micros());

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

void mqttMessageCallback(char* topic, byte* payload, unsigned int length) {
  for(int i=0; i<length;++i){
    msgReceived[i] = (char)payload[i];
  }
  msgReceived[length] = '\0';
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("]: ");
  Serial.println(msgReceived);
  
  update_oled();
}

void setupMqttClient() {
  // to call in the setup of the arduino
  MQTTclient.setServer(mqtt_server, mqtt_port);
  MQTTclient.setCallback(mqttMessageCallback);
}

void mqttSubscribeTopic(char* topic){
  MQTTclient.subscribe(topic);
  Serial.print("MQTT subscribed to: ");
  Serial.println(topic);
}

boolean mqttPublishMessage(const char* topic, const char* payload) {
  Serial.print("Publishing message: ");
  Serial.println(payload);
  // API to publish to MQTT server. 
  MQTTclient.publish(topic, payload);
}

void mqttReconnect() {
  // Loop until MQTT is reconnected
  while (!MQTTclient.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Create a random client ID
    String clientId = "WemosSensorClient-";
    clientId += String(random(0xffff), HEX);
    // Attempt to connect
    if (MQTTclient.connect(clientId.c_str())) {
      Serial.println("connected");
    } else {
      Serial.print("failed, rc=");
      Serial.print(MQTTclient.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

int handleMQTTClient() {
  int reconn = 0;
  // to call in the loop of the arduino
  if (!MQTTclient.connected()) {
    // LED on to  indicate connection problem
    digitalWrite(LED_BUILTIN, LOW);
    mqttReconnect();
    reconn = 1;
  } else {
    // LED off all is ok
    digitalWrite(LED_BUILTIN, HIGH);
  }
  MQTTclient.loop();
  return reconn;
}

void initial_display()
{
  if (not initialized)
  {
    u8g2.clearBuffer(); // clear the internal memory
    u8g2.setFont(u8g2_font_smart_patrol_nbp_tf); // choose a suitable font
    u8g2.drawStr(0,20,"Dist. Sensor");
    u8g2.sendBuffer();          // transfer internal memory to the display
    delay(2000);  
 
    initialized = true;
  }
}

bool update_oled()
{
  u8g2.clearBuffer(); // clear the internal memory
  u8g2.setFont(u8g2_font_smart_patrol_nbp_tf); // choose a suitable font
  u8g2.drawStr(0, 20, msgReceived);  // write something to the internal memory
  u8g2.sendBuffer();          // transfer internal memory to the display
  delay(1);
}

void setup() {
  pinMode(BUILTIN_LED, OUTPUT);     // Initialize the BUILTIN_LED pin as an output
  Serial.begin(9600);
  Serial.println();
  Serial.println("init setup...");
  
  setup_wifi();

  //MQTT
  setupMqttClient();
  mqttReconnect();
  mqttSubscribeTopic(mqttOLEDTopic);
  mqttPublishMessage(mqttServerStatus, "OLED Connected.");
  
  //OLED
  Wire.begin(PIN_SDA, PIN_SCL);
  u8g2.begin();
  initial_display();
}

void loop() {
  //handle MQTT messages
  if(handleMQTTClient()){
    //reconnected, subscribe again
    mqttSubscribeTopic(mqttOLEDTopic);
    // inform server wemos is re-connected
    mqttPublishMessage(mqttServerStatus, "OLED RE-Connected.");
  }
}