From f2df0c45ef17f5a6180ecd21654c74506d7d7dd8 Mon Sep 17 00:00:00 2001
From: FintasticMan <finlay.neon.kid@gmail.com>
Date: Mon, 12 Feb 2024 11:37:34 +0100
Subject: [PATCH] lowersleep: Improve algorithm by checking wrist angle

Inspired by https://github.com/InfiniTimeOrg/InfiniTime/pull/827#issuecomment-1881580414.
---
 src/components/motion/MotionController.cpp | 24 ++++++++++++++++------
 src/components/motion/MotionController.h   |  8 +++++---
 2 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index d28378d5..72507ac5 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -40,15 +40,15 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
     service->OnNewStepCountValue(nbSteps);
   }
 
-  if (service != nullptr && (this->x != x || yHistory[0] != y || zHistory[0] != z)) {
+  if (service != nullptr && (xHistory[0] != x || yHistory[0] != y || zHistory[0] != z)) {
     service->OnNewMotionValues(x, y, z);
   }
 
   lastTime = time;
   time = xTaskGetTickCount();
 
-  lastX = this->x;
-  this->x = x;
+  xHistory++;
+  xHistory[0] = x;
   yHistory++;
   yHistory[0] = y;
   zHistory++;
@@ -67,20 +67,26 @@ MotionController::AccelStats MotionController::GetAccelStats() const {
   AccelStats stats;
 
   for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
+    stats.xMean += xHistory[histSize - i];
     stats.yMean += yHistory[histSize - i];
     stats.zMean += zHistory[histSize - i];
+    stats.prevXMean += xHistory[1 + i];
     stats.prevYMean += yHistory[1 + i];
     stats.prevZMean += zHistory[1 + i];
   }
+  stats.xMean /= AccelStats::numHistory;
   stats.yMean /= AccelStats::numHistory;
   stats.zMean /= AccelStats::numHistory;
+  stats.prevXMean /= AccelStats::numHistory;
   stats.prevYMean /= AccelStats::numHistory;
   stats.prevZMean /= AccelStats::numHistory;
 
   for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
+    stats.xVariance += (xHistory[histSize - i] - stats.xMean) * (xHistory[histSize - i] - stats.xMean);
     stats.yVariance += (yHistory[histSize - i] - stats.yMean) * (yHistory[histSize - i] - stats.yMean);
     stats.zVariance += (zHistory[histSize - i] - stats.zMean) * (zHistory[histSize - i] - stats.zMean);
   }
+  stats.xVariance /= AccelStats::numHistory;
   stats.yVariance /= AccelStats::numHistory;
   stats.zVariance /= AccelStats::numHistory;
 
@@ -93,7 +99,7 @@ bool MotionController::ShouldRaiseWake() const {
   constexpr int16_t yThresh = -64;
   constexpr int16_t rollDegreesThresh = -45;
 
-  if (x < -xThresh || x > xThresh) {
+  if (std::abs(stats.xMean) > xThresh) {
     return false;
   }
 
@@ -107,8 +113,9 @@ bool MotionController::ShouldRaiseWake() const {
 
 bool MotionController::ShouldShakeWake(uint16_t thresh) {
   /* Currently Polling at 10hz, If this ever goes faster scalar and EMA might need adjusting */
-  int32_t speed =
-    std::abs(zHistory[0] - zHistory[histSize - 1] + (yHistory[0] - yHistory[histSize - 1]) / 2 + (x - lastX) / 4) * 100 / (time - lastTime);
+  int32_t speed = std::abs(zHistory[0] - zHistory[histSize - 1] + (yHistory[0] - yHistory[histSize - 1]) / 2 +
+                           (xHistory[0] - xHistory[histSize - 1]) / 4) *
+                  100 / (time - lastTime);
   // (.2 * speed) + ((1 - .2) * accumulatedSpeed);
   accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5;
 
@@ -116,6 +123,11 @@ bool MotionController::ShouldShakeWake(uint16_t thresh) {
 }
 
 bool MotionController::ShouldLowerSleep() const {
+  if ((stats.xMean > 887 && DegreesRolled(stats.xMean, stats.zMean, stats.prevXMean, stats.prevZMean) > 30) ||
+      (stats.xMean < -887 && DegreesRolled(stats.xMean, stats.zMean, stats.prevXMean, stats.prevZMean) < -30)) {
+    return true;
+  }
+
   if (stats.yMean < 724 || DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < 30) {
     return false;
   }
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index b2e7e7fe..be0241d3 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -21,7 +21,7 @@ namespace Pinetime {
       void Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps);
 
       int16_t X() const {
-        return x;
+        return xHistory[0];
       }
 
       int16_t Y() const {
@@ -76,11 +76,14 @@ namespace Pinetime {
       struct AccelStats {
         static constexpr uint8_t numHistory = 2;
 
+        int16_t xMean = 0;
         int16_t yMean = 0;
         int16_t zMean = 0;
+        int16_t prevXMean = 0;
         int16_t prevYMean = 0;
         int16_t prevZMean = 0;
 
+        uint32_t xVariance = 0;
         uint32_t yVariance = 0;
         uint32_t zVariance = 0;
       };
@@ -89,9 +92,8 @@ namespace Pinetime {
 
       AccelStats stats = {};
 
-      int16_t lastX = 0;
-      int16_t x = 0;
       static constexpr uint8_t histSize = 8;
+      Utility::CircularBuffer<int16_t, histSize> xHistory = {};
       Utility::CircularBuffer<int16_t, histSize> yHistory = {};
       Utility::CircularBuffer<int16_t, histSize> zHistory = {};
       int32_t accumulatedSpeed = 0;