[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/include/frc/LEDPattern.h

@@ -18,21 +18,36 @@
 
 namespace frc {
 
-/**
- * Sets the LED at the given index to the given color.
- */
-using LEDWriterFn = std::function<void(int, frc::Color)>;
-
-/**
- * Accepts a data buffer (1st argument) and a callback (2nd argument) for
- * writing data.
- */
-using LEDPatternFn =
-    std::function<void(std::span<frc::AddressableLED::LEDData>, LEDWriterFn)>;
-
 class LEDPattern {
  public:
-  explicit LEDPattern(LEDPatternFn impl);
+  /**
+   * A wrapper around a length and an arbitrary reader function that accepts an
+   * LED index and returns data for the LED at that index. This configuration
+   * allows us to abstract over different container types without templating.
+   */
+  class LEDReader {
+   public:
+    LEDReader(std::function<frc::AddressableLED::LEDData(int)> impl,
+              size_t size)
+        : m_impl{std::move(impl)}, m_size{size} {}
+
+    frc::AddressableLED::LEDData operator[](size_t index) const {
+      return m_impl(index);
+    }
+
+    size_t size() const { return m_size; }
+
+   private:
+    std::function<frc::AddressableLED::LEDData(int)> m_impl;
+    size_t m_size;
+  };
+
+  explicit LEDPattern(std::function<void(frc::LEDPattern::LEDReader,
+                                         std::function<void(int, frc::Color)>)>
+                          impl);
+
+  void ApplyTo(LEDReader reader,
+               std::function<void(int, frc::Color)> writer) const;
 
   /**
    * Writes the pattern to an LED buffer. Dynamic animations should be called
@@ -48,7 +63,7 @@ class LEDPattern {
    * @param writer data writer for setting new LED colors on the LED strip
    */
   void ApplyTo(std::span<frc::AddressableLED::LEDData> data,
-               LEDWriterFn writer) const;
+               std::function<void(int, frc::Color)> writer) const;
 
   /**
    * Writes the pattern to an LED buffer. Dynamic animations should be called
@@ -64,6 +79,15 @@ class LEDPattern {
    */
   void ApplyTo(std::span<frc::AddressableLED::LEDData> data) const;
 
+  /**
+   * Creates a pattern with remapped indices.
+   *
+   * @param indexMapper the index mapper
+   * @return the mapped pattern
+   */
+  [[nodiscard]]
+  LEDPattern MapIndex(std::function<size_t(size_t, size_t)> indexMapper);
+
   /**
    * Creates a pattern that displays this one in reverse. Scrolling patterns
    * will scroll in the opposite direction (but at the same speed). It will
@@ -373,6 +397,8 @@ class LEDPattern {
   static LEDPattern Rainbow(int saturation, int value);
 
  private:
-  LEDPatternFn m_impl;
+  std::function<void(frc::LEDPattern::LEDReader,
+                     std::function<void(int, frc::Color)>)>
+      m_impl;
 };
 }  // namespace frc