From 476dbb87b7331dff01a9f3678a32d98e6a9878b3 Mon Sep 17 00:00:00 2001
From: penguin212 <dhuber06@gmail.com>
Date: Sat, 17 Feb 2024 12:34:51 -0800
Subject: [PATCH] started work on note layer

---
 .../robot/subsystems/leds/LEDSubsystem.java   |  6 +++--
 .../robot/subsystems/leds/RotaryLEDLayer.java | 23 +++++++++++++++++++
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
index 72594734..e2fc9155 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -16,7 +16,8 @@ public class LEDSubsystem extends SubsystemBase {
     private final RotaryLEDLayer driveAngleLayer;
     private final RotaryLEDLayer driveBackAngleLayer;
     private final RotaryLEDLayer rainbowLayer;
-    private final LEDLayer aprilDetectedLayer;
+    private final RotaryLEDLayer noteLayer;
+    private final RotaryLEDLayer aprilDetectedLayer;
 
     private final Timer blinkTimer;
     private static final double BLINK_DURATION_SECONDS = 0.5;
@@ -64,7 +65,8 @@ public LEDSubsystem() {
         driveAngleLayer = new RotaryLEDLayer(LED_LENGTH);
         driveBackAngleLayer = new RotaryLEDLayer(LED_LENGTH);
         rainbowLayer = new RotaryLEDLayer(LED_LENGTH);
-        aprilDetectedLayer = new LEDLayer(LED_LENGTH);
+        noteLayer = new RotaryLEDLayer(LED_LENGTH);
+        aprilDetectedLayer = new RotaryLEDLayer(LED_LENGTH);
 
         blinkTimer = new Timer();
         ledTimer = new Timer();
diff --git a/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java b/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java
index b89eae38..ecdff9ba 100644
--- a/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java
+++ b/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java
@@ -69,4 +69,27 @@ public void setRainbow(int offset){
             ));
         }
     }
+
+    public void setGroups(double startAngle, double endAngle, int onGroupLength, int offGroupLength, int borderLength, Color color, double opacity, int offset, boolean inverted){
+        for (int i = (int) (colorArray.length * startAngle / (2 * Math.PI)); i < colorArray.length * endAngle / (2 * Math.PI); i++) {
+            int ledNumInSegment = (i + (inverted ? offset : -offset)) % (2 * borderLength + onGroupLength + offGroupLength);
+            if (ledNumInSegment < borderLength){
+                setLED(i, color, opacity *  (ledNumInSegment + 1) / (borderLength + 1));
+            } else if (ledNumInSegment < onGroupLength + borderLength) {
+                setLED(i, color, opacity);
+            } else if(ledNumInSegment < onGroupLength + borderLength * 2){
+                setLED(i, color, opacity * (1 - ((ledNumInSegment - onGroupLength - borderLength + 1.) / (borderLength + 1))));                
+            } else {
+                setLED(i, null, opacity);
+            }
+        }
+    }
+
+    public void setGroups(int onGroupLength, int offGroupLength, int borderLength, Color color, double opacity, int offset){
+        setGroups(0, Math.PI * 2, onGroupLength, offGroupLength, borderLength, color, opacity, offset, false);
+    }
+
+    public void setGroups(int onGroupLength, int offGroupLength, Color color, double opacity, int offset){
+        setGroups(0, Math.PI * 2, onGroupLength, offGroupLength, 0, color, opacity, offset, false);
+    }
 }
\ No newline at end of file