From d8c32a6d4044900df2562c9e3d74ac6a9d1ce192 Mon Sep 17 00:00:00 2001
From: Corey Applegate <cpapplefamily@gmail.com>
Date: Sat, 21 Dec 2024 22:29:33 -0600
Subject: [PATCH] Add field stack light API and update PLC handling logic

---
 field/arena.go     | 16 ++++++++--------
 plc/plc.go         |  6 ++++++
 web/alternateIO.go | 31 +++++++++++++++++++++++++++++++
 web/web.go         |  1 +
 4 files changed, 46 insertions(+), 8 deletions(-)

diff --git a/field/arena.go b/field/arena.go
index 42d8c5ac..dd6be200 100644
--- a/field/arena.go
+++ b/field/arena.go
@@ -966,9 +966,9 @@ func (arena *Arena) handlePlcInputOutput() {
 		case PostTimeout:
 			// Set the stack light state -- solid alliance color(s) if robots are not connected, solid orange if scores are
 			// not input, or blinking green if ready.
-			//*greenStackLight := redAllianceReady && blueAllianceReady //&& arena.Plc.GetCycleState(2, 0, 2)
-			//*arena.Plc.SetStackLights(!redAllianceReady, !blueAllianceReady, false, greenStackLight)
-			//*arena.Plc.SetStackBuzzer(redAllianceReady && blueAllianceReady)
+			greenStackLight := redAllianceReady && blueAllianceReady //&& arena.Plc.GetCycleState(2, 0, 2)
+			arena.Plc.SetStackLights(!redAllianceReady, !blueAllianceReady, false, greenStackLight)
+			arena.Plc.SetStackBuzzer(redAllianceReady && blueAllianceReady)
 
 			// Turn off lights if all teams become ready.
 			if redAllianceReady && blueAllianceReady {
@@ -982,12 +982,12 @@ func (arena *Arena) handlePlcInputOutput() {
 			if arena.FieldReset {
 				//*arena.Plc.SetFieldResetLight(true)
 			}
-			//*scoreReady := arena.RedRealtimeScore.FoulsCommitted && arena.BlueRealtimeScore.FoulsCommitted &&
-			//*	arena.alliancePostMatchScoreReady("red") && arena.alliancePostMatchScoreReady("blue")
-			//*arena.Plc.SetStackLights(false, false, !scoreReady, false)
+			scoreReady := arena.RedRealtimeScore.FoulsCommitted && arena.BlueRealtimeScore.FoulsCommitted &&
+				arena.alliancePostMatchScoreReady("red") && arena.alliancePostMatchScoreReady("blue")
+			arena.Plc.SetStackLights(false, false, !scoreReady, false)
 		case AutoPeriod, PausePeriod, TeleopPeriod:
-			//*arena.Plc.SetStackBuzzer(false)
-			//*arena.Plc.SetStackLights(!redAllianceReady, !blueAllianceReady, false, true)
+			arena.Plc.SetStackBuzzer(false)
+			arena.Plc.SetStackLights(!redAllianceReady, !blueAllianceReady, false, true)
 		}
 
 		// Get all the game-specific inputs and update the score.
diff --git a/plc/plc.go b/plc/plc.go
index ba877def..a1d61c8f 100644
--- a/plc/plc.go
+++ b/plc/plc.go
@@ -43,6 +43,7 @@ type Plc interface {
 	//Freezy Arena
 	SetAlternateIOStopState(input int, state bool)
 	ResetEstops()
+	GetFieldStackLight() (bool, bool, bool, bool)
 }
 
 type ModbusPlc struct {
@@ -529,4 +530,9 @@ func (plc *ModbusPlc) ResetEstops(){
 // used for Alternate IO stops
 func (plc *ModbusPlc) SetAlternateIOStopState(input int, state bool){
 	plc.inputs[input] = state
+}
+
+
+func (plc *ModbusPlc) GetFieldStackLight() (bool, bool, bool, bool) {
+	return plc.coils[stackLightRed], plc.coils[stackLightBlue], plc.coils[stackLightOrange], plc.coils[stackLightGreen]
 }
\ No newline at end of file
diff --git a/web/alternateIO.go b/web/alternateIO.go
index d9a8deaf..c12b6759 100644
--- a/web/alternateIO.go
+++ b/web/alternateIO.go
@@ -44,3 +44,34 @@ func (web *Web) eStopStatePostHandler(w http.ResponseWriter, r *http.Request) {
 	w.Write([]byte("eStop state updated successfully."))
 
 }
+
+type fieldStackLight struct {
+	Red bool `json:"redStackLight"`
+	Blue bool `json:"blueStackLight"`
+	Orange bool `json:"orangeStackLight"`
+	Green bool `json:"greenStackLight"`
+
+}
+
+func (web *Web) fieldStackLightGetHandler(w http.ResponseWriter, r *http.Request) {
+	// Ensure the request is a GET request.
+	if r.Method != http.MethodGet {
+		http.Error(w, "Invalid request method", http.StatusMethodNotAllowed)
+		return
+	}
+
+	// Get the current state of the field stack light.
+	var stackLight fieldStackLight
+	stackLight.Red, stackLight.Blue, stackLight.Orange, stackLight.Green = web.arena.Plc.GetFieldStackLight()
+	
+	// Marshal the response payload.
+	response, err := json.Marshal(stackLight)
+	if err != nil {
+		http.Error(w, "Failed to marshal eStop state", http.StatusInternalServerError)
+		return
+	}
+
+	// Send the response.
+	w.Write(response)
+}
+
diff --git a/web/web.go b/web/web.go
index fd497b85..0c81185e 100644
--- a/web/web.go
+++ b/web/web.go
@@ -232,6 +232,7 @@ func (web *Web) newHandler() http.Handler {
 	mux.HandleFunc("GET /api/allianceStatus", web.allianceStatusApiHandler)
 	mux.HandleFunc("GET /field_monitor_help", web.fieldMonitorDisplayHelpHandler)
 	mux.HandleFunc("POST /freezy/eStopState", web.eStopStatePostHandler)
+	mux.HandleFunc("GET /field_stack_light", web.fieldStackLightGetHandler)
 	return mux
 }