From 1d557ac092b92ea3eef0753463c7bf2c3a04c00d Mon Sep 17 00:00:00 2001
From: QwertyChouskie <asdfghrbljzmkd@outlook.com>
Date: Sun, 18 Feb 2024 04:49:30 -0800
Subject: [PATCH] Loop times: Go brrrrrrrrr (~30-40ms savings)

Only update one color read per sensor per loop cycle.  Certain aspects of collector sensing now theoretically have slightly longer peak possible latency, but the overall loop time savings are well worth it.

Tested to function via telemetry, needs real drive testing as well.
---
 .../robotTwo/subsystems/Transfer.kt           | 48 +++++++++++++++----
 1 file changed, 39 insertions(+), 9 deletions(-)

diff --git a/RoboCode/src/main/java/us/brainstormz/robotTwo/subsystems/Transfer.kt b/RoboCode/src/main/java/us/brainstormz/robotTwo/subsystems/Transfer.kt
index ee25d3f6..989762d1 100644
--- a/RoboCode/src/main/java/us/brainstormz/robotTwo/subsystems/Transfer.kt
+++ b/RoboCode/src/main/java/us/brainstormz/robotTwo/subsystems/Transfer.kt
@@ -74,11 +74,11 @@ class Transfer(private val telemetry: Telemetry) {
         return ActualTransfer(
                 left = ActualTransferHalf(
                         upperSensor = TeleopTest.emptySensorReading,
-                        lowerSensor = getSensorReading(hardware.leftTransferLowerSensor),
+                        lowerSensor = getSensorReading(hardware.leftTransferLowerSensor, 0, 0),
                 ),
                 right = ActualTransferHalf(
                         upperSensor = TeleopTest.emptySensorReading,
-                        lowerSensor = getSensorReading(hardware.rightTransferLowerSensor),
+                        lowerSensor = getSensorReading(hardware.rightTransferLowerSensor, 0, 0),
                 )
         )
     }
@@ -293,17 +293,47 @@ class Transfer(private val telemetry: Telemetry) {
         Blue
     }
 
-    data class SensorReading(val red: Int, val green: Int, val blue: Int, val alpha: Int): CleanToStringPrint() {
+    data class SensorReading(var red: Int, var green: Int, var blue: Int, var alpha: Int): CleanToStringPrint() {
         val asList = listOf(red, green, blue, alpha)
         val rgbAsMap = mapOf(RGB.Red to red, RGB.Green to green, RGB.Blue to blue)
     }
-    fun getSensorReading(sensor: ColorSensor): SensorReading {
-        return SensorReading(
-                red= sensor.red(),
-                green= sensor.green(),
-                blue= sensor.blue(),
+
+    // I'm sorry James, I made the code more ugly.  On the plus side, about 40ms knocked off every loop cycle!
+    // EDIT: Cleaner and not bugged version, thanks to AI
+    val sensorReadings = mutableMapOf<ColorSensor, SensorReading>()
+    val sensorReadCounter = mutableMapOf<ColorSensor, Int>()
+
+    fun getSensorReading(sensor: ColorSensor, updateSlot: Int, totalUpdateSlots: Int): SensorReading {
+        var callcounter = sensorReadCounter[sensor]
+        if (callcounter == null)
+            callcounter = 0
+        else
+            callcounter += 1
+        // Store the updated callcounter back into the map
+        sensorReadCounter[sensor] = callcounter
+
+        var reading = sensorReadings[sensor]
+        if (reading == null) {
+            reading = SensorReading(
+                red = sensor.red(),
+                green = sensor.green(),
+                blue = sensor.blue(),
                 alpha = sensor.alpha()
-        )
+            )
+            sensorReadings[sensor] = reading
+        }
+//        else if (true || callcounter % totalUpdateSlots == updateSlot) { // FIXME: needs rethink
+            if (callcounter % 4 == 1) {
+                reading.red = sensor.red()
+            } else if (callcounter % 4 == 2) {
+                reading.green = sensor.green()
+            } else if (callcounter % 4 == 3) {
+                reading.blue = sensor.blue()
+            } else if (callcounter % 4 == 0) {
+                reading.alpha = sensor.alpha()
+            }
+//        }
+        return reading
     }
 //
 //    data class RGBValue(val red: Double, val green: Double, val blue: Double) {