From 9c375be80cb87418e1b36284cb297168d26ff523 Mon Sep 17 00:00:00 2001
From: Finlay Davidson <finlay.davidson@coderclass.nl>
Date: Thu, 28 Apr 2022 15:53:18 +0200
Subject: [PATCH] Replace lastY with circular buffer to prevent turning off
 when shaken

---
 src/components/motion/MotionController.cpp | 17 +++++++++++++++--
 src/components/motion/MotionController.h   |  7 +++----
 src/displayapp/screens/Motion.cpp          |  8 --------
 src/displayapp/screens/Motion.h            |  1 -
 4 files changed, 18 insertions(+), 15 deletions(-)

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index 2386327995..dc3fdc0d53 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -11,7 +11,9 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
     service->OnNewMotionValues(x, y, z);
   }
 
-  lastY = this->y;
+  lastYIndex++;
+  lastYIndex %= lastYs.size();
+  lastYs.at(lastYIndex) = this->y;
   this->x = x;
   this->y = y;
   this->z = z;
@@ -68,7 +70,18 @@ int32_t MotionController::currentShakeSpeed() {
 }
 
 bool MotionController::ShouldLowerSleep() const {
-  return y >= 512 && y >= lastY + 192;
+  if (lastYs.at((lastYIndex + 2) % lastYs.size()) < lastYs.at((lastYIndex + 1) % lastYs.size()) + 192 ||
+      lastYs.at((lastYIndex + 2) % lastYs.size()) < 512) {
+    return false;
+  }
+
+  for (uint8_t i = 3; i < lastYs.size(); i++) {
+    if (lastYs.at((lastYIndex + i) % lastYs.size()) < 0) {
+      return false;
+    }
+  }
+
+  return true;
 }
 
 void MotionController::IsSensorOk(bool isOk) {
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index 1c9364af6f..bef5278641 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cstdint>
+#include <array>
 #include <drivers/Bma421.h>
 #include <components/ble/MotionService.h>
 
@@ -25,9 +26,6 @@ namespace Pinetime {
       int16_t Z() const {
         return z;
       }
-      int16_t LastY() const {
-        return lastY;
-      }
       uint32_t NbSteps() const {
         return nbSteps;
       }
@@ -61,7 +59,8 @@ namespace Pinetime {
       int16_t x = 0;
       int16_t y = 0;
       int16_t z = 0;
-      int16_t lastY = 0;
+      std::array<int16_t, 8> lastYs = {};
+      uint8_t lastYIndex = 0;
       int16_t lastYForWakeUp = 0;
       bool isSensorOk = false;
       DeviceTypes deviceType = DeviceTypes::Unknown;
diff --git a/src/displayapp/screens/Motion.cpp b/src/displayapp/screens/Motion.cpp
index 6afbdd4645..f7ffcc7c0f 100644
--- a/src/displayapp/screens/Motion.cpp
+++ b/src/displayapp/screens/Motion.cpp
@@ -37,11 +37,6 @@ Motion::Motion(Pinetime::Applications::DisplayApp* app, Controllers::MotionContr
   lv_obj_align(labelStep, chart, LV_ALIGN_IN_BOTTOM_LEFT, 0, 0);
   lv_label_set_text_static(labelStep, "Steps ---");
 
-  labelLastY = lv_label_create(lv_scr_act(), NULL);
-  lv_label_set_text(labelLastY, "LastY 0");
-  lv_label_set_align(labelLastY, LV_LABEL_ALIGN_RIGHT);
-  lv_obj_align(labelLastY, chart, LV_ALIGN_IN_BOTTOM_RIGHT, 0, 0);
-
   taskRefresh = lv_task_create(RefreshTaskCallback, LV_DISP_DEF_REFR_PERIOD, LV_TASK_PRIO_MID, this);
 }
 
@@ -63,7 +58,4 @@ void Motion::Refresh() {
                         motionController.Y() / 0x10,
                         motionController.Z() / 0x10);
   lv_obj_align(label, NULL, LV_ALIGN_IN_TOP_MID, 0, 10);
-
-  lv_label_set_text_fmt(labelLastY, "LastY %d", motionController.LastY() / 0x10);
-  lv_obj_align(labelLastY, chart, LV_ALIGN_IN_BOTTOM_RIGHT, 0, 0);
 }
diff --git a/src/displayapp/screens/Motion.h b/src/displayapp/screens/Motion.h
index 315660c6f0..4d2bd4f2e6 100644
--- a/src/displayapp/screens/Motion.h
+++ b/src/displayapp/screens/Motion.h
@@ -27,7 +27,6 @@ namespace Pinetime {
         lv_obj_t* label;
 
         lv_obj_t* labelStep;
-        lv_obj_t* labelLastY;
         lv_task_t* taskRefresh;
       };
     }