From 1db41a2a0ac850732d0fc38c398f1df7b5d7b11f Mon Sep 17 00:00:00 2001
From: Daniel Burke <danielburke1206@gmail.com>
Date: Sun, 28 Jul 2024 15:53:05 -0400
Subject: [PATCH] Rainbow LEDs

---
 .../lib/drivers/leds/LEDController.java       |  5 ++
 .../team1701/robot/subsystems/leds/LED.java   | 60 +++++++++++++------
 2 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/src/main/java/com/team1701/lib/drivers/leds/LEDController.java b/src/main/java/com/team1701/lib/drivers/leds/LEDController.java
index 4ae4bf31..c39de95a 100644
--- a/src/main/java/com/team1701/lib/drivers/leds/LEDController.java
+++ b/src/main/java/com/team1701/lib/drivers/leds/LEDController.java
@@ -63,6 +63,11 @@ private void setRBGFromGradient(int index, double red, double green, double blue
         mLEDBuffer.setRGB(index, (int) (red * brightness), (int) (blue * brightness), (int) (green * brightness));
     }
 
+    private void setHSV(int index, int hue, int saturation, int value) {
+        var brightness = mBrightness[index];
+        mLEDBuffer.setHSV(index, hue, saturation, value);
+    }
+
     public void update() {
         for (var i = 0; i < mColors.length; i++) {
             var color = mColors[i];
diff --git a/src/main/java/com/team1701/robot/subsystems/leds/LED.java b/src/main/java/com/team1701/robot/subsystems/leds/LED.java
index fb8eb2e2..128badaa 100644
--- a/src/main/java/com/team1701/robot/subsystems/leds/LED.java
+++ b/src/main/java/com/team1701/robot/subsystems/leds/LED.java
@@ -1,7 +1,6 @@
 package com.team1701.robot.subsystems.leds;
 
 import com.team1701.lib.drivers.leds.LEDController;
-import com.team1701.robot.Configuration;
 import com.team1701.robot.states.RobotState;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -60,28 +59,55 @@ private void setStrobe(Color color) {
         mLEDController.setAll(tick % 2 == 0 ? color : Color.kWhite);
     }
 
-    private void setDisabledLEDStates() {
-        var color = Configuration.isBlueAlliance() ? LEDColors.kDisabledBlue : LEDColors.kDisabledRed;
-        var cylonColumn = (int) ((Timer.getFPGATimestamp() * kCylonFrequency) % ((kTopLEDsPerRow - 1) * 2));
-        if (cylonColumn >= kTopLEDsPerRow) {
-            cylonColumn = (kTopLEDsPerRow - 1) * 2 - cylonColumn;
-        }
+    private void setRainbow() {
+        var tick = (int) (Timer.getFPGATimestamp() * 60);
+        var hue = tick % 180;
+        // for (var row = 0; row < kTopLEDsRowCount; row++) {
+        //     var start = row * kTopLEDsPerRow;
+        //     mLEDController.setRange(start, start + 3, Color.fromHSV(hue, 255, 128));
+        // }
 
-        mLEDController.setAll(Color.kBlack);
         for (var row = 0; row < kTopLEDsRowCount; row++) {
-            var rowStart = row * kTopLEDsPerRow;
-            var rowEnd = rowStart + kTopLEDsPerRow - 1;
-            var column = row % 2 == 0 ? rowStart + cylonColumn : rowEnd - cylonColumn;
-            mLEDController.set(column, color);
-            if (column > rowStart) {
-                mLEDController.set(column - 1, color, 0.25);
-            }
-            if (column < rowEnd) {
-                mLEDController.set(column + 1, color, 0.25);
+            if (row % 2 == 0) {
+                for (var i = 0; i < kTopLEDsPerRow; i++) {
+                    mLEDController.set(i + row * kTopLEDsPerRow, Color.fromHSV((hue + i * 10) % 180, 255, 128));
+                }
+            } else {
+                for (var i = 0; i < kTopLEDsPerRow; i++) {
+                    mLEDController.set(
+                            kTopLEDsPerRow - i - 1 + row * kTopLEDsPerRow,
+                            Color.fromHSV((hue + i * 10) % 180, 255, 128));
+                }
             }
         }
     }
 
+    // private void setDisabledLEDStates() {
+    //     var color = Configuration.isBlueAlliance() ? LEDColors.kDisabledBlue : LEDColors.kDisabledRed;
+    //     var cylonColumn = (int) ((Timer.getFPGATimestamp() * kCylonFrequency) % ((kTopLEDsPerRow - 1) * 2));
+    //     if (cylonColumn >= kTopLEDsPerRow) {
+    //         cylonColumn = (kTopLEDsPerRow - 1) * 2 - cylonColumn;
+    //     }
+
+    //     mLEDController.setAll(Color.kBlack);
+    //     for (var row = 0; row < kTopLEDsRowCount; row++) {
+    //         var rowStart = row * kTopLEDsPerRow;
+    //         var rowEnd = rowStart + kTopLEDsPerRow - 1;
+    //         var column = row % 2 == 0 ? rowStart + cylonColumn : rowEnd - cylonColumn;
+    //         mLEDController.set(column, color);
+    //         if (column > rowStart) {
+    //             mLEDController.set(column - 1, color, 0.25);
+    //         }
+    //         if (column < rowEnd) {
+    //             mLEDController.set(column + 1, color, 0.25);
+    //         }
+    //     }
+    // }
+
+    private void setDisabledLEDStates() {
+        setRainbow();
+    }
+
     private void setScoringLEDStates() {
         var state = mRobotState.getShootingState();
         if (state.isShooting) {