[wpilibj, wpilibc] Fix LED patterns not offsetting reads (#6948)

Was causing bugs when combined with patterns that need to read back from the buffer (eg masks and overlays)

Co-authored-by: Joseph Eng <s-engjo@bsd405.org>

wpilibc/src/main/native/cpp/LEDPattern.cpp

@@ -18,33 +18,46 @@
 
 using namespace frc;
 
-LEDPattern::LEDPattern(LEDPatternFn impl) : m_impl(std::move(impl)) {}
+LEDPattern::LEDPattern(std::function<void(frc::LEDPattern::LEDReader,
+                                          std::function<void(int, frc::Color)>)>
+                           impl)
+    : m_impl(std::move(impl)) {}
+
+void LEDPattern::ApplyTo(LEDPattern::LEDReader reader,
+                         std::function<void(int, frc::Color)> writer) const {
+  m_impl(reader, writer);
+}
 
 void LEDPattern::ApplyTo(std::span<AddressableLED::LEDData> data,
-                         LEDWriterFn writer) const {
-  m_impl(data, writer);
+                         std::function<void(int, frc::Color)> writer) const {
+  ApplyTo(LEDPattern::LEDReader{[=](size_t i) { return data[i]; }, data.size()},
+          writer);
 }
 
 void LEDPattern::ApplyTo(std::span<AddressableLED::LEDData> data) const {
   ApplyTo(data, [&](int index, Color color) { data[index].SetLED(color); });
 }
 
-LEDPattern LEDPattern::Reversed() {
-  return LEDPattern{[self = *this](auto data, auto writer) {
-    self.ApplyTo(data, [&](int i, Color color) {
-      writer((data.size() - 1) - i, color);
-    });
+LEDPattern LEDPattern::MapIndex(
+    std::function<size_t(size_t, size_t)> indexMapper) {
+  return LEDPattern{[self = *this, indexMapper](auto data, auto writer) {
+    size_t bufLen = data.size();
+    self.ApplyTo(
+        LEDPattern::LEDReader{
+            [=](auto i) { return data[indexMapper(bufLen, i)]; }, bufLen},
+        [&](int i, Color color) { writer(indexMapper(bufLen, i), color); });
   }};
 }
 
+LEDPattern LEDPattern::Reversed() {
+  return MapIndex([](size_t bufLen, size_t i) { return bufLen - 1 - i; });
+}
+
 LEDPattern LEDPattern::OffsetBy(int offset) {
-  return LEDPattern{[=, self = *this](auto data, auto writer) {
-    self.ApplyTo(data, [&data, &writer, offset](int i, Color color) {
-      int shiftedIndex =
-          frc::FloorMod(i + offset, static_cast<int>(data.size()));
-      writer(shiftedIndex, color);
-    });
-  }};
+  return MapIndex([offset](size_t bufLen, size_t i) {
+    return frc::FloorMod(static_cast<int>(i) + offset,
+                         static_cast<int>(bufLen));
+  });
 }
 
 LEDPattern LEDPattern::ScrollAtRelativeSpeed(units::hertz_t velocity) {
@@ -53,8 +66,7 @@ LEDPattern LEDPattern::ScrollAtRelativeSpeed(units::hertz_t velocity) {
   // Invert and multiply by 1,000,000 to get microseconds
   double periodMicros = 1e6 / velocity.value();
 
-  return LEDPattern{[=, self = *this](auto data, auto writer) {
-    auto bufLen = data.size();
+  return MapIndex([=](size_t bufLen, size_t i) {
     auto now = wpi::Now();
 
     // index should move by (bufLen) / (period)
@@ -62,12 +74,9 @@ LEDPattern LEDPattern::ScrollAtRelativeSpeed(units::hertz_t velocity) {
         (now % static_cast<int64_t>(std::floor(periodMicros))) / periodMicros;
     int offset = static_cast<int>(std::floor(t * bufLen));
 
-    self.ApplyTo(data, [=](int i, Color color) {
-      // floorMod so if the offset is negative, we still get positive outputs
-      int shiftedIndex = frc::FloorMod(i + offset, static_cast<int>(bufLen));
-      writer(shiftedIndex, color);
-    });
-  }};
+    return frc::FloorMod(static_cast<int>(i) + offset,
+                         static_cast<int>(bufLen));
+  });
 }
 
 LEDPattern LEDPattern::ScrollAtAbsoluteSpeed(
@@ -77,8 +86,7 @@ LEDPattern LEDPattern::ScrollAtAbsoluteSpeed(
   auto microsPerLed =
       static_cast<int64_t>(std::floor((ledSpacing / velocity).value() * 1e6));
 
-  return LEDPattern{[=, self = *this](auto data, auto writer) {
-    auto bufLen = data.size();
+  return MapIndex([=](size_t bufLen, size_t i) {
     auto now = wpi::Now();
 
     // every step in time that's a multiple of microsPerLED will increment
@@ -87,13 +95,9 @@ LEDPattern LEDPattern::ScrollAtAbsoluteSpeed(
     // offset values for negative velocities
     auto offset = static_cast<int64_t>(now) / microsPerLed;
 
-    self.ApplyTo(data, [=, &writer](int i, Color color) {
-      // FloorMod so if the offset is negative, we still get positive outputs
-      int shiftedIndex = frc::FloorMod(i + offset, static_cast<int>(bufLen));
-
-      writer(shiftedIndex, color);
-    });
-  }};
+    return frc::FloorMod(static_cast<int>(i) + offset,
+                         static_cast<int>(bufLen));
+  });
 }
 
 LEDPattern LEDPattern::Blink(units::second_t onTime, units::second_t offTime) {