[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>

wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java

@@ -93,6 +93,19 @@ import java.util.function.DoubleSupplier;
  */
 @FunctionalInterface
 public interface LEDPattern {
+  /** A functional interface for index mapping functions. */
+  @FunctionalInterface
+  interface IndexMapper {
+    /**
+     * Maps the index.
+     *
+     * @param bufLen Length of the buffer
+     * @param index The index to map
+     * @return The mapped index
+     */
+    int apply(int bufLen, int index);
+  }
+
   /**
    * Writes the pattern to an LED buffer. Dynamic animations should be called periodically (such as
    * with a command or with a periodic method) to refresh the buffer over time.
@@ -129,6 +142,41 @@ public interface LEDPattern {
     applyTo(readWriter, readWriter);
   }
 
+  /**
+   * Creates a pattern with remapped indices.
+   *
+   * @param indexMapper the index mapper
+   * @return the mapped pattern
+   */
+  default LEDPattern mapIndex(IndexMapper indexMapper) {
+    return (reader, writer) -> {
+      int bufLen = reader.getLength();
+      applyTo(
+          new LEDReader() {
+            @Override
+            public int getLength() {
+              return reader.getLength();
+            }
+
+            @Override
+            public int getRed(int index) {
+              return reader.getRed(indexMapper.apply(bufLen, index));
+            }
+
+            @Override
+            public int getGreen(int index) {
+              return reader.getGreen(indexMapper.apply(bufLen, index));
+            }
+
+            @Override
+            public int getBlue(int index) {
+              return reader.getBlue(indexMapper.apply(bufLen, index));
+            }
+          },
+          (i, r, g, b) -> writer.setRGB(indexMapper.apply(bufLen, i), r, g, b));
+    };
+  }
+
   /**
    * Creates a pattern that displays this one in reverse. Scrolling patterns will scroll in the
    * opposite direction (but at the same speed). It will treat the end of an LED strip as the start,
@@ -143,10 +191,7 @@ public interface LEDPattern {
    * @see AddressableLEDBufferView#reversed()
    */
   default LEDPattern reversed() {
-    return (reader, writer) -> {
-      int bufLen = reader.getLength();
-      applyTo(reader, (i, r, g, b) -> writer.setRGB((bufLen - 1) - i, r, g, b));
-    };
+    return mapIndex((length, index) -> length - 1 - index);
   }
 
   /**
@@ -157,15 +202,7 @@ public interface LEDPattern {
    * @return the offset pattern
    */
   default LEDPattern offsetBy(int offset) {
-    return (reader, writer) -> {
-      int bufLen = reader.getLength();
-      applyTo(
-          reader,
-          (i, r, g, b) -> {
-            int shiftedIndex = Math.floorMod(i + offset, bufLen);
-            writer.setRGB(shiftedIndex, r, g, b);
-          });
-    };
+    return mapIndex((length, index) -> Math.floorMod(index + offset, length));
   }
 
   /**
@@ -189,23 +226,16 @@ public interface LEDPattern {
   default LEDPattern scrollAtRelativeSpeed(Frequency velocity) {
     final double periodMicros = velocity.asPeriod().in(Microseconds);
 
-    return (reader, writer) -> {
-      int bufLen = reader.getLength();
-      long now = RobotController.getTime();
+    return mapIndex(
+        (bufLen, index) -> {
+          long now = RobotController.getTime();
 
-      // index should move by (buf.length) / (period)
-      double t = (now % (long) periodMicros) / periodMicros;
-      int offset = (int) (t * bufLen);
+          // index should move by (buf.length) / (period)
+          double t = (now % (long) periodMicros) / periodMicros;
+          int offset = (int) (t * bufLen);
 
-      applyTo(
-          reader,
-          (i, r, g, b) -> {
-            // floorMod so if the offset is negative, we still get positive outputs
-            int shiftedIndex = Math.floorMod(i + offset, bufLen);
-
-            writer.setRGB(shiftedIndex, r, g, b);
-          });
-    };
+          return Math.floorMod(index + offset, bufLen);
+        });
   }
 
   /**
@@ -240,22 +270,16 @@ public interface LEDPattern {
     var metersPerMicro = velocity.in(Meters.per(Microsecond));
     var microsPerLED = (int) (ledSpacing.in(Meters) / metersPerMicro);
 
-    return (reader, writer) -> {
-      int bufLen = reader.getLength();
-      long now = RobotController.getTime();
+    return mapIndex(
+        (bufLen, index) -> {
+          long now = RobotController.getTime();
 
-      // every step in time that's a multiple of microsPerLED will increment the offset by 1
-      var offset = now / microsPerLED;
+          // every step in time that's a multiple of microsPerLED will increment the offset by 1
+          var offset = (int) (now / microsPerLED);
 
-      applyTo(
-          reader,
-          (i, r, g, b) -> {
-            // floorMod so if the offset is negative, we still get positive outputs
-            int shiftedIndex = Math.floorMod(i + offset, bufLen);
-
-            writer.setRGB(shiftedIndex, r, g, b);
-          });
-    };
+          // floorMod so if the offset is negative, we still get positive outputs
+          return Math.floorMod(index + offset, bufLen);
+        });
   }
 
   /**