2 files changed, 58 insertions, 9 deletions

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index cae49105..7dd32127 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -1,5 +1,5 @@
 #include "components/motion/MotionController.h"
-
+#include "os/os_cputime.h"
 using namespace Pinetime::Controllers;
 
 void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) {
@@ -7,17 +7,21 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
     service->OnNewStepCountValue(nbSteps);
   }
 
-  if(service != nullptr && (this->x != x || this->y != y || this->z != z)) {
+  if (service != nullptr && (this->x != x || this->y != y || this->z != z)) {
     service->OnNewMotionValues(x, y, z);
   }
 
   this->x = x;
   this->y = y;
   this->z = z;
+  int32_t deltaSteps = nbSteps - this->nbSteps;
   this->nbSteps = nbSteps;
+  if (deltaSteps > 0) {
+    currentTripSteps += deltaSteps;
+  }
 }
 
-bool MotionController::ShouldWakeUp(bool isSleeping) {
+bool MotionController::Should_RaiseWake(bool isSleeping) {
   if ((x + 335) <= 670 && z < 0) {
     if (not isSleeping) {
       if (y <= 0) {
@@ -39,14 +43,43 @@ bool MotionController::ShouldWakeUp(bool isSleeping) {
   }
   return false;
 }
+
+bool MotionController::Should_ShakeWake(uint16_t thresh) {
+  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);
+  // implemented without floats as .25Alpha
+  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;
+}
+
 void MotionController::IsSensorOk(bool isOk) {
   isSensorOk = isOk;
 }
 void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) {
-  switch(types){
-    case Drivers::Bma421::DeviceTypes::BMA421: this->deviceType = DeviceTypes::BMA421; break;
-    case Drivers::Bma421::DeviceTypes::BMA425: this->deviceType = DeviceTypes::BMA425; break;
-    default: this->deviceType = DeviceTypes::Unknown; break;
+  switch (types) {
+    case Drivers::Bma421::DeviceTypes::BMA421:
+      this->deviceType = DeviceTypes::BMA421;
+      break;
+    case Drivers::Bma421::DeviceTypes::BMA425:
+      this->deviceType = DeviceTypes::BMA425;
+      break;
+    default:
+      this->deviceType = DeviceTypes::Unknown;
+      break;
   }
 }
 void MotionController::SetService(Pinetime::Controllers::MotionService* service) {
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index c72d8a4a..f80b11b9 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -8,7 +8,7 @@ namespace Pinetime {
   namespace Controllers {
     class MotionController {
     public:
-      enum class DeviceTypes{
+      enum class DeviceTypes {
         Unknown,
         BMA421,
         BMA425,
@@ -28,8 +28,17 @@ namespace Pinetime {
       uint32_t NbSteps() const {
         return nbSteps;
       }
-      bool ShouldWakeUp(bool isSleeping);
 
+      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 IsSensorOk() const {
         return isSensorOk;
@@ -44,6 +53,7 @@ namespace Pinetime {
 
     private:
       uint32_t nbSteps;
+      uint32_t currentTripSteps = 0;
       int16_t x;
       int16_t y;
       int16_t z;
@@ -51,6 +61,12 @@ namespace Pinetime {
       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