diff --git a/Makefile b/Makefile
index 451d7f3..113944c 100644
--- a/Makefile
+++ b/Makefile
@@ -10,6 +10,7 @@
 #
 # Makefile heavily adapted to esp-link and wireless flashing by Thorsten von Eicken
 # Original from esphttpd and others...
+#VERBOSE=1
 
 # --------------- toolchain configuration ---------------
 
@@ -128,6 +129,8 @@ GZIP_COMPRESSION ?= yes
 COMPRESS_W_YUI ?= yes
 YUI-COMPRESSOR ?= yuicompressor-2.4.8.jar
 
+# Optional Modules
+MODULES ?= rest
 
 # -------------- End of config options -------------
 
@@ -142,15 +145,29 @@ TARGET		= httpd
 # espressif tool to concatenate sections for OTA upload using bootloader v1.2+
 APPGEN_TOOL	?= gen_appbin.py
 
+CFLAGS=
+
+# set defines for optional modules
+ifneq (,$(findstring mqtt,$(MODULES)))
+	CFLAGS		+= -DMQTT
+endif
+
+ifneq (,$(findstring rest,$(MODULES)))
+	CFLAGS		+= -DREST
+endif
+
 # which modules (subdirectories) of the project to include in compiling
-MODULES		= espfs httpd user serial cmd mqtt esp-link
+LIBRARIES_DIR	= libraries
+MODULES			+= espfs httpd user serial cmd esp-link
+MODULES			+= $(foreach sdir,$(LIBRARIES_DIR),$(wildcard $(sdir)/*))
+EXTRA_INCDIR	=include .
 EXTRA_INCDIR	= include .
 
 # libraries used in this project, mainly provided by the SDK
 LIBS		= c gcc hal phy pp net80211 wpa main lwip
 
 # compiler flags using during compilation of source files
-CFLAGS		= -Os -ggdb -std=c99 -Werror -Wpointer-arith -Wundef -Wall -Wl,-EL -fno-inline-functions \
+CFLAGS		+= -Os -ggdb -std=c99 -Werror -Wpointer-arith -Wundef -Wall -Wl,-EL -fno-inline-functions \
 		-nostdlib -mlongcalls -mtext-section-literals -ffunction-sections -fdata-sections \
 		-D__ets__ -DICACHE_FLASH -D_STDINT_H -Wno-address -DFIRMWARE_SIZE=$(ESP_FLASH_MAX) \
 		-DMCU_RESET_PIN=$(MCU_RESET_PIN) -DMCU_ISP_PIN=$(MCU_ISP_PIN) \
@@ -167,7 +184,7 @@ LD_SCRIPT2	:= build/eagle.esphttpd2.v6.ld
 
 # various paths from the SDK used in this project
 SDK_LIBDIR		= lib
-SDK_LDDIR			= ld
+SDK_LDDIR		= ld
 SDK_INCDIR		= include include/json
 SDK_TOOLSDIR	= tools
 
@@ -175,8 +192,8 @@ SDK_TOOLSDIR	= tools
 CC		:= $(XTENSA_TOOLS_ROOT)xtensa-lx106-elf-gcc
 AR		:= $(XTENSA_TOOLS_ROOT)xtensa-lx106-elf-ar
 LD		:= $(XTENSA_TOOLS_ROOT)xtensa-lx106-elf-gcc
-OBJCP := $(XTENSA_TOOLS_ROOT)xtensa-lx106-elf-objcopy
-OBJDP := $(XTENSA_TOOLS_ROOT)xtensa-lx106-elf-objdump
+OBJCP	:= $(XTENSA_TOOLS_ROOT)xtensa-lx106-elf-objcopy
+OBJDP	:= $(XTENSA_TOOLS_ROOT)xtensa-lx106-elf-objdump
 
 
 ####
@@ -214,7 +231,7 @@ CFLAGS		+= -DGZIP_COMPRESSION
 endif
 
 ifeq ("$(CHANGE_TO_STA)","yes")
-CFLAGS          += -DCHANGE_TO_STA
+CFLAGS		+= -DCHANGE_TO_STA
 endif
 
 vpath %.c $(SRC_DIR)
diff --git a/cmd/cmd.h b/cmd/cmd.h
index 97c8236..2d07e1c 100644
--- a/cmd/cmd.h
+++ b/cmd/cmd.h
@@ -55,8 +55,8 @@ typedef enum {
   CMD_REST_REQUEST,
   CMD_REST_SETHEADER,
   CMD_REST_EVENTS,
-  CMD_ADD_CALLBACK,   // 15
-  CMD_SENSOR_EVENTS
+  CMD_CB_ADD,         // 15
+  CMD_CB_EVENTS
 } CmdName;
 
 typedef uint32_t (*cmdfunc_t)(CmdPacket *cmd);
diff --git a/cmd/handlers.c b/cmd/handlers.c
index b54773f..af6eab7 100644
--- a/cmd/handlers.c
+++ b/cmd/handlers.c
@@ -4,12 +4,13 @@
 
 #include "esp8266.h"
 #include "cmd.h"
-#include "rest.h"
-#include "crc16.h"
-#include "serbridge.h"
-#include "uart.h"
-#include "cgiwifi.h"
-#include "mqtt_cmd.h"
+#include <cgiwifi.h>
+#ifdef MQTT
+#include <mqtt_cmd.h>
+#endif
+#ifdef REST
+#include <rest.h>
+#endif
 
 static uint32_t CMD_Null(CmdPacket *cmd);
 static uint32_t CMD_IsReady(CmdPacket *cmd);
@@ -27,22 +28,20 @@ const CmdList commands[] = {
   {CMD_RESET,           CMD_Reset},
   {CMD_IS_READY,        CMD_IsReady},
   {CMD_WIFI_CONNECT,    CMD_WifiConnect},
-
-/*
-  {CMD_MQTT_SETUP,      MQTTAPP_Setup},
-  {CMD_MQTT_CONNECT,    MQTTAPP_Connect},
-  {CMD_MQTT_DISCONNECT, MQTTAPP_Disconnect},
-  {CMD_MQTT_PUBLISH,    MQTTAPP_Publish},
-  {CMD_MQTT_SUBSCRIBE , MQTTAPP_Subscribe},
-  {CMD_MQTT_LWT,        MQTTAPP_Lwt},
-  */
-
+#ifdef MQTT
+  {CMD_MQTT_SETUP,      MQTTCMD_Setup},
+  {CMD_MQTT_CONNECT,    MQTTCMD_Connect},
+  {CMD_MQTT_DISCONNECT, MQTTCMD_Disconnect},
+  {CMD_MQTT_PUBLISH,    MQTTCMD_Publish},
+  {CMD_MQTT_SUBSCRIBE , MQTTCMD_Subscribe},
+  {CMD_MQTT_LWT,        MQTTCMD_Lwt},
+#endif
+#ifdef REST
   {CMD_REST_SETUP,      REST_Setup},
   {CMD_REST_REQUEST,    REST_Request},
   {CMD_REST_SETHEADER,  REST_SetHeader},
-
-  {CMD_ADD_CALLBACK,    CMD_AddCallback },
-
+#endif
+  { CMD_CB_ADD,         CMD_AddCallback },
   {CMD_NULL,            NULL}
 };
 
diff --git a/esp-link.vcxproj b/esp-link.vcxproj
index f371f1a..72b0ea6 100644
--- a/esp-link.vcxproj
+++ b/esp-link.vcxproj
@@ -65,8 +65,8 @@
   <ItemGroup>
     <ClCompile Include="cmd\cmd.c" />
     <ClCompile Include="cmd\handlers.c" />
-    <ClCompile Include="cmd\mqtt_cmd.c" />
-    <ClCompile Include="cmd\rest.c" />
+    <ClCompile Include="mqtt\mqtt_cmd.c" />
+    <ClCompile Include="rest\rest.c" />
     <ClCompile Include="cmd\tcpclient.c" />
     <ClCompile Include="espfs\espfs.c" />
     <ClCompile Include="espfs\mkespfsimage\main.c" />
@@ -108,8 +108,8 @@
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="cmd\cmd.h" />
-    <ClInclude Include="cmd\mqtt_cmd.h" />
-    <ClInclude Include="cmd\rest.h" />
+    <ClInclude Include="mqtt\mqtt_cmd.h" />
+    <ClInclude Include="rest\rest.h" />
     <ClInclude Include="cmd\tcpclient.h" />
     <ClInclude Include="espfs\espfs.h" />
     <ClInclude Include="espfs\espfsformat.h" />
diff --git a/esp-link/cgi.c b/esp-link/cgi.c
index 8f87d38..65cab94 100644
--- a/esp-link/cgi.c
+++ b/esp-link/cgi.c
@@ -12,11 +12,7 @@ Some random cgi routines.
  * Heavily modified and enhanced by Thorsten von Eicken in 2015
  * ----------------------------------------------------------------------------
  */
-
-
-#include <esp8266.h>
 #include "cgi.h"
-#include "espfs.h"
 
 void ICACHE_FLASH_ATTR
 jsonHeader(HttpdConnData *connData, int code) {
@@ -28,6 +24,44 @@ jsonHeader(HttpdConnData *connData, int code) {
 	httpdEndHeaders(connData);
 }
 
+uint8_t ICACHE_FLASH_ATTR
+UTILS_StrToIP(const char* str, void *ip){
+  /* The count of the number of bytes processed. */
+  int i;
+  /* A pointer to the next digit to process. */
+  const char * start;
+
+  start = str;
+  for (i = 0; i < 4; i++) {
+    /* The digit being processed. */
+    char c;
+    /* The value of this byte. */
+    int n = 0;
+    while (1) {
+      c = *start;
+      start++;
+      if (c >= '0' && c <= '9') {
+        n *= 10;
+        n += c - '0';
+      }
+      /* We insist on stopping at "." if we are still parsing
+      the first, second, or third numbers. If we have reached
+      the end of the numbers, we will allow any character. */
+      else if ((i < 3 && c == '.') || i == 3) {
+        break;
+      }
+      else {
+        return 0;
+      }
+    }
+    if (n >= 256) {
+      return 0;
+    }
+    ((uint8_t*)ip)[i] = n;
+  }
+  return 1;
+}
+
 #define TOKEN(x) (os_strcmp(token, x) == 0)
 #if 0
 // Handle system information variables and print their value, returns the number of
diff --git a/esp-link/cgi.h b/esp-link/cgi.h
index b5690cf..200a176 100644
--- a/esp-link/cgi.h
+++ b/esp-link/cgi.h
@@ -1,9 +1,11 @@
 #ifndef CGI_H
 #define CGI_H
 
+#include <esp8266.h>
 #include "httpd.h"
 
 void jsonHeader(HttpdConnData *connData, int code);
 int cgiMenu(HttpdConnData *connData);
+uint8_t UTILS_StrToIP(const char* str, void *ip);
 
 #endif
diff --git a/include/esp8266.h b/include/esp8266.h
index c3d0c40..3e7eeb9 100644
--- a/include/esp8266.h
+++ b/include/esp8266.h
@@ -1,4 +1,7 @@
 // Combined include file for esp8266
+#ifndef _ESP8266_H_
+#define _ESP8266_H_
+
 #include <user_config.h>
 #include <ctype.h>
 #include <stdio.h>
@@ -16,10 +19,9 @@
 
 #include "espmissingincludes.h"
 #include "uart_hw.h"
-extern char* esp_link_version;
-
-void ICACHE_FLASH_ATTR init(void);
 
 #ifdef __WIN32__
 #include <_mingw.h>
 #endif
+
+#endif // _ESP8266_H_
\ No newline at end of file
diff --git a/include/user_config.h b/include/user_config.h
index 7bdfd38..3be06f4 100644
--- a/include/user_config.h
+++ b/include/user_config.h
@@ -1,5 +1,9 @@
 #ifndef _USER_CONFIG_H_
 #define _USER_CONFIG_H_
+#include <c_types.h>
+#ifdef __WIN32__
+#include <_mingw.h>
+#endif
 
 #define MQTT_RECONNECT_TIMEOUT 	5	// seconds
 #define MQTT_BUF_SIZE		1024
@@ -17,4 +21,10 @@
 #define PROTOCOL_NAMEv31	// MQTT version 3.1 compatible with Mosquitto v0.15
 //PROTOCOL_NAMEv311			// MQTT version 3.11 compatible with https://eclipse.org/paho/clients/testing/
 
+extern char* esp_link_version;
+
+extern uint8_t UTILS_StrToIP(const char* str, void *ip);
+
+extern void ICACHE_FLASH_ATTR init(void);
+
 #endif
\ No newline at end of file
diff --git a/mqtt/mqtt.h b/mqtt/mqtt.h
index 9ad6825..00c432a 100644
--- a/mqtt/mqtt.h
+++ b/mqtt/mqtt.h
@@ -33,7 +33,6 @@
 #include <esp8266.h>
 #include "mqtt_msg.h"
 #include "queue.h"
-#include <rest.h>
 
 typedef struct mqtt_event_data_t {
   uint8_t type;
diff --git a/cmd/mqtt_cmd.c b/mqtt/mqtt_cmd.c
similarity index 100%
rename from cmd/mqtt_cmd.c
rename to mqtt/mqtt_cmd.c
diff --git a/cmd/mqtt_cmd.h b/mqtt/mqtt_cmd.h
similarity index 100%
rename from cmd/mqtt_cmd.h
rename to mqtt/mqtt_cmd.h
diff --git a/cmd/rest.c b/rest/rest.c
similarity index 93%
rename from cmd/rest.c
rename to rest/rest.c
index 3556681..a9cd298 100644
--- a/cmd/rest.c
+++ b/rest/rest.c
@@ -392,42 +392,3 @@ fail:
   os_printf("\n");
   return 0;
 }
-
-uint8_t ICACHE_FLASH_ATTR
-UTILS_StrToIP(const char* str, void *ip)
-{
-  /* The count of the number of bytes processed. */
-  int i;
-  /* A pointer to the next digit to process. */
-  const char * start;
-
-  start = str;
-  for (i = 0; i < 4; i++) {
-    /* The digit being processed. */
-    char c;
-    /* The value of this byte. */
-    int n = 0;
-    while (1) {
-      c = * start;
-      start++;
-      if (c >= '0' && c <= '9') {
-        n *= 10;
-        n += c - '0';
-      }
-      /* We insist on stopping at "." if we are still parsing
-         the first, second, or third numbers. If we have reached
-         the end of the numbers, we will allow any character. */
-      else if ((i < 3 && c == '.') || i == 3) {
-        break;
-      }
-      else {
-        return 0;
-      }
-    }
-    if (n >= 256) {
-      return 0;
-    }
-    ((uint8_t*)ip)[i] = n;
-  }
-  return 1;
-}
diff --git a/cmd/rest.h b/rest/rest.h
similarity index 93%
rename from cmd/rest.h
rename to rest/rest.h
index 9c6cd5c..7161e1f 100644
--- a/cmd/rest.h
+++ b/rest/rest.h
@@ -36,6 +36,5 @@ typedef struct {
 uint32_t REST_Setup(CmdPacket *cmd);
 uint32_t REST_Request(CmdPacket *cmd);
 uint32_t REST_SetHeader(CmdPacket *cmd);
-uint8_t UTILS_StrToIP(const char* str, void *ip);
 
 #endif /* MODULES_INCLUDE_API_H_ */
diff --git a/user/user_main.c b/user/user_main.c
index 5738807..e75876b 100644
--- a/user/user_main.c
+++ b/user/user_main.c
@@ -1,70 +1,70 @@
 #include <esp8266.h>
-#include <mqtt.h>
-#include <cgiwifi.h>
+//#include <mqtt.h>
+//#include <cgiwifi.h>
 
-MQTT_Client mqttClient;
-
-void ICACHE_FLASH_ATTR 
-mqttConnectedCb(uint32_t *args) {
-  MQTT_Client* client = (MQTT_Client*)args;
-  MQTT_Publish(client, "announce/all", "Hello World!", 0, 0);
-}
-
-void ICACHE_FLASH_ATTR 
-mqttDisconnectedCb(uint32_t *args) {
-//  MQTT_Client* client = (MQTT_Client*)args;
-  os_printf("MQTT Disconnected\n");
-}
-
-void ICACHE_FLASH_ATTR 
-mqttTcpDisconnectedCb(uint32_t *args) {
+//MQTT_Client mqttClient;
+//
+//void ICACHE_FLASH_ATTR 
+//mqttConnectedCb(uint32_t *args) {
 //  MQTT_Client* client = (MQTT_Client*)args;
-  os_printf("MQTT TCP Disconnected\n");
-}
-
-void ICACHE_FLASH_ATTR 
-mqttPublishedCb(uint32_t *args) {
-//  MQTT_Client* client = (MQTT_Client*)args;
-  os_printf("MQTT Published\n");
-}
-
-void ICACHE_FLASH_ATTR 
-mqttDataCb(uint32_t *args, const char* topic, uint32_t topic_len, const char *data, uint32_t data_len) {
-  char *topicBuf = (char*)os_zalloc(topic_len + 1);
-  char *dataBuf = (char*)os_zalloc(data_len + 1);
-
-//  MQTT_Client* client = (MQTT_Client*)args;
-
-  os_memcpy(topicBuf, topic, topic_len);
-  topicBuf[topic_len] = 0;
-
-  os_memcpy(dataBuf, data, data_len);
-  dataBuf[data_len] = 0;
-
-  os_printf("Receive topic: %s, data: %s\n", topicBuf, dataBuf);
-  os_free(topicBuf);
-  os_free(dataBuf);
-}
-
-void ICACHE_FLASH_ATTR 
-wifiStateChangeCb(uint8_t status)
-{
-  if (status == wifiGotIP  && mqttClient.connState != TCP_CONNECTING){
-    MQTT_Connect(&mqttClient);
-  }
-  else if (status == wifiIsDisconnected && mqttClient.connState == TCP_CONNECTING){    
-    MQTT_Disconnect(&mqttClient);
-  }
-}
+//  MQTT_Publish(client, "announce/all", "Hello World!", 0, 0);
+//}
+//
+//void ICACHE_FLASH_ATTR 
+//mqttDisconnectedCb(uint32_t *args) {
+////  MQTT_Client* client = (MQTT_Client*)args;
+//  os_printf("MQTT Disconnected\n");
+//}
+//
+//void ICACHE_FLASH_ATTR 
+//mqttTcpDisconnectedCb(uint32_t *args) {
+////  MQTT_Client* client = (MQTT_Client*)args;
+//  os_printf("MQTT TCP Disconnected\n");
+//}
+//
+//void ICACHE_FLASH_ATTR 
+//mqttPublishedCb(uint32_t *args) {
+////  MQTT_Client* client = (MQTT_Client*)args;
+//  os_printf("MQTT Published\n");
+//}
+//
+//void ICACHE_FLASH_ATTR 
+//mqttDataCb(uint32_t *args, const char* topic, uint32_t topic_len, const char *data, uint32_t data_len) {
+//  char *topicBuf = (char*)os_zalloc(topic_len + 1);
+//  char *dataBuf = (char*)os_zalloc(data_len + 1);
+//
+////  MQTT_Client* client = (MQTT_Client*)args;
+//
+//  os_memcpy(topicBuf, topic, topic_len);
+//  topicBuf[topic_len] = 0;
+//
+//  os_memcpy(dataBuf, data, data_len);
+//  dataBuf[data_len] = 0;
+//
+//  os_printf("Receive topic: %s, data: %s\n", topicBuf, dataBuf);
+//  os_free(topicBuf);
+//  os_free(dataBuf);
+//}
+//
+//void ICACHE_FLASH_ATTR 
+//wifiStateChangeCb(uint8_t status)
+//{
+//  if (status == wifiGotIP  && mqttClient.connState != TCP_CONNECTING){
+//    MQTT_Connect(&mqttClient);
+//  }
+//  else if (status == wifiIsDisconnected && mqttClient.connState == TCP_CONNECTING){    
+//    MQTT_Disconnect(&mqttClient);
+//  }
+//}
 
 void init() {
-  wifiAddStateChangeCb(wifiStateChangeCb);
-  MQTT_InitConnection(&mqttClient, MQTT_HOST, MQTT_PORT, MQTT_SECURITY);
-  MQTT_InitClient(&mqttClient, MQTT_CLIENT_ID, MQTT_USER, MQTT_PASS, MQTT_KEEPALIVE, MQTT_CLSESSION);
-  MQTT_InitLWT(&mqttClient, "/lwt", "offline", 0, 0);
-  MQTT_OnConnected(&mqttClient, mqttConnectedCb);
-  MQTT_OnDisconnected(&mqttClient, mqttDisconnectedCb);
-  MQTT_OnDisconnected(&mqttClient, mqttTcpDisconnectedCb);
-  MQTT_OnPublished(&mqttClient, mqttPublishedCb);
-  MQTT_OnData(&mqttClient, mqttDataCb);
+//  wifiAddStateChangeCb(wifiStateChangeCb);
+//  MQTT_InitConnection(&mqttClient, MQTT_HOST, MQTT_PORT, MQTT_SECURITY);
+//  MQTT_InitClient(&mqttClient, MQTT_CLIENT_ID, MQTT_USER, MQTT_PASS, MQTT_KEEPALIVE, MQTT_CLSESSION);
+//  MQTT_InitLWT(&mqttClient, "/lwt", "offline", 0, 0);
+//  MQTT_OnConnected(&mqttClient, mqttConnectedCb);
+//  MQTT_OnDisconnected(&mqttClient, mqttDisconnectedCb);
+//  MQTT_OnDisconnected(&mqttClient, mqttTcpDisconnectedCb);
+//  MQTT_OnPublished(&mqttClient, mqttPublishedCb);
+//  MQTT_OnData(&mqttClient, mqttDataCb);
 }
\ No newline at end of file