[wpilibj] AddressableLEDBuffer: Add methods for reading individual RG…

…B values (#6333)

This avoids the allocation/GC overhead of returning a Color8 value.

Also add an indexed iterator forEach to loop over the entire buffer.

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

@@ -131,8 +131,7 @@ public int getLength() {
    * @return the LED color at the specified index
    */
   public Color8Bit getLED8Bit(int index) {
-    return new Color8Bit(
-        m_buffer[index * 4 + 2] & 0xFF, m_buffer[index * 4 + 1] & 0xFF, m_buffer[index * 4] & 0xFF);
+    return new Color8Bit(getRed(index), getGreen(index), getBlue(index));
   }
 
   /**
@@ -142,9 +141,70 @@ public Color8Bit getLED8Bit(int index) {
    * @return the LED color at the specified index
    */
   public Color getLED(int index) {
-    return new Color(
-        (m_buffer[index * 4 + 2] & 0xFF) / 255.0,
-        (m_buffer[index * 4 + 1] & 0xFF) / 255.0,
-        (m_buffer[index * 4] & 0xFF) / 255.0);
+    return new Color(getRed(index) / 255.0, getGreen(index) / 255.0, getBlue(index) / 255.0);
+  }
+
+  /**
+   * Gets the red channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the red channel, from [0, 255]
+   */
+  public int getRed(int index) {
+    return m_buffer[index * 4 + 2] & 0xFF;
+  }
+
+  /**
+   * Gets the green channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the green channel, from [0, 255]
+   */
+  public int getGreen(int index) {
+    return m_buffer[index * 4 + 1] & 0xFF;
+  }
+
+  /**
+   * Gets the blue channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the blue channel, from [0, 255]
+   */
+  public int getBlue(int index) {
+    return m_buffer[index * 4] & 0xFF;
+  }
+
+  /**
+   * A functional interface that allows for iteration over an LED buffer without manually writing an
+   * indexed for-loop.
+   */
+  @FunctionalInterface
+  public interface IndexedColorIterator {
+    /**
+     * Accepts an index of an LED in the buffer and the red, green, and blue components of the
+     * currently stored color for that LED.
+     *
+     * @param index the index of the LED in the buffer that the red, green, and blue channels
+     *     corresponds to
+     * @param r the value of the red channel of the color currently in the buffer at index {@code i}
+     * @param g the value of the green channel of the color currently in the buffer at index {@code
+     *     i}
+     * @param b the value of the blue channel of the color currently in the buffer at index {@code
+     *     i}
+     */
+    void accept(int index, int r, int g, int b);
+  }
+
+  /**
+   * Iterates over the LEDs in the buffer, starting from index 0. The iterator function is passed
+   * the current index of iteration, along with the values for the red, green, and blue components
+   * of the color written to the LED at that index.
+   *
+   * @param iterator the iterator function to call for each LED in the buffer.
+   */
+  public void forEach(IndexedColorIterator iterator) {
+    for (int i = 0; i < getLength(); i++) {
+      iterator.accept(i, getRed(i), getGreen(i), getBlue(i));
+    }
   }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java

@@ -6,6 +6,7 @@
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.fail;
 import static org.junit.jupiter.params.provider.Arguments.arguments;
 
 import edu.wpi.first.wpilibj.util.Color;
@@ -70,4 +71,73 @@ void getColorTest() {
     assertEquals(firstRedColor8Bit, buffer.getLED8Bit(2));
     assertEquals(firstBlueColor8Bit, buffer.getLED8Bit(3));
   }
+
+  @Test
+  void getRed() {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setRGB(0, 127, 128, 129);
+    assertEquals(127, buffer.getRed(0));
+  }
+
+  @Test
+  void getGreen() {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setRGB(0, 127, 128, 129);
+    assertEquals(128, buffer.getGreen(0));
+  }
+
+  @Test
+  void getBlue() {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setRGB(0, 127, 128, 129);
+    assertEquals(129, buffer.getBlue(0));
+  }
+
+  @Test
+  void forEach() {
+    var buffer = new AddressableLEDBuffer(3);
+    buffer.setRGB(0, 1, 2, 3);
+    buffer.setRGB(1, 4, 5, 6);
+    buffer.setRGB(2, 7, 8, 9);
+
+    buffer.forEach(
+        (index, r, g, b) -> {
+          switch (index) {
+            case 0:
+              {
+                assertAll(
+                    () -> assertEquals(1, r, "red at index 0"),
+                    () -> assertEquals(2, g, "green at index 0"),
+                    () -> assertEquals(3, b, "blue at index 0"));
+              }
+              break;
+            case 1:
+              {
+                assertAll(
+                    () -> assertEquals(4, r, "red at index 1"),
+                    () -> assertEquals(5, g, "green at index 1"),
+                    () -> assertEquals(6, b, "blue at index 1"));
+              }
+              break;
+            case 2:
+              {
+                assertAll(
+                    () -> assertEquals(7, r, "red at index 2"),
+                    () -> assertEquals(8, g, "green at index 2"),
+                    () -> assertEquals(9, b, "blue at index 2"));
+              }
+              break;
+            default:
+              fail("Unexpected index " + index);
+              break;
+          }
+        });
+  }
+
+  @Test
+  void forEachOnEmptyBuffer() {
+    var buffer = new AddressableLEDBuffer(0);
+
+    buffer.forEach((i, r, g, b) -> fail("Iterator should not be called on an empty buffer"));
+  }
 }