diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index cd729921..c72f7c9c 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -750,8 +750,8 @@ add_definitions(-DNIMBLE_CFG_CONTROLLER)
 add_definitions(-DOS_CPUTIME_FREQ)
 add_definitions(-DNRF52 -DNRF52832 -DNRF52832_XXAA -DNRF52_PAN_74 -DNRF52_PAN_64 -DNRF52_PAN_12 -DNRF52_PAN_58 -DNRF52_PAN_54 -DNRF52_PAN_31 -DNRF52_PAN_51 -DNRF52_PAN_36 -DNRF52_PAN_15 -DNRF52_PAN_20 -DNRF52_PAN_55 -DBOARD_PCA10040)
 add_definitions(-DFREERTOS)
-add_definitions(-D__STACK_SIZE=8192)
-add_definitions(-D__HEAP_SIZE=8192)
+add_definitions(-D__STACK_SIZE=1024)
+add_definitions(-D__HEAP_SIZE=4096)
 
 # NOTE : Add the following defines to enable debug mode of the NRF SDK:
 #add_definitions(-DDEBUG)
diff --git a/src/FreeRTOSConfig.h b/src/FreeRTOSConfig.h
index 15185766..07c152dc 100644
--- a/src/FreeRTOSConfig.h
+++ b/src/FreeRTOSConfig.h
@@ -62,7 +62,7 @@
 #define configTICK_RATE_HZ                      1024
 #define configMAX_PRIORITIES                    (3)
 #define configMINIMAL_STACK_SIZE                (120)
-#define configTOTAL_HEAP_SIZE                   (1024 * 16)
+#define configTOTAL_HEAP_SIZE                   (1024 * 17)
 #define configMAX_TASK_NAME_LEN                 (4)
 #define configUSE_16_BIT_TICKS                  0
 #define configIDLE_SHOULD_YIELD                 1
diff --git a/src/components/ble/AlertNotificationClient.cpp b/src/components/ble/AlertNotificationClient.cpp
index 6043a129..c3d1d69a 100644
--- a/src/components/ble/AlertNotificationClient.cpp
+++ b/src/components/ble/AlertNotificationClient.cpp
@@ -159,7 +159,7 @@ void AlertNotificationClient::OnNotification(ble_gap_event* event) {
     notif.category = Pinetime::Controllers::NotificationManager::Categories::SimpleAlert;
     notificationManager.Push(std::move(notif));
 
-    systemTask.PushMessage(Pinetime::System::SystemTask::Messages::OnNewNotification);
+    systemTask.PushMessage(Pinetime::System::Messages::OnNewNotification);
   }
 }
 
diff --git a/src/components/ble/AlertNotificationService.cpp b/src/components/ble/AlertNotificationService.cpp
index e9f5941e..d5fc7f65 100644
--- a/src/components/ble/AlertNotificationService.cpp
+++ b/src/components/ble/AlertNotificationService.cpp
@@ -79,7 +79,7 @@ int AlertNotificationService::OnAlert(uint16_t conn_handle, uint16_t attr_handle
         break;
     }
 
-    auto event = Pinetime::System::SystemTask::Messages::OnNewNotification;
+    auto event = Pinetime::System::Messages::OnNewNotification;
     notificationManager.Push(std::move(notif));
     systemTask.PushMessage(event);
   }
diff --git a/src/components/ble/DfuService.cpp b/src/components/ble/DfuService.cpp
index 2031668e..cec194cc 100644
--- a/src/components/ble/DfuService.cpp
+++ b/src/components/ble/DfuService.cpp
@@ -205,7 +205,7 @@ int DfuService::ControlPointHandler(uint16_t connectionHandle, os_mbuf* om) {
         bleController.State(Pinetime::Controllers::Ble::FirmwareUpdateStates::Running);
         bleController.FirmwareUpdateTotalBytes(0xffffffffu);
         bleController.FirmwareUpdateCurrentBytes(0);
-        systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateStarted);
+        systemTask.PushMessage(Pinetime::System::Messages::BleFirmwareUpdateStarted);
         return 0;
       } else {
         NRF_LOG_INFO("[DFU] -> Start DFU, mode %d not supported!", imageType);
@@ -279,7 +279,7 @@ int DfuService::ControlPointHandler(uint16_t connectionHandle, os_mbuf* om) {
       }
       NRF_LOG_INFO("[DFU] -> Activate image and reset!");
       bleController.StopFirmwareUpdate();
-      systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateFinished);
+      systemTask.PushMessage(Pinetime::System::Messages::BleFirmwareUpdateFinished);
       Reset();
       bleController.State(Pinetime::Controllers::Ble::FirmwareUpdateStates::Validated);
       return 0;
@@ -304,7 +304,7 @@ void DfuService::Reset() {
   notificationManager.Reset();
   bleController.State(Pinetime::Controllers::Ble::FirmwareUpdateStates::Error);
   bleController.StopFirmwareUpdate();
-  systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateFinished);
+  systemTask.PushMessage(Pinetime::System::Messages::BleFirmwareUpdateFinished);
 }
 
 DfuService::NotificationManager::NotificationManager() {
diff --git a/src/components/ble/ImmediateAlertService.cpp b/src/components/ble/ImmediateAlertService.cpp
index fd6430af..820d3b6e 100644
--- a/src/components/ble/ImmediateAlertService.cpp
+++ b/src/components/ble/ImmediateAlertService.cpp
@@ -67,7 +67,7 @@ int ImmediateAlertService::OnAlertLevelChanged(uint16_t connectionHandle, uint16
       notif.category = Pinetime::Controllers::NotificationManager::Categories::SimpleAlert;
       notificationManager.Push(std::move(notif));
 
-      systemTask.PushMessage(Pinetime::System::SystemTask::Messages::OnNewNotification);
+      systemTask.PushMessage(Pinetime::System::Messages::OnNewNotification);
     }
   }
 
diff --git a/src/components/ble/NimbleController.cpp b/src/components/ble/NimbleController.cpp
index 67a6d691..2c1d0f99 100644
--- a/src/components/ble/NimbleController.cpp
+++ b/src/components/ble/NimbleController.cpp
@@ -149,7 +149,7 @@ int NimbleController::OnGAPEvent(ble_gap_event* event) {
         bleController.Disconnect();
       } else {
         bleController.Connect();
-        systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleConnected);
+        systemTask.PushMessage(Pinetime::System::Messages::BleConnected);
         connectionHandle = event->connect.conn_handle;
         // Service discovery is deffered via systemtask
       }
diff --git a/src/components/datetime/DateTimeController.cpp b/src/components/datetime/DateTimeController.cpp
index 4f9302eb..28a70abc 100644
--- a/src/components/datetime/DateTimeController.cpp
+++ b/src/components/datetime/DateTimeController.cpp
@@ -5,9 +5,6 @@
 
 using namespace Pinetime::Controllers;
 
