diff --git a/esp32/include/RunningMedian.h b/esp32/include/RunningMedian.h
new file mode 100644
index 0000000..4ba5516
--- /dev/null
+++ b/esp32/include/RunningMedian.h
@@ -0,0 +1,78 @@
+#pragma once
+//
+//    FILE: RunningMedian.h
+//  AUTHOR: Rob dot Tillaart at gmail dot com
+// PURPOSE: RunningMedian library for Arduino
+// VERSION: 0.2.1
+//     URL: https://github.com/RobTillaart/RunningMedian
+//     URL: http://arduino.cc/playground/Main/RunningMedian
+// HISTORY: See RunningMedian.cpp
+//
+
+#include "Arduino.h"
+
+#define RUNNING_MEDIAN_VERSION "0.2.1"
+
+// prepare for dynamic version
+// not tested ==> use at own risk :)
+// #define RUNNING_MEDIAN_USE_MALLOC
+
+
+// should at least be 5 to be practical,
+// odd sizes results in a 'real' middle element and will be a bit faster.
+// even sizes takes the average of the two middle elements as median
+#define MEDIAN_MIN_SIZE     5
+#define MEDIAN_MAX_SIZE     19
+
+
+class RunningMedian
+{
+public:
+  // # elements in the internal buffer
+  explicit RunningMedian(const uint8_t size);
+  ~RunningMedian();
+
+  // resets internal buffer and var
+  void clear();
+  // adds a new value to internal buffer, optionally replacing the oldest element.
+  void add(const float value);
+  // returns the median == middle element
+  float getMedian();
+
+  // returns average of the values in the internal buffer
+  float getAverage();
+  // returns average of the middle nMedian values, removes noise from outliers
+  float getAverage(uint8_t nMedian);
+
+  float getHighest() { return getSortedElement(_cnt - 1); };
+  float getLowest()  { return getSortedElement(0); };
+
+  // get n'th element from the values in time order
+  float getElement(const uint8_t n);
+  // get n'th element from the values in size order
+  float getSortedElement(const uint8_t n);
+  // predict the max change of median after n additions
+  float predict(const uint8_t n);
+
+  uint8_t getSize() { return _size; };
+  // returns current used elements, getCount() <= getSize()
+  uint8_t getCount() { return _cnt; };
+
+
+protected:
+  boolean _sorted;
+  uint8_t _size;
+  uint8_t _cnt;
+  uint8_t _idx;
+
+#ifdef RUNNING_MEDIAN_USE_MALLOC
+  float * _ar;
+  uint8_t * _p;
+#else
+  float _ar[MEDIAN_MAX_SIZE];
+  uint8_t _p[MEDIAN_MAX_SIZE];
+#endif
+  void sort();
+};
+
+// END OF FILE
\ No newline at end of file
diff --git a/esp32/src/RunningMedian.cpp b/esp32/src/RunningMedian.cpp
new file mode 100644
index 0000000..e18f0e8
--- /dev/null
+++ b/esp32/src/RunningMedian.cpp
@@ -0,0 +1,170 @@
+//
+//    FILE: RunningMedian.cpp
+//  AUTHOR: Rob.Tillaart at gmail.com
+// VERSION: 0.2.1
+// PURPOSE: RunningMedian library for Arduino
+//
+// HISTORY:
+// 0.1.00 - 2011-02-16 initial version
+// 0.1.01 - 2011-02-22 added remarks from CodingBadly
+// 0.1.02 - 2012-03-15 added
+// 0.1.03 - 2013-09-30 added _sorted flag, minor refactor
+// 0.1.04 - 2013-10-17 added getAverage(uint8_t) - kudo's to Sembazuru
+// 0.1.05 - 2013-10-18 fixed bug in sort; removes default constructor; dynamic memory
+// 0.1.06 - 2013-10-19 faster sort, dynamic arrays, replaced sorted float array with indirection array
+// 0.1.07 - 2013-10-19 add correct median if _cnt is even.
+// 0.1.08 - 2013-10-20 add getElement(), add getSottedElement() add predict()
+// 0.1.09 - 2014-11-25 float to double (support ARM)
+// 0.1.10 - 2015-03-07 fix clear
+// 0.1.11 - 2015-03-29 undo 0.1.10 fix clear
+// 0.1.12 - 2015-07-12 refactor constructor + const
+// 0.1.13 - 2015-10-30 fix getElement(n) - kudos to Gdunge
+// 0.1.14 - 2017-07-26 revert double to float - issue #33
+// 0.1.15 - 2018-08-24 make runningMedian Configurable #110
+// 0.2.0    2020-04-16 refactor.
+// 0.2.1    2020-06-19 fix library.json
+
+#include "RunningMedian.h"
+
+RunningMedian::RunningMedian(const uint8_t size)
+{
+  _size = constrain(size, MEDIAN_MIN_SIZE, MEDIAN_MAX_SIZE);
+
+#ifdef RUNNING_MEDIAN_USE_MALLOC
+  _ar = (float *) malloc(_size * sizeof(float));
+  _p = (uint8_t *) malloc(_size * sizeof(uint8_t));
+#endif
+
+  clear();
+}
+
+RunningMedian::~RunningMedian()
+{
+#ifdef RUNNING_MEDIAN_USE_MALLOC
+  free(_ar);
+  free(_p);
+#endif
+}
+
+// resets all counters
+void RunningMedian::clear()
+{
+  _cnt = 0;
+  _idx = 0;
+  _sorted = false;
+  for (uint8_t i = 0; i < _size; i++)
+  {
+    _p[i] = i;
+  }
+}
+
+// adds a new value to the data-set
+// or overwrites the oldest if full.
+void RunningMedian::add(float value)
+{
+  _ar[_idx++] = value;
+  if (_idx >= _size) _idx = 0; // wrap around
+  if (_cnt < _size) _cnt++;
+  _sorted = false;
+}
+
+float RunningMedian::getMedian()
+{
+  if (_cnt == 0) return NAN;
+
+  if (_sorted == false) sort();
+
+  if (_cnt & 0x01)  // is it odd sized?
+  {
+    return _ar[_p[_cnt / 2]];
+  }
+  return (_ar[_p[_cnt / 2]] + _ar[_p[_cnt / 2 - 1]]) / 2;
+}
+
+float RunningMedian::getAverage()
+{
+  if (_cnt == 0) return NAN;
+
+  float sum = 0;
+  for (uint8_t i = 0; i < _cnt; i++)
+  {
+    sum += _ar[i];
+  }
+  return sum / _cnt;
+}
+
+float RunningMedian::getAverage(uint8_t nMedians)
+{
+  if ((_cnt == 0) || (nMedians == 0)) return NAN;
+
+  if (_cnt < nMedians) nMedians = _cnt;     // when filling the array for first time
+  uint8_t start = ((_cnt - nMedians) / 2);
+  uint8_t stop = start + nMedians;
+
+  if (_sorted == false) sort();
+
+  float sum = 0;
+  for (uint8_t i = start; i < stop; i++)
+  {
+    sum += _ar[_p[i]];
+  }
+  return sum / nMedians;
+}
+
+float RunningMedian::getElement(const uint8_t n)
+{
+  if ((_cnt == 0) || (n >= _cnt)) return NAN;
+
+  uint8_t pos = _idx + n;
+  if (pos >= _cnt) // faster than %
+  {
+    pos -= _cnt;
+  }
+  return _ar[pos];
+}
+
+float RunningMedian::getSortedElement(const uint8_t n)
+{
+  if ((_cnt == 0) || (n >= _cnt)) return NAN;
+
+  if (_sorted == false) sort();
+  return _ar[_p[n]];
+}
+
+// n can be max <= half the (filled) size
+float RunningMedian::predict(const uint8_t n)
+{
+  if ((_cnt == 0) || (n >= _cnt / 2)) return NAN;
+
+  float med = getMedian();  // takes care of sorting !
+  if (_cnt & 0x01)
+  {
+    return max(med - _ar[_p[_cnt / 2 - n]], _ar[_p[_cnt / 2 + n]] - med);
+  }
+  float f1 = (_ar[_p[_cnt / 2 - n]] + _ar[_p[_cnt / 2 - n - 1]]) / 2;
+  float f2 = (_ar[_p[_cnt / 2 + n]] + _ar[_p[_cnt / 2 + n - 1]]) / 2;
+  return max(med - f1, f2 - med) / 2;
+}
+
+void RunningMedian::sort()
+{
+  // bubble sort with flag
+  for (uint8_t i = 0; i < _cnt - 1; i++)
+  {
+    bool flag = true;
+    for (uint8_t j = 1; j < _cnt - i; j++)
+    {
+      if (_ar[_p[j - 1]] > _ar[_p[j]])
+      {
+        uint8_t t = _p[j - 1];
+        _p[j - 1] = _p[j];
+        _p[j] = t;
+        flag = false;
+      }
+    }
+    if (flag) break;
+  }
+  _sorted = true;
+}
+
+// -- END OF FILE --
diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp
index 384ef19..c2cebec 100644
--- a/esp32/src/main.cpp
+++ b/esp32/src/main.cpp
@@ -24,7 +24,11 @@ const unsigned long TEMPREADCYCLE = 30000; /**< Check temperature all half minut
 #define TEMP_INIT_VALUE       -999.0f
 #define TEMP_MAX_VALUE        85.0f
 
-RTC_DATA_ATTR bool coldBoot = false;
+RTC_DATA_ATTR bool coldBoot = true;
+bool warmBoot = true;
+bool mode2Active = false;
+bool mode3Active = false;
+
 
 bool mLoopInited = false;
 bool mDeepSleep = false;
@@ -326,10 +330,16 @@ bool switch3Handler(const HomieRange& range, const String& value) {
   return switchGeneralPumpHandler(2, range, value);
 }
 
-
-
 void systemInit(){
-    // Set default values
+  WiFi.mode(WIFI_STA);
+
+  Homie_setFirmware("PlantControl", FIRMWARE_VERSION);
+  Homie.setLoopFunction(loopHandler);
+  Homie.setup();
+
+  mConfigured = Homie.isConfigured();
+
+  // Set default values
   deepSleepTime.setDefaultValue(300000);    /* 5 minutes in milliseconds */
   deepSleepNightTime.setDefaultValue(0);
   wateringDeepSleep.setDefaultValue(60000); /* 1 minute in milliseconds */
