   unsigned char mac[6];
   WiFi.macAddress(mac);
   clientMac += macToStr(mac);
-  Serial.print("Mac address is...");
+  Serial.print("WiFi MAC: ");
   Serial.println(clientMac);
 }
 
-void init_wifi() {
-  Serial.println("Nappy Monitor v1");
-  Serial.println("Initialising Wifi");
+// Scan for WiFi networks to detect known network
+bool scanWiFiNetworks() {
+  // Scan for WiFi networks
+  int numOfNetworks = WiFi.scanNetworks(false);
+  // print list of WiFI networks detected
+  for (int i=0; i < numOfNetworks; i++) {
+    Serial.print("Network name: ");
+    Serial.println(WiFi.SSID(i));
+  }
+  // Check if we find known wifi networks
+  int i=0;
+  bool match = false;
+  while ((i < numOfNetworks) && (match == false)) {
+    // check the network SSID against our list of known networks
+    Serial.print("Checking "+String(numOfKnownNetworks)+" known wifi networks against network: i="+String(i));
+    for (int j=0; j< numOfKnownNetworks; j++) {
+      Serial.println(" and j="+String(j));
+      Serial.println("Comparing ["+WiFi.SSID(i)+"] with ["+knownWifiNetworks[j].ssid+"]");
+      if (strcmp(WiFi.SSID(i).c_str(),knownWifiNetworks[j].ssid.c_str()) == 0) {
+        // if its a match, set it as the selected WiFi SSID
+        match = true;
+        selectedWiFiNetwork = i;
+        selectedWiFiNetworkSSID = WiFi.SSID(i);
+        selectedWiFiNetworkPassword = knownWifiNetworks[j].password;
+        Serial.println("Selected: "+selectedWiFiNetworkSSID);
+        return true;
+      }
+    }
+    i++;
+  }
+  return false;
+}
+
+bool connectWiFiNetwork() {
+  // First disconnect from previous WiFi network
+  //WiFi.disconnect(bool wifioff);
+  // Connect to the discovered network
+  //WiFi.begin(selectedWiFiNetworkSSID.c_str());
+  if (strcmp(selectedWiFiNetworkPassword.c_str(), "") == 0) {
+    WiFi.begin(selectedWiFiNetworkSSID.c_str());
+  } else {
+    WiFi.begin(selectedWiFiNetworkSSID.c_str(),selectedWiFiNetworkPassword.c_str());
+  }
+  int connectCounter = 0;
+  Serial.println("Connecting to "+selectedWiFiNetworkSSID);
+  // Wait up to connectCounter (seconds) to connect
+  while ((WiFi.status() != WL_CONNECTED) && (connectCounter < 30)) {
+    Serial.print(".");
+    delay(1000);
+    connectCounter++;
+  }
+  Serial.println("");
+  // if we're connected, return true otherwise false
+  if (WiFi.status() != WL_CONNECTED) {
+    Serial.println("Failed to connect to WiFi");
+    return false;
+  } else {
+    Serial.println("Connected to WiFi");
+    return true;
+  }
+}
+
+void initWiFi() {
+  Serial.println("Initialising WiFi");
   WiFi.mode(WIFI_STA);
+  WiFi.disconnect();
   printMacAddress();
-  Serial.print("Connecting to ");
-  Serial.println(ssid);
-
-
-  if (strcmp (WiFi.SSID().c_str(), ssid) != 0) {
-    //@WiFiChange
-    //WiFi.begin(ssid, password);
-    WiFi.begin(ssid);
-  }
-
-  while (WiFi.status() != WL_CONNECTED) {
-    delay(1000);
-    Serial.print(".");
-  }
-  Serial.println("");
-  Serial.print("WiFi connected with OTA4, IP address: ");
-  Serial.println(WiFi.localIP());
-  //publishDebug("WiFi Connected with OTA on IP: "+WiFi.localIP());
-  Serial.println("About to configure OTA");
-  configureOTA();
-  Serial.println("Configured OTA");
-  Serial.println("Checking WiFi again");
-  while (WiFi.status() != WL_CONNECTED) {
-    delay(1000);
-    Serial.print(".");
-  }
-}
+}
+
+// void init_wifi() {
+//   Serial.println("WiFi ");
+//   //int numOfNetworks = WiFi.scanNetworks(false);
+//
+//   // if (scanWiFiNetworks() == false {
+//   //   //NEXT: We want to sleep for 30 seconds before trying again
+//   //   // but need a timer, so we need to enter the loop. Let things drop out here
+//   //   // to get to the loop and then in the loop always check if "connected" variable
+//   //   // is true before taking a reading.
+//   // }
+//
+//   WiFi.mode(WIFI_STA);
+//   printMacAddress();
+//   Serial.print("Connecting to ");
+//   Serial.println(ssid);
+//
+//
+//   if (strcmp (WiFi.SSID().c_str(), ssid) != 0) {
+//     //@WiFiChange
+//     //WiFi.begin(ssid, password);
+//     WiFi.begin(ssid);
+//   }
+//
+//   while (WiFi.status() != WL_CONNECTED) {
+//     delay(1000);
+//     Serial.print(".");
+//   }
+//   Serial.println("");
+//   Serial.print("WiFi connected with OTA4, IP address: ");
+//   Serial.println(WiFi.localIP());
+//   //publishDebug("WiFi Connected with OTA on IP: "+WiFi.localIP());
+//   Serial.println("About to configure OTA");
+//   configureOTA();
+//   Serial.println("Configured OTA");
+//   Serial.println("Checking WiFi again");
+//   while (WiFi.status() != WL_CONNECTED) {
+//     delay(1000);
+//     Serial.print(".");
+//   }
+// }
 
 
 void connectWithBroker() {
 
-  Serial.println("Calling HTTP first");
-  WiFiClient httpClient;
-
-  while(httpClient.available()){
-      String line = httpClient.readStringUntil('\r');
-      Serial.print(line);
-  }
+  // Serial.println("Calling HTTP first");
+  // WiFiClient httpClient;
+  //
+  // while(httpClient.available()){
+  //     String line = httpClient.readStringUntil('\r');
+  //     Serial.print(line);
+  // }
 
   if (!!!client.connected()) {
-    Serial.print("Reconnecting client to ");
+    Serial.print("Connecting to ");
     Serial.println(server);
-
     int cursorPosition = 0;
     char connectionStateString[20];
     int connectionState;
 
-    Serial.println("Delaying for 10 seconds before trying to connect with Broker");
-    delay(10000);
+    //Serial.println("Delaying for 10 seconds before trying to connect with Broker");
+    delay(1000);
     while (!!!client.connect(clientId, authMethod, token)) {
-
       connectionState = client.state();
       sprintf(connectionStateString,"state=%d",connectionState);
       Serial.println(connectionStateString);

float nappyHumid = 0;
uint16_t volts = 0;
int counter = 0;

const char* ssid = WIFI_SSID;

//@WiFiChange
//const char* password = WIFI_PASSWORD;

char token[] = IOT_TOKEN;
char clientId[] = "d:" IOT_ORG ":" IOT_DEVICE_TYPE ":" IOT_DEVICE_ID;


// MQTT topics

char sensorTopic[] = "iot-2/evt/sensor/fmt/json";
