diff --git a/src/main.cpp b/src/main.cpp
index b322e0f..b655600 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,5 +1,6 @@
 #include <Arduino.h>
-#include "ESP8266WiFi.h"
+#include <ESP8266WiFi.h>
+#include <PubSubClient.h>
 
 #define TURN_ON LOW
 #define TURN_OFF HIGH
@@ -8,54 +9,111 @@
 #define wifi_password "4300sf08fhln"
 
 #define mqtt_server "192.168.249.5"
-#define mqtt_port "1883"
+#define mqtt_port 1883
 #define mqtt_user "homedevice"
 #define mqtt_password "WNzAb4VazNFUPlpkn0ED"
+#define mqtt_client_id "home_DistanceSensor"
+
+#define distance_topic    "garagesensor/distance"
+#define heartbeatMillis 10000
+#define messageMillis 1000
+#define will_topic "garagesensor/online"
+#define will_message "false"
+
+WiFiClient espClient;
+PubSubClient client(espClient);
 
 void heartbeat();
 void longbeat();
+void setup_wifi();
+void reconnect();
 
 void setup() {
   Serial.begin(115200);
+  randomSeed(analogRead(0));
   pinMode(LED_BUILTIN, OUTPUT);
   
-  // Set WiFi to station mode and disconnect from an AP if it was previously connected
-  WiFi.mode(WIFI_STA);
-  WiFi.disconnect();
-  delay(100);
-  Serial.println("Setup done");
+  setup_wifi();
+  client.setServer(mqtt_server, mqtt_port);
+}
+
+long lastMessage = 0;
+long lastHeartbeat = 0;
+float distance = 0.0;
+float diff = 4.0;
+
+bool checkBound(float newValue, float prevValue, float maxDiff) {
+  return !isnan(newValue) &&
+         (newValue < prevValue + maxDiff || newValue > prevValue - maxDiff);
 }
 
 void loop() {
-  Serial.println("scan start");
-  heartbeat();
+  if (!client.connected()) {
+    reconnect();
+  }
+  client.loop();
+  client.publish(will_topic, "true");
 
-  // WiFi.scanNetworks will return the number of networks found
-  int n = WiFi.scanNetworks();
-  Serial.println("scan done");
-  if (n == 0)
-    Serial.println("no networks found");
-  else
-  {
-    Serial.print(n);
-    Serial.println(" networks found");
-    for (int i = 0; i < n; ++i)
-    {
-      // Print SSID and RSSI for each network found
-      Serial.print(i + 1);
-      Serial.print(": ");
-      Serial.print(WiFi.SSID(i));
-      Serial.print(" (");
-      Serial.print(WiFi.RSSI(i));
-      Serial.print(")");
-      Serial.println((WiFi.encryptionType(i) == ENC_TYPE_NONE)?" ":"*");
-      delay(10);
+  long now = millis();
+  if (now - lastHeartbeat > heartbeatMillis) {
+    lastHeartbeat = now;
+    heartbeat();
+  }
+
+  if (now - lastMessage > messageMillis) {
+    lastMessage = now;
+
+    float newDistance = random(5,400)/100.0;
+    Serial.print("New distance:");
+    Serial.println(String(newDistance).c_str());
+
+    if (checkBound(newDistance, distance, diff)) {
+      distance = newDistance;
+      Serial.print(distance_topic);
+      Serial.print(" = ");
+      Serial.println(String(distance).c_str());
+      client.publish(distance_topic, String(distance).c_str());
     }
   }
+}
+
+void setup_wifi() {
+  delay(10);
+  // We start by connecting to a WiFi network
+  Serial.println();
+  Serial.print("Connecting to ");
+  Serial.println(wifi_ssid);
+
+  WiFi.begin(wifi_ssid, wifi_password);
+
+  while (WiFi.status() != WL_CONNECTED) {
+    delay(500);
+    Serial.print(".");
+  }
+
   Serial.println("");
-  longbeat();
-  // Wait a bit before scanning again
-  delay(5000);
+  Serial.println("WiFi connected");
+  Serial.print("IP address: ");
+  Serial.println(WiFi.localIP());
+}
+
+void reconnect() {
+  // Loop until we're reconnected
+  while (!client.connected()) {
+    Serial.print("Attempting MQTT connection...");
+    // Attempt to connect
+    // If you do not want to use a username and password, change next line to
+    // if (client.connect("ESP8266Client")) {
+    if (client.connect(mqtt_client_id, mqtt_user, mqtt_password, will_topic, 2, false, will_message)) {
+      Serial.println("connected");
+    } else {
+      Serial.print("failed, rc=");
+      Serial.print(client.state());
+      Serial.println(" try again in 5 seconds");
+      // Wait 5 seconds before retrying
+      delay(5000);
+    }
+  }
 }
 
 void heartbeat()