From f78d1d43402f218cd018a7036dd8b1406163e0bb Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Thu, 10 Dec 2020 20:30:12 -0800
Subject: [PATCH] [sim] Process WS Encoder reset internally (#2927)

Currently, Encoder.reset() must make a round trip to the sensor and back
in order for the count to be updated for the user program.  As the sim layer
also resets the internal encoder count, this creates a race condition (a WS
message with a new count can be "in flight" during a reset and update the
count).

This changes the WS layer to not put reset on the wire, but instead keep an
offset count internal to the robot program.  The value on the wire is not
reset, but rather all sends and receives are adjusted as necessary to the
internal robot count.

This approach is straightforward, but does result in the value on the wire
not matching the value in the user program.  A future improvement will fix
this, but this change fixes the immediate race condition problem.
---
 hal/src/main/native/sim/Encoder.cpp           |  2 +-
 .../main/native/cpp/WSProvider_Encoder.cpp    | 25 ++++++++++++++++---
 .../main/native/include/WSProvider_Encoder.h  |  2 ++
 3 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/hal/src/main/native/sim/Encoder.cpp b/hal/src/main/native/sim/Encoder.cpp
index 32416f0d7a..3f4e487fcf 100644
--- a/hal/src/main/native/sim/Encoder.cpp
+++ b/hal/src/main/native/sim/Encoder.cpp
@@ -174,9 +174,9 @@ void HAL_ResetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
     return;
   }
 
+  SimEncoderData[encoder->index].reset = true;
   SimEncoderData[encoder->index].count = 0;
   SimEncoderData[encoder->index].period = std::numeric_limits<double>::max();
-  SimEncoderData[encoder->index].reset = true;
 }
 double HAL_GetEncoderPeriod(HAL_EncoderHandle encoderHandle, int32_t* status) {
   auto encoder = encoderHandles->Get(encoderHandle);
diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp
index 58d47d06a2..8f7e8e77e2 100644
--- a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp
@@ -49,9 +49,27 @@ void HALSimWSProviderEncoder::RegisterCallbacks() {
         provider->ProcessHalCallback(payload);
       },
       this, true);
-  m_countCbKey = REGISTER(Count, ">count", int32_t, int);
+  m_countCbKey = HALSIM_RegisterEncoderCountCallback(
+      m_channel,
+      [](const char* name, void* param, const struct HAL_Value* value) {
+        auto provider = static_cast<HALSimWSProviderEncoder*>(param);
+        provider->ProcessHalCallback(
+            {{">count", static_cast<int32_t>(value->data.v_int +
+                                             provider->m_countOffset)}});
+      },
+      this, true);
   m_periodCbKey = REGISTER(Period, ">period", double, double);
-  m_resetCbKey = REGISTER(Reset, "<reset", bool, boolean);
+  m_resetCbKey = HALSIM_RegisterEncoderResetCallback(
+      m_channel,
+      [](const char* name, void* param, const struct HAL_Value* value) {
+        auto provider = static_cast<HALSimWSProviderEncoder*>(param);
+        bool reset = static_cast<bool>(value->data.v_boolean);
+        if (reset) {
+          provider->m_countOffset +=
+              HALSIM_GetEncoderCount(provider->m_channel);
+        }
+      },
+      this, true);
   m_reverseDirectionCbKey =
       REGISTER(ReverseDirection, "<reverse_direction", bool, boolean);
   m_samplesCbKey = REGISTER(SamplesToAverage, "<samples_to_avg", int32_t, int);
@@ -79,7 +97,8 @@ void HALSimWSProviderEncoder::DoCancelCallbacks() {
 void HALSimWSProviderEncoder::OnNetValueChanged(const wpi::json& json) {
   wpi::json::const_iterator it;
   if ((it = json.find(">count")) != json.end()) {
-    HALSIM_SetEncoderCount(m_channel, it.value());
+    HALSIM_SetEncoderCount(m_channel,
+                           static_cast<int32_t>(it.value()) - m_countOffset);
   }
   if ((it = json.find(">period")) != json.end()) {
     HALSIM_SetEncoderPeriod(m_channel, it.value());
diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h
index 882e447da6..b5728c0b9d 100644
--- a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h
@@ -34,6 +34,8 @@ class HALSimWSProviderEncoder : public HALSimWSHalChanProvider {
   int32_t m_resetCbKey = 0;
   int32_t m_reverseDirectionCbKey = 0;
   int32_t m_samplesCbKey = 0;
+
+  int32_t m_countOffset = 0;
 };
 
 }  // namespace wpilibws