From bb8f651481b1c0ea96898936a88472c84f3cefb7 Mon Sep 17 00:00:00 2001
From: Peter Cai <peter@typeblog.net>
Date: Tue, 14 Jun 2022 18:36:07 -0400
Subject: [PATCH] Improve raise-to-wake algorithm

From <https://github.com/InfiniTimeOrg/InfiniTime/pull/826>, with
constants tuned a bit for my personal preference.
---
 wasp/wasp.py | 42 +++++++++++++++++++++++++-----------------
 1 file changed, 25 insertions(+), 17 deletions(-)

diff --git a/wasp/wasp.py b/wasp/wasp.py
index 64bd322..187dd56 100644
--- a/wasp/wasp.py
+++ b/wasp/wasp.py
@@ -26,6 +26,12 @@ from apps.launcher import LauncherApp
 from apps.pager import PagerApp, CrashApp, NotificationApp
 from apps.steps import StepCounterApp
 
+RAISE_WAKE_Y_SWITCH_THRESHOLD = -1536
+RAISE_WAKE_SPEED_MODIFIER = 8
+RAISE_WAKE_X_THRESHOLD = 512
+RAISE_WAKE_Y_THRESHOLD = 0
+RAISE_WAKE_REQUIRED_SPEED = 512
+
 class EventType():
     """Enumerated interface actions.
 
@@ -115,7 +121,8 @@ class Manager():
         self.weatherinfo = {}
         self.units = "Metric"
         self.raise_wake = False
-        self.last_raise_y = 0
+        self.raise_wake_last_y = sys.maxsize
+        self.raise_wake_last_z = sys.maxsize
         self.accel_poll_ms = 100
         self.accel_poll_expiry = 0
 
@@ -510,23 +517,24 @@ class Manager():
 
         y = -y
 
-        if (x + 335) <= 670 and z < 0:
-            if self.sleep_at:
-                if y <= 0:
-                    return False
-                else:
-                    self.last_raise_y = 0
-                    return False
-
-            if y >= 0:
-                self.last_raise_y = 0
-                return False
-
-            if y + 230 < self.last_raise_y:
-                self.last_raise_y = y
-                return True
-
+        if self.raise_wake_last_y == sys.maxsize:
+            self.raise_wake_last_y = y
+            self.raise_wake_last_z = z
             return False
+
+        delta_y = y - self.raise_wake_last_y
+        delta_z = z - self.raise_wake_last_z
+
+        self.raise_wake_last_y = y
+        self.raise_wake_last_z = z
+
+        if y < RAISE_WAKE_Y_SWITCH_THRESHOLD:
+            return delta_z > RAISE_WAKE_REQUIRED_SPEED
+
+        if z > 0:
+            return delta_y > (RAISE_WAKE_REQUIRED_SPEED + (y - delta_y / 2) / RAISE_WAKE_SPEED_MODIFIER)
+
+        return delta_y < (-RAISE_WAKE_REQUIRED_SPEED - (y - delta_y / 2) / RAISE_WAKE_SPEED_MODIFIER)
     
     def run(self, no_except=True):
         """Run the system manager synchronously.