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

#include <VL53L0X.h>//sensor libraries
#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 msg[MSG_BUFFER_SIZE];

//sensor
VL53L0X sensor;
const int8_t PIN_SDA = D1;
const int8_t PIN_SCL = D2;

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) {
  char* message = (char*) malloc(sizeof(char)*(length+1));
  for(int i=0; i<length;++i){
    message[i] = (char)payload[i];
  }
  message[length] = '\0';
  wemosListener = String(message);
  free(message);
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("]: ");
  Serial.println(wemosListener);
}

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 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(mqttSensorTopic);
  mqttPublishMessage(mqttServerStatus, "Distance Sensor Connected.");
  
  //Sensor
  Wire.begin(PIN_SDA, PIN_SCL);
  sensor.init();
  sensor.setTimeout(500);
  sensor.startContinuous();
  
  Serial.println("Sensor ready.");
}

void loop() {

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

  unsigned long now = millis();
  if (now - lastMsg > 2000) {
    long distance = sensor.readRangeContinuousMillimeters();
    lastMsg = now;
    snprintf (msg, MSG_BUFFER_SIZE, "sensor #%ld mm", distance/10);
    Serial.print("Publish message: ");
    Serial.println(msg);
    mqttPublishMessage(mqttOLEDTopic, msg);
  }
}