From 6ca0fb07ea27571dfa23c73c93761a34cab11f43 Mon Sep 17 00:00:00 2001
From: Peter Cai <peter@typeblog.net>
Date: Wed, 30 Jun 2021 19:48:02 +0800
Subject: [PATCH] motor_control: refactor: cache next_tick_half_micros

---
 motor_control.cpp | 27 +++++++++++++++++----------
 motor_control.h   |  5 +++--
 2 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/motor_control.cpp b/motor_control.cpp
index 4f668d6..ec41e7c 100644
--- a/motor_control.cpp
+++ b/motor_control.cpp
@@ -33,13 +33,20 @@ void TickController::CalculatePeriod() {
     period_with_bend = (unsigned int) (((float) period_half_micros) * pgm_read_float_near(midi_pitch_bend_scale + _bend));
 }
 
-unsigned int TickController::GetTruePeriod() {
-    return period_with_bend;
+unsigned long TickController::NextTick() {
+    return NextTickWithTs(timer2.get_count());
+}
+
+unsigned long TickController::NextTickWithTs(unsigned long cur_half_micros) {
+    if (period_with_bend == 0) {
+        return 0;
+    }
+    return cur_half_micros + period_with_bend;
 }
 
 MotorControl::MotorControl(int pin_dir, int pin_step) :
     pin_dir(pin_dir), pin_step(pin_step),
-    last_tick_half_micros(0)
+    next_tick_half_micros(0)
 {
     // No actual constructor logic -- initialization is in Init()
 }
@@ -54,8 +61,7 @@ void MotorControl::Init() {
 
 void MotorControl::TickOn(unsigned long period_half_micros) {
     tick_ctrl.SetPeriod(period_half_micros);
-    // Force a tick
-    last_tick_half_micros = 0;
+    next_tick_half_micros = tick_ctrl.NextTick();
 }
 
 void MotorControl::TickAtPitch(unsigned int midi_pitch) {
@@ -75,22 +81,23 @@ void MotorControl::TickPitchBend(int bend) {
     if (bend < 0 || bend >= 16384) return;
     
     tick_ctrl.SetBend(bend);
+    // We don't need to re-calculate next tick here, because if a note is currently on,
+    // it will eventually call NextTick() anyway
 }
 
 void MotorControl::TickOff() {
     tick_ctrl.SetPeriod(0);
+    next_tick_half_micros = tick_ctrl.NextTick();
 }
 
 void MotorControl::Tick(unsigned long cur_half_micros) {
-    unsigned long period = tick_ctrl.GetTruePeriod();
-
-    if (period == 0) {
+    if (next_tick_half_micros == 0) {
         return;
     }
 
-    if (last_tick_half_micros == 0 || cur_half_micros >= last_tick_half_micros + period) {
+    if (cur_half_micros >= next_tick_half_micros) {
         DoTick();
-        last_tick_half_micros = cur_half_micros;
+        next_tick_half_micros = tick_ctrl.NextTickWithTs(cur_half_micros);
     }
 }
 
diff --git a/motor_control.h b/motor_control.h
index 73f13a1..b0ddf05 100644
--- a/motor_control.h
+++ b/motor_control.h
@@ -16,14 +16,15 @@ class TickController {
         void SetBend(unsigned int bend);
         // In units of half microseconds
         // 0 = disabled
-        unsigned int GetTruePeriod();
+        unsigned long NextTick();
+        unsigned long NextTickWithTs(unsigned long cur_half_micros);
 };
 
 class MotorControl {
     private:
         // Control pins of the stepper motor
         int pin_dir, pin_step;
-        unsigned long last_tick_half_micros;
+        unsigned long next_tick_half_micros;
         TickController tick_ctrl;
 
         void DoTick();