-DateTime::DateTime(System::SystemTask& systemTask) : systemTask {systemTask} {
-}
-
 void DateTime::SetTime(
   uint16_t year, uint8_t month, uint8_t day, uint8_t dayOfWeek, uint8_t hour, uint8_t minute, uint8_t second, uint32_t systickCounter) {
   std::tm tm = {
@@ -70,7 +67,8 @@ void DateTime::UpdateTime(uint32_t systickCounter) {
   // Notify new day to SystemTask
   if (hour == 0 and not isMidnightAlreadyNotified) {
     isMidnightAlreadyNotified = true;
-    systemTask.PushMessage(System::SystemTask::Messages::OnNewDay);
+    if(systemTask != nullptr)
+      systemTask->PushMessage(System::Messages::OnNewDay);
   } else if (hour != 0) {
     isMidnightAlreadyNotified = false;
   }
@@ -104,6 +102,10 @@ const char* DateTime::DayOfWeekShortToStringLow() {
   return DateTime::DaysStringShortLow[(uint8_t) dayOfWeek];
 }
 
+void DateTime::Register(Pinetime::System::SystemTask* systemTask) {
+  this->systemTask = systemTask;
+}
+
 char const* DateTime::DaysStringLow[] = {"--", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday", "Sunday"};
 
 char const* DateTime::DaysStringShortLow[] = {"--", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat", "Sun"};
diff --git a/src/components/datetime/DateTimeController.h b/src/components/datetime/DateTimeController.h
index d0ae727e..265d6e9d 100644
--- a/src/components/datetime/DateTimeController.h
+++ b/src/components/datetime/DateTimeController.h
@@ -27,8 +27,6 @@ namespace Pinetime {
         December
       };
 
-      DateTime(System::SystemTask& systemTask);
-
       void SetTime(uint16_t year,
                    uint8_t month,
                    uint8_t day,
@@ -75,8 +73,9 @@ namespace Pinetime {
         return uptime;
       }
 
+      void Register(System::SystemTask* systemTask);
+
     private:
-      System::SystemTask& systemTask;
       uint16_t year = 0;
       Months month = Months::Unknown;
       uint8_t day = 0;
@@ -90,6 +89,7 @@ namespace Pinetime {
       std::chrono::seconds uptime {0};
 
       bool isMidnightAlreadyNotified = false;
+      System::SystemTask* systemTask = nullptr;
 
       static char const* DaysString[];
       static char const* DaysStringShort[];
diff --git a/src/components/heartrate/HeartRateController.cpp b/src/components/heartrate/HeartRateController.cpp
index e84d665a..716813b3 100644
--- a/src/components/heartrate/HeartRateController.cpp
+++ b/src/components/heartrate/HeartRateController.cpp
@@ -4,9 +4,6 @@
 
 using namespace Pinetime::Controllers;
 
-HeartRateController::HeartRateController(Pinetime::System::SystemTask& systemTask) : systemTask {systemTask} {
-}
-
 void HeartRateController::Update(HeartRateController::States newState, uint8_t heartRate) {
   this->state = newState;
   if (this->heartRate != heartRate) {
diff --git a/src/components/heartrate/HeartRateController.h b/src/components/heartrate/HeartRateController.h
index d3a8460d..a63f1a70 100644
--- a/src/components/heartrate/HeartRateController.h
+++ b/src/components/heartrate/HeartRateController.h
@@ -15,8 +15,7 @@ namespace Pinetime {
     public:
       enum class States { Stopped, NotEnoughData, NoTouch, Running };
 
-      explicit HeartRateController(System::SystemTask& systemTask);
-
+      HeartRateController() = default;
       void Start();
       void Stop();
       void Update(States newState, uint8_t heartRate);
@@ -32,7 +31,6 @@ namespace Pinetime {
       void SetService(Pinetime::Controllers::HeartRateService* service);
 
     private:
-      System::SystemTask& systemTask;
       Applications::HeartRateTask* task = nullptr;
       States state = States::Stopped;
       uint8_t heartRate = 0;
diff --git a/src/components/heartrate/Ppg.cpp b/src/components/heartrate/Ppg.cpp
index da0789e0..fcba3815 100644
--- a/src/components/heartrate/Ppg.cpp
+++ b/src/components/heartrate/Ppg.cpp
@@ -38,9 +38,8 @@ namespace {
   }
 }
 
-Ppg::Ppg(float spl)
-  : offset {spl},
-    hpf {0.87033078, -1.74066156, 0.87033078, -1.72377617, 0.75754694},
+Ppg::Ppg()
+  : hpf {0.87033078, -1.74066156, 0.87033078, -1.72377617, 0.75754694},
     agc {20, 0.971, 2},
     lpf {0.11595249, 0.23190498, 0.11595249, -0.72168143, 0.18549138} {
 }
@@ -67,13 +66,7 @@ float Ppg::HeartRate() {
   dataIndex = 0;
   return hr;
 }
-
-int cccount = 0;
 float Ppg::ProcessHeartRate() {
-
-  if (cccount > 2)
-    asm("nop");
-  cccount++;
   auto t0 = Trough(data.data(), dataIndex, 7, 48);
   if (t0 < 0)
     return 0;
diff --git a/src/components/heartrate/Ppg.h b/src/components/heartrate/Ppg.h
index ee07dfcf..00014162 100644
--- a/src/components/heartrate/Ppg.h
+++ b/src/components/heartrate/Ppg.h
@@ -8,8 +8,7 @@ namespace Pinetime {
   namespace Controllers {
     class Ppg {
     public:
-      explicit Ppg(float spl);
-
+      Ppg();
       int8_t Preprocess(float spl);
       float HeartRate();
 
diff --git a/src/components/timer/TimerController.cpp b/src/components/timer/TimerController.cpp
index 3b25901c..8d5f5c33 100644
--- a/src/components/timer/TimerController.cpp
+++ b/src/components/timer/TimerController.cpp
@@ -12,14 +12,17 @@ using namespace Pinetime::Controllers;
 
 APP_TIMER_DEF(timerAppTimer);
 
-
-TimerController::TimerController(System::SystemTask& systemTask) : systemTask{systemTask} {
+namespace {
+  void TimerEnd(void* p_context) {
+    auto* controller = static_cast<Pinetime::Controllers::TimerController*> (p_context);
+    if(controller != nullptr)
+      controller->OnTimerEnd();
+  }
 }
 
 
 void TimerController::Init() {
-  app_timer_create(&timerAppTimer, APP_TIMER_MODE_SINGLE_SHOT, timerEnd);
-  
+  app_timer_create(&timerAppTimer, APP_TIMER_MODE_SINGLE_SHOT, TimerEnd);
 }
 
 void TimerController::StartTimer(uint32_t duration) {
@@ -47,13 +50,6 @@ uint32_t TimerController::GetTimeRemaining() {
   return (static_cast<TickType_t>(deltaTicks) / static_cast<TickType_t>(configTICK_RATE_HZ)) * 1000;
 }
 
-void TimerController::timerEnd(void* p_context) {
-  
-  auto* controller = static_cast<Controllers::TimerController*> (p_context);
-  controller->timerRunning = false;
-  controller->systemTask.PushMessage(System::SystemTask::Messages::OnTimerDone);
-}
-
 void TimerController::StopTimer() {
   app_timer_stop(timerAppTimer);
   timerRunning = false;
@@ -61,4 +57,13 @@ void TimerController::StopTimer() {
 
 bool TimerController::IsRunning() {
   return timerRunning;
-}
\ No newline at end of file
+}
+void TimerController::OnTimerEnd() {
+  timerRunning = false;
+  if(systemTask != nullptr)
+    systemTask->PushMessage(System::Messages::OnTimerDone);
+}
+
+void TimerController::Register(Pinetime::System::SystemTask* systemTask) {
+  this->systemTask = systemTask;
+}
diff --git a/src/components/timer/TimerController.h b/src/components/timer/TimerController.h
index 5a0b293e..fa7bc90d 100644
--- a/src/components/timer/TimerController.h
+++ b/src/components/timer/TimerController.h
@@ -12,7 +12,7 @@ namespace Pinetime {
     
     class TimerController {
     public:
-      TimerController(Pinetime::System::SystemTask& systemTask);
+      TimerController() = default;
       
       void Init();
       
@@ -23,12 +23,13 @@ namespace Pinetime {
       uint32_t GetTimeRemaining();
       
       bool IsRunning();
-    
+
+      void OnTimerEnd();
+
+      void Register(System::SystemTask* systemTask);
+
     private:
-      System::SystemTask& systemTask;
-      
-      static void timerEnd(void* p_context);
-  
+      System::SystemTask* systemTask = nullptr;
       TickType_t endTicks;
       bool timerRunning = false;
     };
diff --git a/src/displayapp/DisplayApp.cpp b/src/displayapp/DisplayApp.cpp
index 7b03d569..05f171be 100644
--- a/src/displayapp/DisplayApp.cpp
+++ b/src/displayapp/DisplayApp.cpp
@@ -32,6 +32,7 @@
 #include "drivers/St7789.h"
 #include "drivers/Watchdog.h"
 #include "systemtask/SystemTask.h"
+#include "systemtask/Messages.h"
 
 #include "displayapp/screens/settings/QuickSettings.h"
 #include "displayapp/screens/settings/Settings.h"
@@ -44,6 +45,12 @@
 using namespace Pinetime::Applications;
 using namespace Pinetime::Applications::Display;
 
+namespace {
+  static inline bool in_isr(void) {
+    return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0;
+  }
+}
+
 DisplayApp::DisplayApp(Drivers::St7789& lcd,
                        Components::LittleVgl& lvgl,
                        Drivers::Cst816S& touchPanel,
@@ -51,7 +58,6 @@ DisplayApp::DisplayApp(Drivers::St7789& lcd,
                        Controllers::Ble& bleController,
                        Controllers::DateTime& dateTimeController,
                        Drivers::WatchdogView& watchdog,
-                       System::SystemTask& systemTask,
                        Pinetime::Controllers::NotificationManager& notificationManager,
                        Pinetime::Controllers::HeartRateController& heartRateController,
                        Controllers::Settings& settingsController,
@@ -65,19 +71,20 @@ DisplayApp::DisplayApp(Drivers::St7789& lcd,
     bleController {bleController},
     dateTimeController {dateTimeController},
     watchdog {watchdog},
-    systemTask {systemTask},
     notificationManager {notificationManager},
     heartRateController {heartRateController},
     settingsController {settingsController},
     motorController {motorController},
     motionController {motionController},
     timerController {timerController} {
-  msgQueue = xQueueCreate(queueSize, itemSize);
-  // Start clock when smartwatch boots
-  LoadApp(Apps::Clock, DisplayApp::FullRefreshDirections::None);
 }
 
 void DisplayApp::Start() {
+  msgQueue = xQueueCreate(queueSize, itemSize);
+
+  // Start clock when smartwatch boots
+  LoadApp(Apps::Clock, DisplayApp::FullRefreshDirections::None);
+
   if (pdPASS != xTaskCreate(DisplayApp::Process, "displayapp", 800, this, 0, &taskHandle)) {
     APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
   }
@@ -130,7 +137,7 @@ void DisplayApp::Refresh() {
           vTaskDelay(100);
         }
         lcd.DisplayOff();
-        systemTask.PushMessage(System::SystemTask::Messages::OnDisplayTaskSleeping);
+        PushMessageToSystemTask(Pinetime::System::Messages::OnDisplayTaskSleeping);
         state = States::Idle;
         break;
       case Messages::GoToRunning:
@@ -139,7 +146,7 @@ void DisplayApp::Refresh() {
         state = States::Running;
         break;
       case Messages::UpdateTimeOut:
-        systemTask.PushMessage(System::SystemTask::Messages::UpdateTimeOut);
+        PushMessageToSystemTask(System::Messages::UpdateTimeOut);
         break;
       case Messages::UpdateBleConnection:
         //        clockScreen.SetBleConnectionState(bleController.IsConnected() ? Screens::Clock::BleConnectionStates::Connected :
@@ -176,7 +183,7 @@ void DisplayApp::Refresh() {
                 LoadApp(Apps::QuickSettings, DisplayApp::FullRefreshDirections::RightAnim);
                 break;
               case TouchEvents::DoubleTap:
-                systemTask.PushMessage(System::SystemTask::Messages::GoToSleep);
+                PushMessageToSystemTask(System::Messages::GoToSleep);
                 break;
               default:
                 break;
@@ -188,7 +195,7 @@ void DisplayApp::Refresh() {
       } break;
       case Messages::ButtonPushed:
         if (currentApp == Apps::Clock) {
-          systemTask.PushMessage(System::SystemTask::Messages::GoToSleep);
+          PushMessageToSystemTask(System::Messages::GoToSleep);
         } else {
           if (!currentScreen->OnButtonPushed()) {
             LoadApp(returnToApp, returnDirection);
@@ -206,6 +213,11 @@ void DisplayApp::Refresh() {
     }
   }
 
+  if(nextApp != Apps::None) {
+    LoadApp(nextApp, nextDirection);
+    nextApp = Apps::None;
+  }
+
   if (state != States::Idle && touchMode == TouchModes::Polling) {
     auto info = touchPanel.GetTouchInfo();
     if (info.action == 2) { // 2 = contact
@@ -224,7 +236,8 @@ void DisplayApp::RunningState() {
 }
 
 void DisplayApp::StartApp(Apps app, DisplayApp::FullRefreshDirections direction) {
-  LoadApp(app, direction);
+  nextApp = app;
+  nextDirection = direction;
 }
 
 void DisplayApp::ReturnApp(Apps app, DisplayApp::FullRefreshDirections direction, TouchEvents touchEvent) {
@@ -267,12 +280,12 @@ void DisplayApp::LoadApp(Apps app, DisplayApp::FullRefreshDirections direction)
 
     case Apps::Notifications:
       currentScreen = std::make_unique<Screens::Notifications>(
-        this, notificationManager, systemTask.nimble().alertService(), Screens::Notifications::Modes::Normal);
+        this, notificationManager, systemTask->nimble().alertService(), Screens::Notifications::Modes::Normal);
       ReturnApp(Apps::Clock, FullRefreshDirections::Up, TouchEvents::SwipeUp);
       break;
     case Apps::NotificationsPreview:
       currentScreen = std::make_unique<Screens::Notifications>(
-        this, notificationManager, systemTask.nimble().alertService(), Screens::Notifications::Modes::Preview);
+        this, notificationManager, systemTask->nimble().alertService(), Screens::Notifications::Modes::Preview);
       ReturnApp(Apps::Clock, FullRefreshDirections::Up, TouchEvents::SwipeUp);
       break;
     case Apps::Timer:
@@ -321,7 +334,7 @@ void DisplayApp::LoadApp(Apps app, DisplayApp::FullRefreshDirections direction)
       //
 
     case Apps::FlashLight:
-      currentScreen = std::make_unique<Screens::FlashLight>(this, systemTask, brightnessController);
+      currentScreen = std::make_unique<Screens::FlashLight>(this, *systemTask, brightnessController);
       ReturnApp(Apps::Clock, FullRefreshDirections::Down, TouchEvents::None);
       break;
     case Apps::StopWatch:
@@ -337,13 +350,13 @@ void DisplayApp::LoadApp(Apps app, DisplayApp::FullRefreshDirections direction)
       currentScreen = std::make_unique<Screens::Paddle>(this, lvgl);
       break;
     case Apps::Music:
-      currentScreen = std::make_unique<Screens::Music>(this, systemTask.nimble().music());
+      currentScreen = std::make_unique<Screens::Music>(this, systemTask->nimble().music());
       break;
     case Apps::Navigation:
-      currentScreen = std::make_unique<Screens::Navigation>(this, systemTask.nimble().navigation());
+      currentScreen = std::make_unique<Screens::Navigation>(this, systemTask->nimble().navigation());
       break;
     case Apps::HeartRate:
-      currentScreen = std::make_unique<Screens::HeartRate>(this, heartRateController, systemTask);
+      currentScreen = std::make_unique<Screens::HeartRate>(this, heartRateController, *systemTask);
       break;
     case Apps::Motion:
       currentScreen = std::make_unique<Screens::Motion>(this, motionController);
@@ -359,12 +372,15 @@ void DisplayApp::IdleState() {
 }
 
 void DisplayApp::PushMessage(Messages msg) {
-  BaseType_t xHigherPriorityTaskWoken;
-  xHigherPriorityTaskWoken = pdFALSE;
-  xQueueSendFromISR(msgQueue, &msg, &xHigherPriorityTaskWoken);
-  if (xHigherPriorityTaskWoken) {
-    /* Actual macro used here is port specific. */
-    // TODO : should I do something here?
+  if(in_isr()) {
+    BaseType_t xHigherPriorityTaskWoken;
+    xHigherPriorityTaskWoken = pdFALSE;
+    xQueueSendFromISR(msgQueue, &msg, &xHigherPriorityTaskWoken);
+    if (xHigherPriorityTaskWoken) {
+      portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    }
+  } else {
+    xQueueSend(msgQueue, &msg, portMAX_DELAY);
   }
 }
 
@@ -425,3 +441,12 @@ void DisplayApp::SetFullRefresh(DisplayApp::FullRefreshDirections direction) {
 void DisplayApp::SetTouchMode(DisplayApp::TouchModes mode) {
   touchMode = mode;
 }
+
+void DisplayApp::PushMessageToSystemTask(Pinetime::System::Messages message) {
+  if(systemTask != nullptr)
+    systemTask->PushMessage(message);
+}
+
+void DisplayApp::Register(Pinetime::System::SystemTask* systemTask) {
+  this->systemTask = systemTask;
+}
diff --git a/src/displayapp/DisplayApp.h b/src/displayapp/DisplayApp.h
index 0c7bd216..73a7cc36 100644
--- a/src/displayapp/DisplayApp.h
+++ b/src/displayapp/DisplayApp.h
@@ -4,6 +4,7 @@
 #include <queue.h>
 #include <task.h>
 #include <memory>
+#include <systemtask/Messages.h>
 #include "Apps.h"
 #include "LittleVgl.h"
 #include "TouchEvents.h"
@@ -49,7 +50,6 @@ namespace Pinetime {
                  Controllers::Ble& bleController,
                  Controllers::DateTime& dateTimeController,
                  Drivers::WatchdogView& watchdog,
-                 System::SystemTask& systemTask,
                  Pinetime::Controllers::NotificationManager& notificationManager,
                  Pinetime::Controllers::HeartRateController& heartRateController,
                  Controllers::Settings& settingsController,
@@ -64,6 +64,8 @@ namespace Pinetime {
       void SetFullRefresh(FullRefreshDirections direction);
       void SetTouchMode(TouchModes mode);
 
+      void Register(Pinetime::System::SystemTask* systemTask);
+
     private:
       Pinetime::Drivers::St7789& lcd;
       Pinetime::Components::LittleVgl& lvgl;
@@ -72,7 +74,7 @@ namespace Pinetime {
       Pinetime::Controllers::Ble& bleController;
       Pinetime::Controllers::DateTime& dateTimeController;
       Pinetime::Drivers::WatchdogView& watchdog;
-      Pinetime::System::SystemTask& systemTask;
+      Pinetime::System::SystemTask* systemTask = nullptr;
       Pinetime::Controllers::NotificationManager& notificationManager;
       Pinetime::Controllers::HeartRateController& heartRateController;
       Pinetime::Controllers::Settings& settingsController;
@@ -108,6 +110,10 @@ namespace Pinetime {
       void Refresh();
       void ReturnApp(Apps app, DisplayApp::FullRefreshDirections direction, TouchEvents touchEvent);
       void LoadApp(Apps app, DisplayApp::FullRefreshDirections direction);
+      void PushMessageToSystemTask(Pinetime::System::Messages message);
+
+      Apps nextApp = Apps::None;
+      DisplayApp::FullRefreshDirections nextDirection;
     };
   }
 }
diff --git a/src/displayapp/DisplayAppRecovery.cpp b/src/displayapp/DisplayAppRecovery.cpp
index 856eafd8..fd517b11 100644
--- a/src/displayapp/DisplayAppRecovery.cpp
+++ b/src/displayapp/DisplayAppRecovery.cpp
@@ -14,7 +14,6 @@ DisplayApp::DisplayApp(Drivers::St7789& lcd,
                        Controllers::Ble& bleController,
                        Controllers::DateTime& dateTimeController,
                        Drivers::WatchdogView& watchdog,
-                       System::SystemTask& systemTask,
                        Pinetime::Controllers::NotificationManager& notificationManager,
                        Pinetime::Controllers::HeartRateController& heartRateController,
                        Controllers::Settings& settingsController,
@@ -22,10 +21,11 @@ DisplayApp::DisplayApp(Drivers::St7789& lcd,
                        Pinetime::Controllers::MotionController& motionController,
                        Pinetime::Controllers::TimerController& timerController)
   : lcd {lcd}, bleController {bleController} {
-  msgQueue = xQueueCreate(queueSize, itemSize);
+
 }
 
 void DisplayApp::Start() {
+  msgQueue = xQueueCreate(queueSize, itemSize);
   if (pdPASS != xTaskCreate(DisplayApp::Process, "displayapp", 512, this, 0, &taskHandle))
     APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
 }
@@ -113,4 +113,8 @@ void DisplayApp::PushMessage(Display::Messages msg) {
     /* Actual macro used here is port specific. */
     // TODO : should I do something here?
   }
-}
\ No newline at end of file
+}
+
+void DisplayApp::Register(Pinetime::System::SystemTask* systemTask) {
+
+}
diff --git a/src/displayapp/DisplayAppRecovery.h b/src/displayapp/DisplayAppRecovery.h
index 2c5a36fb..638c0071 100644
--- a/src/displayapp/DisplayAppRecovery.h
+++ b/src/displayapp/DisplayAppRecovery.h
@@ -39,7 +39,6 @@ namespace Pinetime {
                  Controllers::Ble& bleController,
                  Controllers::DateTime& dateTimeController,
                  Drivers::WatchdogView& watchdog,
-                 System::SystemTask& systemTask,
                  Pinetime::Controllers::NotificationManager& notificationManager,
                  Pinetime::Controllers::HeartRateController& heartRateController,
                  Controllers::Settings& settingsController,
@@ -48,6 +47,7 @@ namespace Pinetime {
                  Pinetime::Controllers::TimerController& timerController);
       void Start();
       void PushMessage(Pinetime::Applications::Display::Messages msg);
+      void Register(Pinetime::System::SystemTask* systemTask);
 
     private:
       TaskHandle_t taskHandle;
diff --git a/src/displayapp/LittleVgl.cpp b/src/displayapp/LittleVgl.cpp
index 36df51b4..c069afa2 100644
--- a/src/displayapp/LittleVgl.cpp
+++ b/src/displayapp/LittleVgl.cpp
@@ -23,6 +23,10 @@ bool touchpad_read(lv_indev_drv_t* indev_drv, lv_indev_data_t* data) {
 
 LittleVgl::LittleVgl(Pinetime::Drivers::St7789& lcd, Pinetime::Drivers::Cst816S& touchPanel)
   : lcd {lcd}, touchPanel {touchPanel}, previousClick {0, 0} {
+
+}
+
+void LittleVgl::Init() {
   lv_init();
   InitTheme();
   InitDisplay();
diff --git a/src/displayapp/LittleVgl.h b/src/displayapp/LittleVgl.h
index 7f7b76e0..41f934a7 100644
--- a/src/displayapp/LittleVgl.h
+++ b/src/displayapp/LittleVgl.h
@@ -19,6 +19,8 @@ namespace Pinetime {
       LittleVgl(LittleVgl&&) = delete;
       LittleVgl& operator=(LittleVgl&&) = delete;
 
+      void Init();
+
       void FlushDisplay(const lv_area_t* area, lv_color_t* color_p);
       bool GetTouchPadInfo(lv_indev_data_t* ptr);
       void SetFullRefresh(FullRefreshDirections direction);
diff --git a/src/displayapp/screens/FlashLight.cpp b/src/displayapp/screens/FlashLight.cpp
index 4568db40..7db2c6c8 100644
--- a/src/displayapp/screens/FlashLight.cpp
+++ b/src/displayapp/screens/FlashLight.cpp
@@ -39,14 +39,14 @@ FlashLight::FlashLight(Pinetime::Applications::DisplayApp* app,
   backgroundAction->user_data = this;
   lv_obj_set_event_cb(backgroundAction, event_handler);
 
-  systemTask.PushMessage(Pinetime::System::SystemTask::Messages::DisableSleeping);
+  systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
 }
 
 FlashLight::~FlashLight() {
   lv_obj_clean(lv_scr_act());
   lv_obj_set_style_local_bg_color(lv_scr_act(), LV_OBJ_PART_MAIN, LV_STATE_DEFAULT, lv_color_hex(0x000000));
   brightness.Restore();
-  systemTask.PushMessage(Pinetime::System::SystemTask::Messages::EnableSleeping);
+  systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping);
 }
 
 void FlashLight::OnClickEvent(lv_obj_t* obj, lv_event_t event) {
diff --git a/src/displayapp/screens/HeartRate.cpp b/src/displayapp/screens/HeartRate.cpp
index 90f6bc26..5689b63e 100644
--- a/src/displayapp/screens/HeartRate.cpp
+++ b/src/displayapp/screens/HeartRate.cpp
@@ -63,12 +63,12 @@ HeartRate::HeartRate(Pinetime::Applications::DisplayApp* app,
   label_startStop = lv_label_create(btn_startStop, nullptr);
   UpdateStartStopButton(isHrRunning);
   if (isHrRunning)
-    systemTask.PushMessage(Pinetime::System::SystemTask::Messages::DisableSleeping);
+    systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
 }
 
 HeartRate::~HeartRate() {
   lv_obj_clean(lv_scr_act());
-  systemTask.PushMessage(Pinetime::System::SystemTask::Messages::EnableSleeping);
+  systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping);
 }
 
 bool HeartRate::Refresh() {
@@ -95,12 +95,12 @@ void HeartRate::OnStartStopEvent(lv_event_t event) {
     if (heartRateController.State() == Controllers::HeartRateController::States::Stopped) {
       heartRateController.Start();
       UpdateStartStopButton(heartRateController.State() != Controllers::HeartRateController::States::Stopped);
-      systemTask.PushMessage(Pinetime::System::SystemTask::Messages::DisableSleeping);
+      systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
       lv_obj_set_style_local_text_color(label_hr, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, LV_COLOR_GREEN);
     } else {
       heartRateController.Stop();
       UpdateStartStopButton(heartRateController.State() != Controllers::HeartRateController::States::Stopped);
-      systemTask.PushMessage(Pinetime::System::SystemTask::Messages::EnableSleeping);
+      systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping);
       lv_obj_set_style_local_text_color(label_hr, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, LV_COLOR_GRAY);
     }
   }
diff --git a/src/displayapp/screens/ScreenList.h b/src/displayapp/screens/ScreenList.h
index 73ea4610..ea66bfb2 100644
--- a/src/displayapp/screens/ScreenList.h
+++ b/src/displayapp/screens/ScreenList.h
@@ -15,12 +15,17 @@ namespace Pinetime {
       public:
         ScreenList(DisplayApp* app,
                    uint8_t initScreen,
-                   std::array<std::function<std::unique_ptr<Screen>()>, N>&& screens,
+                   const std::array<std::function<std::unique_ptr<Screen>()>, N>&& screens,
                    ScreenListModes mode)
-          : Screen(app), initScreen {initScreen}, screens {std::move(screens)}, mode {mode}, current {this->screens[initScreen]()} {
-          screenIndex = initScreen;
+          : Screen(app), initScreen {initScreen}, screens {std::move(screens)}, mode {mode}, screenIndex{initScreen}, current {this->screens[initScreen]()} {
+
         }
 
+        ScreenList(const ScreenList&) = delete;
+        ScreenList& operator=(const ScreenList&) = delete;
+        ScreenList(ScreenList&&) = delete;
+        ScreenList& operator=(ScreenList&&) = delete;
+
         ~ScreenList() override {
           lv_obj_clean(lv_scr_act());
         }
@@ -97,7 +102,7 @@ namespace Pinetime {
 
       private:
         uint8_t initScreen = 0;
-        std::array<std::function<std::unique_ptr<Screen>()>, N> screens;
+        const std::array<std::function<std::unique_ptr<Screen>()>, N> screens;
         ScreenListModes mode = ScreenListModes::UpDown;
 
         uint8_t screenIndex = 0;
diff --git a/src/displayapp/screens/SystemInfo.cpp b/src/displayapp/screens/SystemInfo.cpp
index f61d2ff1..a7387dac 100644
--- a/src/displayapp/screens/SystemInfo.cpp
+++ b/src/displayapp/screens/SystemInfo.cpp
@@ -81,7 +81,7 @@ std::unique_ptr<Screen> SystemInfo::CreateScreen1() {
                         __TIME__);
   lv_label_set_align(label, LV_LABEL_ALIGN_CENTER);
   lv_obj_align(label, lv_scr_act(), LV_ALIGN_CENTER, 0, 0);
-  return std::unique_ptr<Screen>(new Screens::Label(0, 5, app, label));
+  return std::make_unique<Screens::Label>(0, 5, app, label);
 }
 
 std::unique_ptr<Screen> SystemInfo::CreateScreen2() {
@@ -161,7 +161,7 @@ std::unique_ptr<Screen> SystemInfo::CreateScreen2() {
                         brightnessController.ToString(),
                         resetReason);
   lv_obj_align(label, lv_scr_act(), LV_ALIGN_CENTER, 0, 0);
-  return std::unique_ptr<Screen>(new Screens::Label(1, 4, app, label));
+  return std::make_unique<Screens::Label>(1, 4, app, label);
 }
 
 std::unique_ptr<Screen> SystemInfo::CreateScreen3() {
@@ -195,7 +195,7 @@ std::unique_ptr<Screen> SystemInfo::CreateScreen3() {
                         (int) mon.free_biggest_size,
                         0);
   lv_obj_align(label, lv_scr_act(), LV_ALIGN_CENTER, 0, 0);
-  return std::unique_ptr<Screen>(new Screens::Label(2, 5, app, label));
+  return std::make_unique<Screens::Label>(2, 5, app, label);
 }
 
 bool sortById(const TaskStatus_t& lhs, const TaskStatus_t& rhs) {
@@ -229,7 +229,7 @@ std::unique_ptr<Screen> SystemInfo::CreateScreen4() {
       lv_table_set_cell_value(infoTask, i + 1, 2, std::to_string(tasksStatus[i].usStackHighWaterMark).c_str());
     }
   }
-  return std::unique_ptr<Screen>(new Screens::Label(3, 5, app, infoTask));
+  return std::make_unique<Screens::Label>(3, 5, app, infoTask);
 }
 
 std::unique_ptr<Screen> SystemInfo::CreateScreen5() {
@@ -245,5 +245,5 @@ std::unique_ptr<Screen> SystemInfo::CreateScreen5() {
                            "#FFFF00 JF002/InfiniTime#");
   lv_label_set_align(label, LV_LABEL_ALIGN_CENTER);
   lv_obj_align(label, lv_scr_act(), LV_ALIGN_CENTER, 0, 0);
-  return std::unique_ptr<Screen>(new Screens::Label(4, 5, app, label));
+  return std::make_unique<Screens::Label>(4, 5, app, label);
 }
diff --git a/src/displayapp/screens/settings/QuickSettings.cpp b/src/displayapp/screens/settings/QuickSettings.cpp
index 3994794d..5db7468c 100644
--- a/src/displayapp/screens/settings/QuickSettings.cpp
+++ b/src/displayapp/screens/settings/QuickSettings.cpp
@@ -7,12 +7,12 @@ using namespace Pinetime::Applications::Screens;
 
 namespace {
   static void ButtonEventHandler(lv_obj_t* obj, lv_event_t event) {
-    QuickSettings* screen = static_cast<QuickSettings*>(obj->user_data);
+    auto* screen = static_cast<QuickSettings*>(obj->user_data);
     screen->OnButtonEvent(obj, event);
   }
 
   static void lv_update_task(struct _lv_task_t* task) {
-    auto user_data = static_cast<QuickSettings*>(task->user_data);
+    auto* user_data = static_cast<QuickSettings*>(task->user_data);
     user_data->UpdateScreen();
   }
 }
diff --git a/src/displayapp/screens/settings/Settings.cpp b/src/displayapp/screens/settings/Settings.cpp
index 2c72c832..e63a3584 100644
--- a/src/displayapp/screens/settings/Settings.cpp
+++ b/src/displayapp/screens/settings/Settings.cpp
@@ -46,7 +46,7 @@ std::unique_ptr<Screen> Settings::CreateScreen1() {
     {Symbols::clock, "Watch face", Apps::SettingWatchFace},
   }};
 
-  return std::unique_ptr<Screen>(new Screens::List(0, 2, app, settingsController, applications));
+  return std::make_unique<Screens::List>(0, 2, app, settingsController, applications);
 }
 
 std::unique_ptr<Screen> Settings::CreateScreen2() {
@@ -58,5 +58,5 @@ std::unique_ptr<Screen> Settings::CreateScreen2() {
     {Symbols::list, "About", Apps::SysInfo},
   }};
 
-  return std::unique_ptr<Screen>(new Screens::List(1, 2, app, settingsController, applications));
+  return std::make_unique<Screens::List>(1, 2, app, settingsController, applications);
 }
diff --git a/src/displayapp/screens/settings/Settings.h b/src/displayapp/screens/settings/Settings.h
index 7e332dfe..711a6be6 100644
--- a/src/displayapp/screens/settings/Settings.h
+++ b/src/displayapp/screens/settings/Settings.h
@@ -16,7 +16,6 @@ namespace Pinetime {
 
         bool Refresh() override;
 
-        void OnButtonEvent(lv_obj_t* object, lv_event_t event);
         bool OnTouchEvent(Pinetime::Applications::TouchEvents event) override;
 
       private:
diff --git a/src/drivers/Bma421.cpp b/src/drivers/Bma421.cpp
index 925b66c7..35b2c105 100644
--- a/src/drivers/Bma421.cpp
+++ b/src/drivers/Bma421.cpp
@@ -103,8 +103,6 @@ Bma421::Values Bma421::Process() {
   uint8_t activity = 0;
   bma423_activity_output(&activity, &bma);
 
-  NRF_LOG_INFO("MOTION : %d - %d/%d/%d", steps, data.x, data.y, data.z);
-
   // X and Y axis are swapped because of the way the sensor is mounted in the PineTime
   return {steps, data.y, data.x, data.z};
 }
diff --git a/src/drivers/SpiMaster.cpp b/src/drivers/SpiMaster.cpp
index 34fcc08a..0d5bb83c 100644
--- a/src/drivers/SpiMaster.cpp
+++ b/src/drivers/SpiMaster.cpp
@@ -7,11 +7,14 @@
 using namespace Pinetime::Drivers;
 
 SpiMaster::SpiMaster(const SpiMaster::SpiModule spi, const SpiMaster::Parameters& params) : spi {spi}, params {params} {
-  mutex = xSemaphoreCreateBinary();
-  ASSERT(mutex != NULL);
 }
 
 bool SpiMaster::Init() {
+  if(mutex == nullptr) {
+    mutex = xSemaphoreCreateBinary();
+    ASSERT(mutex != nullptr);
+  }
+
   /* Configure GPIO pins used for pselsck, pselmosi, pselmiso and pselss for SPI0 */
   nrf_gpio_pin_set(params.pinSCK);
   nrf_gpio_cfg_output(params.pinSCK);
@@ -132,17 +135,17 @@ void SpiMaster::OnEndEvent() {
 
     spiBaseAddress->TASKS_START = 1;
   } else {
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
     if (taskToNotify != nullptr) {
-      BaseType_t xHigherPriorityTaskWoken = pdFALSE;
       vTaskNotifyGiveFromISR(taskToNotify, &xHigherPriorityTaskWoken);
       portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
     }
 
     nrf_gpio_pin_set(this->pinCsn);
     currentBufferAddr = 0;
-    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-    xSemaphoreGiveFromISR(mutex, &xHigherPriorityTaskWoken);
-    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    BaseType_t xHigherPriorityTaskWoken2 = pdFALSE;
+    xSemaphoreGiveFromISR(mutex, &xHigherPriorityTaskWoken2);
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken | xHigherPriorityTaskWoken2);
   }
 }
 
diff --git a/src/drivers/SpiMaster.h b/src/drivers/SpiMaster.h
index dfc195b7..5ea624f2 100644
--- a/src/drivers/SpiMaster.h
+++ b/src/drivers/SpiMaster.h
@@ -10,7 +10,6 @@ namespace Pinetime {
   namespace Drivers {
     class SpiMaster {
     public:
-      ;
       enum class SpiModule : uint8_t { SPI0, SPI1 };
       enum class BitOrder : uint8_t { Msb_Lsb, Lsb_Msb };
       enum class Modes : uint8_t { Mode0, Mode1, Mode2, Mode3 };
@@ -60,7 +59,7 @@ namespace Pinetime {
       volatile uint32_t currentBufferAddr = 0;
       volatile size_t currentBufferSize = 0;
       volatile TaskHandle_t taskToNotify;
-      SemaphoreHandle_t mutex;
+      SemaphoreHandle_t mutex = nullptr;
     };
   }
 }
diff --git a/src/drivers/TwiMaster.cpp b/src/drivers/TwiMaster.cpp
index 7b6582dd..fc9edf81 100644
--- a/src/drivers/TwiMaster.cpp
+++ b/src/drivers/TwiMaster.cpp
@@ -9,10 +9,12 @@ using namespace Pinetime::Drivers;
 // TODO use DMA/IRQ
 
 TwiMaster::TwiMaster(const Modules module, const Parameters& params) : module {module}, params {params} {
-  mutex = xSemaphoreCreateBinary();
 }
 
 void TwiMaster::Init() {
+  if(mutex == nullptr)
+    mutex = xSemaphoreCreateBinary();
+  
   NRF_GPIO->PIN_CNF[params.pinScl] =
     ((uint32_t) GPIO_PIN_CNF_DIR_Input << GPIO_PIN_CNF_DIR_Pos) | ((uint32_t) GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) |
     ((uint32_t) GPIO_PIN_CNF_PULL_Pullup << GPIO_PIN_CNF_PULL_Pos) | ((uint32_t) GPIO_PIN_CNF_DRIVE_S0D1 << GPIO_PIN_CNF_DRIVE_Pos) |
diff --git a/src/drivers/TwiMaster.h b/src/drivers/TwiMaster.h
index 1c0648a2..6175b99b 100644
--- a/src/drivers/TwiMaster.h
+++ b/src/drivers/TwiMaster.h
@@ -31,7 +31,7 @@ namespace Pinetime {
       ErrorCodes Write(uint8_t deviceAddress, const uint8_t* data, size_t size, bool stop);
       void FixHwFreezed();
       NRF_TWIM_Type* twiBaseAddress;
-      SemaphoreHandle_t mutex;
+      SemaphoreHandle_t mutex = nullptr;
       const Modules module;
       const Parameters params;
       static constexpr uint8_t maxDataSize {16};
diff --git a/src/heartratetask/HeartRateTask.cpp b/src/heartratetask/HeartRateTask.cpp
index 36c8cc18..fddc05d7 100644
--- a/src/heartratetask/HeartRateTask.cpp
+++ b/src/heartratetask/HeartRateTask.cpp
@@ -6,12 +6,13 @@
 using namespace Pinetime::Applications;
 
 HeartRateTask::HeartRateTask(Drivers::Hrs3300& heartRateSensor, Controllers::HeartRateController& controller)
-  : heartRateSensor {heartRateSensor}, controller {controller}, ppg {static_cast<float>(heartRateSensor.ReadHrs())} {
-  messageQueue = xQueueCreate(10, 1);
-  controller.SetHeartRateTask(this);
+  : heartRateSensor {heartRateSensor}, controller {controller}, ppg{} {
 }
 
 void HeartRateTask::Start() {
+  messageQueue = xQueueCreate(10, 1);
+  controller.SetHeartRateTask(this);
+
   if (pdPASS != xTaskCreate(HeartRateTask::Process, "Heartrate", 500, this, 0, &taskHandle))
     APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
 }
diff --git a/src/main.cpp b/src/main.cpp
index 61194b95..4c2c5de8 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -33,7 +33,7 @@
 #include "components/ble/NotificationManager.h"
 #include "components/motor/MotorController.h"
 #include "components/datetime/DateTimeController.h"
-#include "components/settings/Settings.h"
+#include "components/heartrate/HeartRateController.h"
 #include "drivers/Spi.h"
 #include "drivers/SpiMaster.h"
 #include "drivers/SpiNorFlash.h"
@@ -50,8 +50,6 @@ Pinetime::Logging::NrfLogger logger;
 Pinetime::Logging::DummyLogger logger;
 #endif
 
-#include <memory>
-
 static constexpr uint8_t pinSpiSck = 2;
 static constexpr uint8_t pinSpiMosi = 3;
 static constexpr uint8_t pinSpiMiso = 4;
@@ -108,15 +106,59 @@ void ble_manager_set_ble_connection_callback(void (*connection)());
 void ble_manager_set_ble_disconnection_callback(void (*disconnection)());
 static constexpr uint8_t pinTouchIrq = 28;
 static constexpr uint8_t pinPowerPresentIrq = 19;
-std::unique_ptr<Pinetime::System::SystemTask> systemTask;
 
 Pinetime::Controllers::Settings settingsController {spiNorFlash};
 
 Pinetime::Controllers::MotorController motorController {settingsController};
 
+Pinetime::Controllers::HeartRateController heartRateController;
+Pinetime::Applications::HeartRateTask heartRateApp(heartRateSensor, heartRateController);
+
+Pinetime::Controllers::DateTime dateTimeController;
+Pinetime::Drivers::Watchdog watchdog;
+Pinetime::Drivers::WatchdogView watchdogView(watchdog);
+Pinetime::Controllers::NotificationManager notificationManager;
+Pinetime::Controllers::MotionController motionController;
+Pinetime::Controllers::TimerController timerController;
+
+Pinetime::Applications::DisplayApp displayApp(lcd,
+                                              lvgl,
+                                              touchPanel,
+                                              batteryController,
+                                              bleController,
+                                              dateTimeController,
+                                              watchdogView,
+                                              notificationManager,
+                                              heartRateController,
+                                              settingsController,
+                                              motorController,
+                                              motionController,
+                                              timerController);
+
+Pinetime::System::SystemTask systemTask(spi,
+                                        lcd,
+                                        spiNorFlash,
+                                        twiMaster,
+                                        touchPanel,
+                                        lvgl,
+                                        batteryController,
+                                        bleController,
+                                        dateTimeController,
+                                        timerController,
+                                        watchdog,
+                                        notificationManager,
+                                        motorController,
+                                        heartRateSensor,
+                                        motionController,
+                                        motionSensor,
+                                        settingsController,
+                                        heartRateController,
+                                        displayApp,
+                                        heartRateApp);
+
 void nrfx_gpiote_evt_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action) {
   if (pin == pinTouchIrq) {
-    systemTask->OnTouchEvent();
+    systemTask.OnTouchEvent();
     return;
   }
 
@@ -141,12 +183,12 @@ void vApplicationIdleHook(void) {
 
 void DebounceTimerChargeCallback(TimerHandle_t xTimer) {
   xTimerStop(xTimer, 0);
-  systemTask->PushMessage(Pinetime::System::SystemTask::Messages::OnChargingEvent);
+  systemTask.PushMessage(Pinetime::System::Messages::OnChargingEvent);
 }
 
 void DebounceTimerCallback(TimerHandle_t xTimer) {
   xTimerStop(xTimer, 0);
-  systemTask->OnButtonPushed();
+  systemTask.OnButtonPushed();
 }
 
 void SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0_IRQHandler(void) {
@@ -264,19 +306,9 @@ int main(void) {
   debounceTimer = xTimerCreate("debounceTimer", 200, pdFALSE, (void*) 0, DebounceTimerCallback);
   debounceChargeTimer = xTimerCreate("debounceTimerCharge", 200, pdFALSE, (void*) 0, DebounceTimerChargeCallback);
 
-  systemTask = std::make_unique<Pinetime::System::SystemTask>(spi,
-                                                              lcd,
-                                                              spiNorFlash,
-                                                              twiMaster,
-                                                              touchPanel,
-                                                              lvgl,
-                                                              batteryController,
-                                                              bleController,
-                                                              motorController,
-                                                              heartRateSensor,
-                                                              motionSensor,
-                                                              settingsController);
-  systemTask->Start();
+  lvgl.Init();
+
+  systemTask.Start();
   nimble_port_init();
 
   vTaskStartScheduler();
diff --git a/src/systemtask/Messages.h b/src/systemtask/Messages.h
new file mode 100644
index 00000000..3a195e2d
--- /dev/null
+++ b/src/systemtask/Messages.h
@@ -0,0 +1,26 @@
+#pragma once
+
+namespace Pinetime {
+  namespace System {
+      enum class Messages {
+        GoToSleep,
+        GoToRunning,
+        TouchWakeUp,
+        OnNewTime,
+        OnNewNotification,
+        OnTimerDone,
+        OnNewCall,
+        BleConnected,
+        UpdateTimeOut,
+        BleFirmwareUpdateStarted,
+        BleFirmwareUpdateFinished,
+        OnTouchEvent,
+        OnButtonEvent,
+        OnDisplayTaskSleeping,
+        EnableSleeping,
+        DisableSleeping,
+        OnNewDay,
+        OnChargingEvent
+      };
+    }
+}
diff --git a/src/systemtask/SystemTask.cpp b/src/systemtask/SystemTask.cpp
index 58377764..38e9793c 100644
--- a/src/systemtask/SystemTask.cpp
+++ b/src/systemtask/SystemTask.cpp
@@ -27,6 +27,12 @@
 
 using namespace Pinetime::System;
 
+namespace {
+  static inline bool in_isr(void) {
+    return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0;
+  }
+}
+
 void IdleTimerCallback(TimerHandle_t xTimer) {
 
   NRF_LOG_INFO("IdleTimerCallback");
@@ -42,10 +48,18 @@ SystemTask::SystemTask(Drivers::SpiMaster& spi,
                        Components::LittleVgl& lvgl,
                        Controllers::Battery& batteryController,
                        Controllers::Ble& bleController,
+                       Controllers::DateTime& dateTimeController,
+                       Controllers::TimerController& timerController,
+                       Drivers::Watchdog& watchdog,
+                       Pinetime::Controllers::NotificationManager& notificationManager,
                        Pinetime::Controllers::MotorController& motorController,
                        Pinetime::Drivers::Hrs3300& heartRateSensor,
+                       Pinetime::Controllers::MotionController& motionController,
                        Pinetime::Drivers::Bma421& motionSensor,
-                       Controllers::Settings& settingsController)
+                       Controllers::Settings& settingsController,
+                       Pinetime::Controllers::HeartRateController& heartRateController,
+                       Pinetime::Applications::DisplayApp& displayApp,
+                       Pinetime::Applications::HeartRateTask& heartRateApp)
   : spi {spi},
     lcd {lcd},
     spiNorFlash {spiNorFlash},
@@ -53,21 +67,25 @@ SystemTask::SystemTask(Drivers::SpiMaster& spi,
     touchPanel {touchPanel},
     lvgl {lvgl},
     batteryController {batteryController},
-    heartRateController {*this},
     bleController {bleController},
-    dateTimeController {*this},
-    timerController {*this},
-    watchdog {},
-    watchdogView {watchdog},
+    dateTimeController {dateTimeController},
+    timerController {timerController},
+    watchdog {watchdog},
+    notificationManager{notificationManager},
     motorController {motorController},
     heartRateSensor {heartRateSensor},
     motionSensor {motionSensor},
     settingsController {settingsController},
-    nimbleController(*this, bleController, dateTimeController, notificationManager, batteryController, spiNorFlash, heartRateController) {
-  systemTasksMsgQueue = xQueueCreate(10, 1);
+    heartRateController{heartRateController},
+    nimbleController(*this, bleController, dateTimeController, notificationManager, batteryController, spiNorFlash, heartRateController),
+    motionController{motionController},
+    displayApp{displayApp},
+    heartRateApp(heartRateApp) {
+
 }
 
 void SystemTask::Start() {
+  systemTasksMsgQueue = xQueueCreate(10, 1);
   if (pdPASS != xTaskCreate(SystemTask::Process, "MAIN", 350, this, 0, &taskHandle))
     APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
 }
@@ -96,9 +114,11 @@ void SystemTask::Work() {
 
   twiMaster.Init();
   touchPanel.Init();
+  dateTimeController.Register(this);
   batteryController.Init();
   motorController.Init();
   motionSensor.SoftReset();
+  timerController.Register(this);
   timerController.Init();
 
   // Reset the TWI device because the motion sensor chip most probably crashed it...
@@ -108,28 +128,14 @@ void SystemTask::Work() {
   motionSensor.Init();
   settingsController.Init();
 
-  displayApp = std::make_unique<Pinetime::Applications::DisplayApp>(lcd,
-                                                                    lvgl,
-                                                                    touchPanel,
-                                                                    batteryController,
-                                                                    bleController,
-                                                                    dateTimeController,
-                                                                    watchdogView,
-                                                                    *this,
-                                                                    notificationManager,
-                                                                    heartRateController,
-                                                                    settingsController,
-                                                                    motorController,
-                                                                    motionController,
-                                                                    timerController);
-  displayApp->Start();
+  displayApp.Register(this);
+  displayApp.Start();
 
-  displayApp->PushMessage(Pinetime::Applications::Display::Messages::UpdateBatteryLevel);
+  displayApp.PushMessage(Pinetime::Applications::Display::Messages::UpdateBatteryLevel);
 
   heartRateSensor.Init();
   heartRateSensor.Disable();
-  heartRateApp = std::make_unique<Pinetime::Applications::HeartRateTask>(heartRateSensor, heartRateController);
-  heartRateApp->Start();
+  heartRateApp.Start();
 
   nrf_gpio_cfg_sense_input(pinButton, (nrf_gpio_pin_pull_t) GPIO_PIN_CNF_PULL_Pulldown, (nrf_gpio_pin_sense_t) GPIO_PIN_CNF_SENSE_High);
   nrf_gpio_cfg_output(15);
@@ -208,9 +214,9 @@ void SystemTask::Work() {
           spiNorFlash.Wakeup();
           lcd.Wakeup();
 
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::GoToRunning);
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::UpdateBatteryLevel);
-          heartRateApp->PushMessage(Pinetime::Applications::HeartRateTask::Messages::WakeUp);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::GoToRunning);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::UpdateBatteryLevel);
+          heartRateApp.PushMessage(Pinetime::Applications::HeartRateTask::Messages::WakeUp);
 
           isSleeping = false;
           isWakingUp = false;
@@ -230,26 +236,26 @@ void SystemTask::Work() {
           isGoingToSleep = true;
           NRF_LOG_INFO("[systemtask] Going to sleep");
           xTimerStop(idleTimer, 0);
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::GoToSleep);
-          heartRateApp->PushMessage(Pinetime::Applications::HeartRateTask::Messages::GoToSleep);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::GoToSleep);
+          heartRateApp.PushMessage(Pinetime::Applications::HeartRateTask::Messages::GoToSleep);
           break;
         case Messages::OnNewTime:
           ReloadIdleTimer();
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::UpdateDateTime);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::UpdateDateTime);
           break;
         case Messages::OnNewNotification:
           if (isSleeping && !isWakingUp) {
             GoToRunning();
           }
           motorController.SetDuration(35);
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::NewNotification);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::NewNotification);
           break;
         case Messages::OnTimerDone:
           if (isSleeping && !isWakingUp) {
             GoToRunning();
           }
           motorController.SetDuration(35);
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::TimerDone);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::TimerDone);
           break;
         case Messages::BleConnected:
           ReloadIdleTimer();
@@ -260,7 +266,7 @@ void SystemTask::Work() {
           doNotGoToSleep = true;
           if (isSleeping && !isWakingUp)
             GoToRunning();
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::BleFirmwareUpdateStarted);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::BleFirmwareUpdateStarted);
           break;
         case Messages::BleFirmwareUpdateFinished:
           doNotGoToSleep = false;
@@ -359,7 +365,7 @@ void SystemTask::OnButtonPushed() {
   if (!isSleeping) {
     NRF_LOG_INFO("[systemtask] Button pushed");
     PushMessage(Messages::OnButtonEvent);
-    displayApp->PushMessage(Pinetime::Applications::Display::Messages::ButtonPushed);
+    displayApp.PushMessage(Pinetime::Applications::Display::Messages::ButtonPushed);
   } else {
     if (!isWakingUp) {
       NRF_LOG_INFO("[systemtask] Button pushed, waking up");
@@ -380,7 +386,7 @@ void SystemTask::OnTouchEvent() {
     return;
   if (!isSleeping) {
     PushMessage(Messages::OnTouchEvent);
-    displayApp->PushMessage(Pinetime::Applications::Display::Messages::TouchEvent);
+    displayApp.PushMessage(Pinetime::Applications::Display::Messages::TouchEvent);
   } else if (!isWakingUp) {
     if (settingsController.getWakeUpMode() == Pinetime::Controllers::Settings::WakeUpMode::None or
         settingsController.getWakeUpMode() == Pinetime::Controllers::Settings::WakeUpMode::RaiseWrist)
@@ -389,16 +395,22 @@ void SystemTask::OnTouchEvent() {
   }
 }
 
-void SystemTask::PushMessage(SystemTask::Messages msg) {
+void SystemTask::PushMessage(System::Messages msg) {
   if (msg == Messages::GoToSleep) {
     isGoingToSleep = true;
   }
-  BaseType_t xHigherPriorityTaskWoken;
-  xHigherPriorityTaskWoken = pdFALSE;
-  xQueueSendFromISR(systemTasksMsgQueue, &msg, &xHigherPriorityTaskWoken);
-  if (xHigherPriorityTaskWoken) {
-    /* Actual macro used here is port specific. */
-    // TODO: should I do something here?
+
+  if(in_isr()) {
+    BaseType_t xHigherPriorityTaskWoken;
+    xHigherPriorityTaskWoken = pdFALSE;
+    xQueueSendFromISR(systemTasksMsgQueue, &msg, &xHigherPriorityTaskWoken);
+    if (xHigherPriorityTaskWoken) {
+      /* Actual macro used here is port specific. */
+      portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+
+    }
+  } else {
+    xQueueSend(systemTasksMsgQueue, &msg, portMAX_DELAY);
   }
 }
 
diff --git a/src/systemtask/SystemTask.h b/src/systemtask/SystemTask.h
index ea41a69d..fa6a949c 100644
--- a/src/systemtask/SystemTask.h
+++ b/src/systemtask/SystemTask.h
@@ -6,7 +6,6 @@
 #include <task.h>
 #include <timers.h>
 #include <heartratetask/HeartRateTask.h>
-#include <components/heartrate/HeartRateController.h>
 #include <components/settings/Settings.h>
 #include <drivers/Bma421.h>
 #include <components/motion/MotionController.h>
@@ -27,6 +26,7 @@
 #endif
 
 #include "drivers/Watchdog.h"
+#include "Messages.h"
 
 namespace Pinetime {
   namespace Drivers {
@@ -40,27 +40,6 @@ namespace Pinetime {
   namespace System {
     class SystemTask {
     public:
-      enum class Messages {
-        GoToSleep,
-        GoToRunning,
-        TouchWakeUp,
-        OnNewTime,
-        OnNewNotification,
-        OnTimerDone,
-        OnNewCall,
-        BleConnected,
-        UpdateTimeOut,
-        BleFirmwareUpdateStarted,
-        BleFirmwareUpdateFinished,
-        OnTouchEvent,
-        OnButtonEvent,
-        OnDisplayTaskSleeping,
-        EnableSleeping,
-        DisableSleeping,
-        OnNewDay,
-        OnChargingEvent
-      };
-
       SystemTask(Drivers::SpiMaster& spi,
                  Drivers::St7789& lcd,
                  Pinetime::Drivers::SpiNorFlash& spiNorFlash,
@@ -69,10 +48,18 @@ namespace Pinetime {
                  Components::LittleVgl& lvgl,
                  Controllers::Battery& batteryController,
                  Controllers::Ble& bleController,
+                 Controllers::DateTime& dateTimeController,
+                 Controllers::TimerController& timerController,
+                 Drivers::Watchdog& watchdog,
+                 Pinetime::Controllers::NotificationManager& notificationManager,
                  Pinetime::Controllers::MotorController& motorController,
                  Pinetime::Drivers::Hrs3300& heartRateSensor,
+                 Pinetime::Controllers::MotionController& motionController,
                  Pinetime::Drivers::Bma421& motionSensor,
-                 Controllers::Settings& settingsController);
+                 Controllers::Settings& settingsController,
+                 Pinetime::Controllers::HeartRateController& heartRateController,
+                 Pinetime::Applications::DisplayApp& displayApp,
+                 Pinetime::Applications::HeartRateTask& heartRateApp);
 
       void Start();
       void PushMessage(Messages msg);
@@ -96,27 +83,29 @@ namespace Pinetime {
       Pinetime::Drivers::Cst816S& touchPanel;
       Pinetime::Components::LittleVgl& lvgl;
       Pinetime::Controllers::Battery& batteryController;
-      std::unique_ptr<Pinetime::Applications::DisplayApp> displayApp;
-      Pinetime::Controllers::HeartRateController heartRateController;
-      std::unique_ptr<Pinetime::Applications::HeartRateTask> heartRateApp;
+
 
       Pinetime::Controllers::Ble& bleController;
-      Pinetime::Controllers::DateTime dateTimeController;
-      Pinetime::Controllers::TimerController timerController;
+      Pinetime::Controllers::DateTime& dateTimeController;
+      Pinetime::Controllers::TimerController& timerController;
       QueueHandle_t systemTasksMsgQueue;
       std::atomic<bool> isSleeping {false};
       std::atomic<bool> isGoingToSleep {false};
       std::atomic<bool> isWakingUp {false};
-      Pinetime::Drivers::Watchdog watchdog;
-      Pinetime::Drivers::WatchdogView watchdogView;
-      Pinetime::Controllers::NotificationManager notificationManager;
+      Pinetime::Drivers::Watchdog& watchdog;
+      Pinetime::Controllers::NotificationManager& notificationManager;
       Pinetime::Controllers::MotorController& motorController;
       Pinetime::Drivers::Hrs3300& heartRateSensor;
       Pinetime::Drivers::Bma421& motionSensor;
       Pinetime::Controllers::Settings& settingsController;
+      Pinetime::Controllers::HeartRateController& heartRateController;
       Pinetime::Controllers::NimbleController nimbleController;
       Controllers::BrightnessController brightnessController;
-      Pinetime::Controllers::MotionController motionController;
+      Pinetime::Controllers::MotionController& motionController;
+
+      Pinetime::Applications::DisplayApp& displayApp;
+      Pinetime::Applications::HeartRateTask& heartRateApp;
+
 
       static constexpr uint8_t pinSpiSck = 2;
       static constexpr uint8_t pinSpiMosi = 3;