@@ -408,18 +418,32 @@ void systemInit(){
 }
 
 
-void mode1(){
+bool mode1(){
+  Serial.println("Init mode 1");
   readSensors();
+  //TODO evaluate if something is to do
+  return false;
 }
 
 void mode2(){
-  WiFi.mode(WIFI_STA);
-  Homie.setup();
+  Serial.println("Init mode 2");
+  mode2Active = true;
+
+
+  systemInit();
+
+  /* Jump into Mode 3, if not configured */
+  if (!mConfigured) {
+    mode2Active = false;
+    mode3Active = true;
+  }
 }
 
 void mode3(){
-  WiFi.mode(WIFI_STA);
-  Homie.setup();
+  Serial.println("Init mode 3");
+  mode3Active = true;
+  
+  systemInit();
 }
 
 /**
@@ -460,19 +484,12 @@ void setup() {
     Serial << "      | Update Limits.hpp : MAX_JSON_CONFIG_FILE_SIZE to 5000" << endl;
   }
 
-  Homie_setFirmware("PlantControl", FIRMWARE_VERSION);
-  Homie.setLoopFunction(loopHandler);
-
-  mConfigured = Homie.isConfigured();
-  systemInit();
-
   esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_PERIPH, ESP_PD_OPTION_OFF);
   esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_SLOW_MEM, ESP_PD_OPTION_ON);
   esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_FAST_MEM, ESP_PD_OPTION_OFF);
   esp_sleep_pd_config(ESP_PD_DOMAIN_XTAL,ESP_PD_OPTION_ON);
 
-
-  mode2();
+  // Big TODO use here the settings in RTC_Memory
 
   // Configure Deep Sleep:
   if (mConfigured && (deepSleepNightTime.get() > 0) &&
@@ -503,50 +520,55 @@ void setup() {
  */
 
 void loop() {
-  if(!coldBoot){
-    coldBoot = true;
+  if(coldBoot){
+    coldBoot = false;
     delay(1000);
-    digitalWrite(OUTPUT_SENSOR, LOW);
-  }
-  if (!mDeepSleep || !mConfigured) {
-    if ((digitalRead(BUTTON) == LOW) && (mButtonClicks % 2) == 0) {
-      mButtonClicks++;
-
-      switch(mButtonClicks) {
-        case 1:
-        case 3:
-        case 5:
-          mode3();
-          break;
-        default:
-            Serial << "No further tests! Please reboot" << endl;
+    if (digitalRead(BUTTON) == LOW){
+      for(int i = 0;i<10;i++){
+        digitalWrite(OUTPUT_SENSOR, LOW);
+        delay(50);
+        digitalWrite(OUTPUT_SENSOR, HIGH);
+        delay(50);
       }
-      Serial.flush();
-    }else if (mButtonClicks > 0 && (digitalRead(BUTTON) == HIGH) && (mButtonClicks % 2) == 1) {
-      Serial << "Self Test Ended" << endl;
-      mButtonClicks++;
-      /* Always reset all outputs */
-      digitalWrite(OUTPUT_SENSOR, LOW);
-      for(int i=0; i < MAX_PLANTS; i++) {
-        digitalWrite(mPlants[i].getPumpPin(), LOW);
-      }
-      digitalWrite(OUTPUT_PUMP4, LOW);
-    } else if (mButtonClicks == 0) {
-      Homie.loop();
-    }
-
-  } else {
-    if (!mAlive) {
-      Serial << (millis()/ 1000) << "s running; sleeeping ..." << endl;
-      Serial.flush();
-      esp_deep_sleep_start();
+      mode3();
+      return;
     } else {
-      mDeepSleep = false;
+      digitalWrite(OUTPUT_SENSOR, LOW);
+    }
+  }
 
-      if (((millis()) % 10000) == 0) {
-        /* tell everybody how long we are awoken */
-        stayAlive.setProperty("alive").send( String(millis()/ 1000) );
+  /* Perform the active modes (non mode1) */
+  if (mode3Active || mode2Active) {
+    Homie.loop();
+    if(!mode3Active){
+      /* Upgrade to mode 3 via reset */
+      if (digitalRead(BUTTON) == LOW){
+        coldBoot=true;
+        ESP.restart();
       }
     }
+  } else {
+    /* Check which mode shall be selected */
+    if(warmBoot){
+      warmBoot = false;
+      if(mode1()){
+        mode2();
+      } else {
+        /* Upgrade to mode 3 via reset */
+        if (digitalRead(BUTTON) == LOW){
+          coldBoot=true;
+          ESP.restart();
+        }
+        Serial.println("Nothing to do back to sleep");
+        Serial.flush();
+        esp_deep_sleep_start();
+      }
+    }
+  }
+
+  if(millis() > 30000 && !mode3Active){
+    Serial << (millis()/ 1000) << "s running; going to suicide ..." << endl;
+    Serial.flush();
+    esp_deep_sleep_start();
   }
 }