From 4d0cc666f60718d3263431577d2bc60ac560568d Mon Sep 17 00:00:00 2001
From: Aadi Desai <21363892+supleed2@users.noreply.github.com>
Date: Fri, 25 Mar 2022 14:55:43 +0000
Subject: [PATCH] Fix Encoder Driver

---
 lib/knob/knob     | 13 +++++---
 lib/knob/knob.cpp | 83 +++++++++++++++++++++++++++++++----------------
 2 files changed, 64 insertions(+), 32 deletions(-)

diff --git a/lib/knob/knob b/lib/knob/knob
index 9c035a1..632bfa1 100644
--- a/lib/knob/knob
+++ b/lib/knob/knob
@@ -3,13 +3,18 @@
 
 class Knob {
   private:
-	int rotation;
-	int minimum, maximum;
+	int rotation, rotationInternal;
+	int minimum, maximum, previousRotation;
 	bool A, B;
-	bool rotPlusOnePrev, rotMinOnePrev;
+	enum PrevRot {
+		NONE,
+		CW,
+		ACW,
+	};
 
   public:
-	Knob(int minimum, int max);
+	Knob(int minimum, int maximum, int initialRotation);
+	Knob(int minimum, int maximum) : Knob(minimum, maximum, minimum) {} // Delegate to full constructor, using minimum as initial rotation
 
 	int getRotation();
 
diff --git a/lib/knob/knob.cpp b/lib/knob/knob.cpp
index 25bf475..4550afb 100644
--- a/lib/knob/knob.cpp
+++ b/lib/knob/knob.cpp
@@ -1,13 +1,13 @@
 #include <knob>
 
-Knob::Knob(int minimum, int maximum) {
+Knob::Knob(int minimum, int maximum, int initialRotation) {
 	Knob::minimum = minimum;
 	Knob::maximum = maximum;
 	Knob::A = false;
 	Knob::B = false;
-	Knob::rotPlusOnePrev = false;
-	Knob::rotMinOnePrev = false;
-	Knob::rotation = 0;
+	Knob::previousRotation = NONE;
+	Knob::rotation = initialRotation;
+	Knob::rotationInternal = initialRotation;
 }
 
 int Knob::getRotation() {
@@ -15,34 +15,61 @@ int Knob::getRotation() {
 };
 
 void Knob::updateRotation(bool ANew, bool BNew) {
-	bool rotPlusOneNew = (!B && !A && !BNew && ANew) ||
-						 (!B && A && BNew && ANew) ||
-						 (B && !A && !BNew && !ANew) ||
-						 (B && A && BNew && !ANew);
+	if (A == ANew && B == BNew)
+		return; // No change, do not update values
 
-	bool rotMinOneNew = (!B && !A && BNew && !ANew) ||
-						(!B && A && !BNew && !ANew) ||
-						(B && !A && BNew && ANew) ||
-						(B && A && !BNew && ANew);
+	bool fullstep = (BNew && ANew) || (!BNew && !ANew);
 
-	bool impossibleState = (!B && !A && BNew && ANew) ||
-						   (!B && A && BNew && !ANew) ||
-						   (B && !A && !BNew && ANew) ||
-						   (B && A && !BNew && !ANew);
+	bool cwRot = (!B && !A && !BNew && ANew) ||
+				 (!B && A && BNew && ANew) ||
+				 (B && !A && !BNew && !ANew) ||
+				 (B && A && BNew && !ANew);
 
-	if (rotPlusOneNew || (impossibleState && rotPlusOnePrev))
-		rotation += 2;
-	if (rotMinOneNew || (impossibleState && rotMinOnePrev))
-		rotation -= 2;
-	if (rotation < minimum)
-		rotation = minimum;
-	if (rotation > maximum)
-		rotation = maximum;
+	bool acwRot = (!B && !A && BNew && !ANew) ||
+				  (!B && A && !BNew && !ANew) ||
+				  (B && !A && BNew && ANew) ||
+				  (B && A && !BNew && ANew);
 
+	bool impossible = (!B && !A && BNew && ANew) ||
+					  (!B && A && BNew && !ANew) ||
+					  (B && !A && !BNew && ANew) ||
+					  (B && A && !BNew && !ANew);
+
+	if (cwRot) {
+		if (previousRotation == CW && fullstep) {
+			rotationInternal++;
+		} else if (previousRotation == ACW) {
+			previousRotation = NONE;
+		} else {
+			previousRotation = CW;
+		}
+	} else if (acwRot) {
+		if (previousRotation == ACW && fullstep) {
+			rotationInternal--;
+		} else if (previousRotation == CW) {
+			previousRotation = NONE;
+		} else {
+			previousRotation = ACW;
+		}
+	} else if (impossible) {
+		if (fullstep) {
+			switch (previousRotation) {
+				case NONE: // Step skipped and no previous direction
+					break;
+				case CW:
+					rotationInternal++;
+					break;
+				case ACW:
+					rotationInternal--;
+					break;
+			}
+		}
+	}
 	A = ANew;
 	B = BNew;
-	if (!impossibleState) {
-		rotPlusOnePrev = rotPlusOneNew;
-		rotMinOnePrev = rotMinOneNew;
-	}
+	if (rotationInternal < minimum)
+		rotationInternal = minimum;
+	if (rotationInternal > maximum)
+		rotationInternal = maximum;
+	rotation = rotationInternal;
 }
\ No newline at end of file