From db909398e0a36f89c8afe115f1bf57baf75662ac Mon Sep 17 00:00:00 2001
From: Dean Brettle <dean@brettle.com>
Date: Sat, 20 Apr 2024 19:01:16 -0700
Subject: [PATCH] Update simulated CANCoder position only when it changes.

Add associated unit test.
---
 .../lib199/sim/MockedCANCoder.java            | 16 ++++---
 .../lib199/sim/MockedCANCoderTest.java        | 43 +++++++++++++++++++
 2 files changed, 54 insertions(+), 5 deletions(-)
 create mode 100644 src/test/java/org/carlmontrobotics/lib199/sim/MockedCANCoderTest.java

diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java
index a86af265..ee39a9c5 100644
--- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java
+++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java
@@ -7,9 +7,12 @@
 
 import org.carlmontrobotics.lib199.Lib199Subsystem;
 
+import edu.wpi.first.hal.HALValue;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.simulation.SimValueCallback;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
 
 public class MockedCANCoder {
 
@@ -19,6 +22,7 @@ public class MockedCANCoder {
 
     private int port;
     private SimDevice device;
+    private SimDeviceSim deviceSim;
     private SimDouble position; // Rotations - Continuous
     private SimDouble gearing;
     private CANcoderSimState sim;
@@ -29,14 +33,16 @@ public MockedCANCoder(CANcoder canCoder) {
         position = device.createDouble("count", Direction.kInput, 0);
         gearing = device.createDouble("gearing", Direction.kOutput, 1);
         sim = canCoder.getSimState();
-        Lib199Subsystem.registerAsyncSimulationPeriodic(this::update);
+        deviceSim = new SimDeviceSim("CANCoder", port);
+        deviceSim.registerValueChangedCallback(position, new SimValueCallback() {
+            @Override
+            public void callback(String name, int handle, int direction, HALValue value) {
+                sim.setRawPosition(value.getDouble() / kCANCoderCPR);
+            }
+        }, true);
         sims.put(port, this);
     }
 
-    public void update() {
-        sim.setRawPosition(position.get() / kCANCoderCPR);
-    }
-
     public void setGearing(double gearing) {
         this.gearing.set(gearing);
     }
diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedCANCoderTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedCANCoderTest.java
new file mode 100644
index 00000000..faf4c6f1
--- /dev/null
+++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedCANCoderTest.java
@@ -0,0 +1,43 @@
+package org.carlmontrobotics.lib199.sim;
+
+import org.carlmontrobotics.lib199.testUtils.TestRules;
+import org.junit.ClassRule;
+import org.junit.Rule;
+import org.junit.Test;
+
+import static org.hamcrest.CoreMatchers.*;
+import static org.hamcrest.MatcherAssert.*;
+import static org.junit.Assert.assertEquals;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.sim.CANcoderSimState;
+
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
+
+public class MockedCANCoderTest {
+    @ClassRule
+    public static TestRules.InitializeHAL simClassRule = new TestRules.InitializeHAL(); 
+    @Rule
+    public TestRules.ResetSimDeviceSimData simTestRule = new TestRules.ResetSimDeviceSimData(); 
+
+    @Test
+    public void testCountUpdatesPosition() throws InterruptedException {
+        CANcoder canCoder = new CANcoder(0);
+
+        // Set up a SimDevice for the CANcoder
+        new MockedCANCoder(canCoder);
+
+        // Consider the current position to be 0 rotations.
+        canCoder.setPosition(0.0);
+
+        // Set the position to 0.42 rotations via the SimDevice interface
+        SimDeviceSim canCoderSim = new SimDeviceSim("CANCoder", 0);
+        canCoderSim.getDouble("count").set(0.42 * MockedCANCoder.kCANCoderCPR);
+
+        // Wait for the CANCoder to update the position. This appears to happen on a separate thread.
+        canCoder.getPosition().waitForUpdate(1.0);
+
+        assertEquals(0.42, canCoder.getPosition().getValueAsDouble(), 0.001);
+    }
+
+}