Game complete hopefully

5 files changed, 138 insertions, 26 deletions

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index 6a3a84d..0404e3f 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -1,4 +1,3 @@
-#include <cmath>
 #include "components/motion/MotionController.h"
 #include "os/os_cputime.h"
 using namespace Pinetime::Controllers;
@@ -9,14 +8,6 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z) {
   this->z = z;
 }
 
-uint16_t MotionController::G() {
-  float X = x; X *= X;
-  float Y = y; X += Y * Y;
-  Y = z; X += Y * Y;
-  X = std::sqrt(X);
-  if (X > 0xffff) { return 0xffff; } else { return X; }
-}
-
 bool MotionController::Should_RaiseWake(bool isSleeping) {
   if ((x + 335) <= 670 && z < 0) {
     if (not isSleeping) {
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index 21bfbdb..06429c8 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cmath>
 #include <cstdint>
 #include <drivers/Bma421.h>
 
@@ -25,8 +26,6 @@ namespace Pinetime {
         return z;
       }
 
-      int16_t G();
-
       bool Should_RaiseWake(bool isSleeping);
       void IsSensorOk(bool isOk);
       bool IsSensorOk() const {
@@ -39,6 +38,30 @@ namespace Pinetime {
 
       void Init(Pinetime::Drivers::Bma421::DeviceTypes types);
 
+      inline double GXYZ(double &X, double &Y, double &Z) const {
+        X = x; Y = y; Z = z;
+        return std::sqrt(X*X + Y*Y + Z*Z);
+      }
+
+      inline double G() const {
+        double X = x; X *= X;
+        double Y = y; X += Y * Y;
+        Y = z; X += Y * Y;
+        return std::sqrt(X) * 0.;
+      }
+
+      inline auto G_v0() const {
+        float X = x; X *= X;
+        float Y = y; X += Y * Y;
+        Y = z; X += Y * Y;
+        return std::sqrt(X);
+      }
+
+      inline uint16_t G_uint16() const {
+        auto X = G_v0();
+        if (X > 0xffff) { return 0xffff; } else { return X; }
+      }
+
     private:
       int16_t x;
       int16_t y;
diff --git a/src/displayapp/DisplayApp.cpp b/src/displayapp/DisplayApp.cpp
index 223b150..cb381aa 100644
--- a/src/displayapp/DisplayApp.cpp
+++ b/src/displayapp/DisplayApp.cpp
@@ -389,7 +389,7 @@ void DisplayApp::LoadApp(Apps app, DisplayApp::FullRefreshDirections direction)
       currentScreen = std::make_unique<Screens::StopWatch>(this, *systemTask);
       break;
     case Apps::Motion:
-      currentScreen = std::make_unique<Screens::Motion>(this, motionController);
+      currentScreen = std::make_unique<Screens::Motion>(this, *systemTask, motionController);
       break;
   }
   currentApp = app;
diff --git a/src/displayapp/screens/Motion.cpp b/src/displayapp/screens/Motion.cpp
index 00c3172..9ad6a16 100644
--- a/src/displayapp/screens/Motion.cpp
+++ b/src/displayapp/screens/Motion.cpp
@@ -2,18 +2,34 @@
 #include <lvgl/lvgl.h>
 #include "displayapp/DisplayApp.h"
 
+// #if (portTICK_PERIOD_MS == 0)
+#define APX_TICK_PERIOD_MS 1
+// #else
+// #define APX_TICK_PERIOD_MS portTICK_PERIOD_MS
+// #endif
+
+#define FRAME_HZ     100
+#define FRAME_MS     (1000 / FRAME_HZ)
+#define FRAME_TICKS  (FRAME_MS / APX_TICK_PERIOD_MS)
+#define REDRAW_FRAME_HZ     40
+#define REDRAW_FRAME_MS     (1000 / REDRAW_FRAME_HZ)
+#define REDRAW_FRAME_TICKS  (REDRAW_FRAME_MS / APX_TICK_PERIOD_MS)
+#define G_SCALE 0.001
+
 using namespace Pinetime::Applications::Screens;
 
-Motion::Motion(Pinetime::Applications::DisplayApp* app, Controllers::MotionController& motionController)
-  : Screen(app), motionController {motionController} {
+Motion::Motion(Pinetime::Applications::DisplayApp* app, System::SystemTask& systemTask, Controllers::MotionController& motionController)
+  : Screen(app), motionController {motionController}, systemTask {systemTask} {
+
+  systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
   chart = lv_chart_create(lv_scr_act(), NULL);
   lv_obj_set_size(chart, 100, 100);
-  lv_obj_align(chart, NULL, LV_ALIGN_IN_TOP_MID, 0, 0);
+  lv_obj_align(chart, NULL, LV_ALIGN_IN_BOTTOM_MID, 0, 0);
   lv_chart_set_type(chart, LV_CHART_TYPE_LINE); /*Show lines and points too*/
   // lv_chart_set_series_opa(chart, LV_OPA_70);                            /*Opacity of the data series*/
   // lv_chart_set_series_width(chart, 4);                                  /*Line width and point radious*/
 
-  lv_chart_set_range(chart, -1100, 1100);
+  lv_chart_set_range(chart, 0, 1000);
   lv_chart_set_update_mode(chart, LV_CHART_UPDATE_MODE_SHIFT);
   lv_chart_set_point_count(chart, 6);
 
@@ -31,28 +47,94 @@ Motion::Motion(Pinetime::Applications::DisplayApp* app, Controllers::MotionContr
   lv_label_set_text_static(label, labelText);
   // lv_label_set_text_fmt(label, "X #FF0000 %d# Y #008000 %d# Z #FFFF00 %d#", 0, 0, 0);
   // lv_label_set_align(label, LV_LABEL_ALIGN_CENTER);
-  // lv_obj_align(label, NULL, LV_ALIGN_IN_TOP_MID, 0, 10);
+  lv_obj_align(label, NULL, LV_ALIGN_IN_TOP_MID, 0, 0);
   // lv_label_set_recolor(label, true);
 
-  taskRefresh = lv_task_create(RefreshTaskCallback, LV_DISP_DEF_REFR_PERIOD, LV_TASK_PRIO_MID, this);
+  lastLabel = lv_label_create(lv_scr_act(), NULL);
+  lv_obj_align(lastLabel, label, LV_ALIGN_IN_TOP_MID, 0, 0);
+
+  recordLabel = lv_label_create(lv_scr_act(), NULL);
+  lv_obj_align(recordLabel, lastLabel, LV_ALIGN_IN_TOP_MID, 0, 0);
+
+  taskRefresh = lv_task_create(RefreshTaskCallback, FRAME_MS, LV_TASK_PRIO_HIGH, this);
 }
 
 Motion::~Motion() {
   lv_task_del(taskRefresh);
   lv_obj_clean(lv_scr_act());
+  systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping);
 }
 
 void Motion::Refresh() {
-  uint16_t G = motionController.G()
-  lv_chart_set_next(chart, ser1, motionController.G());
+  bool new_record = false;
+  bool jump_started_or_ended = false;
+  double X; double Y; double_t Z;
+  double G = motionController.GXYZ(X,Y,Z);
   // lv_chart_set_next(chart, ser2, motionController.Y());
   // lv_chart_set_next(chart, ser3, motionController.Z());
-  labelText[4] = '0'+(G%10); G /= 10;
-  labelText[3] = '0'+(G%10); G /= 10;
-  labelText[2] = '0'+(G%10); G /= 10;
-  labelText[1] = '0'+(G%10); G /= 10;
+  uint16_t current_time = xTaskGetTickCount();
+  if (started) {
+    uint16_t current_frame_ms = current_time - last_frame_time;
+    G *= G_SCALE;
+    if (G < 1) {
+      if (!jumping) {
+        jump_started_or_ended = true;
+        current_jump_speed = 0;
+        current_jump_length = 0;
+        jumping = true;
+      }
+      double current_jump_accel = 1 - G; 
+      double current_frame_s = current_frame_ms * 0.001;
+      current_jump_speed += current_jump_accel * current_frame_s;
+      current_jump_length += current_jump_speed * current_frame_s;
+    } else {
+      if (jumping) {
+        double best_jump_length = records[0].jump_length;  
+        if (current_jump_length >= best_jump_length) {
+          records[4] = records[3];
+          records[3] = records[2];
+          records[2] = records[1];
+          records[1] = records[0];
+          best_jump_length = current_jump_length;
+          records[0].jump_length = current_jump_length;
+          new_record = 1;
+        }
+        jumping = false;
+        jump_started_or_ended = true;
+      }
+    }
+  }
+  last_frame_time = current_time;
+  if (started) {
+    if (current_time - last_redraw_frame_time < REDRAW_FRAME_TICKS) { return; }
+  } else {
+    started = true;
+  }
+  last_redraw_frame_time = current_time;
+  uint16_t G_uint16 = G < 0xffff ? G : 0xffff;
+  lv_chart_set_next(chart, ser1, 1000 - ((int16_t)G_uint16));
+  labelText[4] = '0'+(G_uint16%10); G /= 10;
+  labelText[3] = '0'+(G_uint16%10); G /= 10;
+  labelText[2] = '0'+(G_uint16%10); G /= 10;
+  labelText[1] = '0'+(G_uint16%10); G /= 10;
   labelText[0] = '0'+G; G /= 10;
   lv_label_set_text_static(label, labelText);
+  if (jump_started_or_ended) {
+    if (new_record) {
+      lv_label_set_text_fmt(recordLabel, "%3.3f", current_jump_length);
+    }
+    lv_color_t color; 
+    if (jumping) {
+      color = LV_COLOR_CYAN;
+    } else {
+      double best_jump_length = records[0].jump_length;  
+      if (current_jump_length * 100 > best_jump_length) {
+        lv_label_set_text_fmt(lastLabel, "%3.3f", current_jump_length);
+      }
+      color = LV_COLOR_ORANGE;
+    }
+    lv_obj_set_style_local_text_color(lastLabel, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, color);
+  }
 #if 0
   lv_label_set_text_fmt(label,
                         "X #FF0000 %d# Y #008000 %d# Z #FFFF00 %d#",
diff --git a/src/displayapp/screens/Motion.h b/src/displayapp/screens/Motion.h
index 2341f24..249d296 100644
--- a/src/displayapp/screens/Motion.h
+++ b/src/displayapp/screens/Motion.h
@@ -2,6 +2,7 @@
 
 #include <cstdint>
 #include <chrono>
+#include "systemtask/SystemTask.h"
 #include "displayapp/screens/Screen.h"
 #include <lvgl/src/lv_core/lv_style.h>
 #include <lvgl/src/lv_core/lv_obj.h>
@@ -13,19 +14,34 @@ namespace Pinetime {
 
       class Motion : public Screen {
       public:
-        Motion(DisplayApp* app, Controllers::MotionController& motionController);
+        Motion(DisplayApp* app, System::SystemTask& systemTask, Controllers::MotionController& motionController);
         ~Motion() override;
 
         void Refresh() override;
 
       private:
         Controllers::MotionController& motionController;
-        char labelText = { '0', '0', '0', '0', '0', 0 };
+        System::SystemTask& systemTask;
+        // bool calibrating = true;
+        bool started = false;
+        bool jumping = false;
+        uint16_t last_frame_time = 0;
+        uint16_t last_redraw_frame_time = 0;
+        // uint8_t dropped_frames = 0;
+        double current_jump_length;
+        double current_jump_speed;
+        struct Record {
+          double jump_length;
+        };
+        Record records[5] = { {-1}, {-1}, {-1}, {-1}, {-1} };
+        char labelText[6] = { '0', '0', '0', '0', '0', 0 };
         lv_obj_t* chart;
         lv_chart_series_t* ser1;
         lv_chart_series_t* ser2;
         lv_chart_series_t* ser3;
         lv_obj_t* label;
+        lv_obj_t* recordLabel;
+        lv_obj_t* lastLabel;
 
         lv_obj_t* labelStep;
         lv_task_t* taskRefresh;