diff --git a/InfiniTime b/InfiniTime
index 9bbd2dd..e55a76f 160000
--- a/InfiniTime
+++ b/InfiniTime
@@ -1 +1 @@
-Subproject commit 9bbd2ddbc4e44742913e35e25b21b80d80efe36b
+Subproject commit e55a76f740561378479eda5ff8c32e09db9f1693
diff --git a/sim/components/motion/MotionController.cpp b/sim/components/motion/MotionController.cpp
index 331b5df..da90eae 100644
--- a/sim/components/motion/MotionController.cpp
+++ b/sim/components/motion/MotionController.cpp
@@ -1,5 +1,7 @@
 #include "components/motion/MotionController.h"
-//#include "os/os_cputime.h"
+
+//#include <task.h>
+
 using namespace Pinetime::Controllers;
 
 void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) {
@@ -10,66 +12,56 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
 //  if (service != nullptr && (this->x != x || this->y != y || this->z != z)) {
 //    service->OnNewMotionValues(x, y, z);
 //  }
+//
+//  lastTime = time;
+//  time = xTaskGetTickCount();
 
   this->x = x;
+  lastY = this->y;
   this->y = y;
+  lastZ = this->z;
   this->z = z;
+
   int32_t deltaSteps = nbSteps - this->nbSteps;
-  this->nbSteps = nbSteps;
   if (deltaSteps > 0) {
     currentTripSteps += deltaSteps;
   }
+  this->nbSteps = nbSteps;
 }
 
-bool MotionController::Should_RaiseWake(bool isSleeping) {
+bool MotionController::ShouldRaiseWake(bool isSleeping) {
   if ((x + 335) <= 670 && z < 0) {
-    if (not isSleeping) {
+    if (!isSleeping) {
       if (y <= 0) {
         return false;
-      } else {
-        lastYForWakeUp = 0;
-        return false;
       }
+      lastYForRaiseWake = 0;
+      return false;
     }
 
     if (y >= 0) {
-      lastYForWakeUp = 0;
+      lastYForRaiseWake = 0;
       return false;
     }
-    if (y + 230 < lastYForWakeUp) {
-      lastYForWakeUp = y;
+    if (y + 230 < lastYForRaiseWake) {
+      lastYForRaiseWake = y;
       return true;
     }
   }
   return false;
 }
 
-bool MotionController::Should_ShakeWake(uint16_t thresh) {
+bool MotionController::ShouldShakeWake(uint16_t thresh) {
   return false;
-//  bool wake = false;
-//  auto diff = xTaskGetTickCount() - lastShakeTime;
-//  lastShakeTime = xTaskGetTickCount();
 //  /* Currently Polling at 10hz, If this ever goes faster scalar and EMA might need adjusting */
-//  int32_t speed = std::abs(z + (y / 2) + (x / 4) - lastYForShake - lastZForShake) / diff * 100;
-//  //(.2 * speed) + ((1 - .2) * accumulatedspeed);
+//  int32_t speed = std::abs(z + (y / 2) + (x / 4) - lastY / 2 - lastZ) / (time - lastTime) * 100;
+//  //(.2 * speed) + ((1 - .2) * accumulatedSpeed);
 //  // implemented without floats as .25Alpha
-//  accumulatedspeed = (speed / 5) + ((accumulatedspeed / 5) * 4);
+//  accumulatedSpeed = (speed / 5) + ((accumulatedSpeed / 5) * 4);
 //
-//  if (accumulatedspeed > thresh) {
-//    wake = true;
-//  }
-//  lastXForShake = x / 4;
-//  lastYForShake = y / 2;
-//  lastZForShake = z;
-//  return wake;
-}
-int32_t MotionController::currentShakeSpeed() {
-  return accumulatedspeed;
+//  return accumulatedSpeed > thresh;
 }
 
-void MotionController::IsSensorOk(bool isOk) {
-  isSensorOk = isOk;
-}
 void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) {
   switch (types) {
     case Drivers::Bma421::DeviceTypes::BMA421:
@@ -83,6 +75,3 @@ void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) {
       break;
   }
 }
-//void MotionController::SetService(Pinetime::Controllers::MotionService* service) {
-//  this->service = service;
-//}
diff --git a/sim/components/motion/MotionController.h b/sim/components/motion/MotionController.h
index a16560b..fd8610a 100644
--- a/sim/components/motion/MotionController.h
+++ b/sim/components/motion/MotionController.h
@@ -1,8 +1,11 @@
 #pragma once
 
 #include <cstdint>
-#include <drivers/Bma421.h>
-//#include <components/ble/MotionService.h>
+
+//#include <FreeRTOS.h>
+
+#include "drivers/Bma421.h"
+//#include "components/ble/MotionService.h"
 
 namespace Pinetime {
   namespace Controllers {
@@ -19,12 +22,15 @@ namespace Pinetime {
       int16_t X() const {
         return x;
       }
+
       int16_t Y() const {
         return y;
       }
+
       int16_t Z() const {
         return z;
       }
+
       uint32_t NbSteps() const {
         return nbSteps;
       }
@@ -32,14 +38,22 @@ namespace Pinetime {
       void ResetTrip() {
         currentTripSteps = 0;
       }
+
       uint32_t GetTripSteps() const {
         return currentTripSteps;
       }
 
-      bool Should_ShakeWake(uint16_t thresh);
-      bool Should_RaiseWake(bool isSleeping);
-      int32_t currentShakeSpeed();
-      void IsSensorOk(bool isOk);
+      bool ShouldShakeWake(uint16_t thresh);
+      bool ShouldRaiseWake(bool isSleeping);
+
+      int32_t CurrentShakeSpeed() const {
+        return accumulatedSpeed;
+      }
+
+      void IsSensorOk(bool isOk) {
+        isSensorOk = isOk;
+      }
+
       bool IsSensorOk() const {
         return isSensorOk;
       }
@@ -49,24 +63,29 @@ namespace Pinetime {
       }
 
       void Init(Pinetime::Drivers::Bma421::DeviceTypes types);
-//      void SetService(Pinetime::Controllers::MotionService* service);
+
+//      void SetService(Pinetime::Controllers::MotionService* service) {
+//        this->service = service;
+//      }
 
     private:
-      uint32_t nbSteps;
+      uint32_t nbSteps = 0;
       uint32_t currentTripSteps = 0;
-      int16_t x;
-      int16_t y;
-      int16_t z;
-      int16_t lastYForWakeUp = 0;
+
+//      TickType_t lastTime = 0;
+//      TickType_t time = 0;
+
+      int16_t x = 0;
+      int16_t lastYForRaiseWake = 0;
+      int16_t lastY = 0;
+      int16_t y = 0;
+      int16_t lastZ = 0;
+      int16_t z = 0;
+      int32_t accumulatedSpeed = 0;
+
       bool isSensorOk = false;
       DeviceTypes deviceType = DeviceTypes::Unknown;
 //      Pinetime::Controllers::MotionService* service = nullptr;
-
-      int16_t lastXForShake = 0;
-      int16_t lastYForShake = 0;
-      int16_t lastZForShake = 0;
-      int32_t accumulatedspeed = 0;
-      uint32_t lastShakeTime = 0;
     };
   }
-}
\ No newline at end of file
+}