From ea405f71b1e0bcc5c35bcdea86ce8026ebbea9b3 Mon Sep 17 00:00:00 2001
From: Patrick Fairbank <pat@patfairbank.com>
Date: Sat, 19 May 2018 22:25:31 -0700
Subject: [PATCH] Implement A-stop.

---
 field/arena.go      | 20 ++++++++++----
 field/arena_test.go | 65 +++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 80 insertions(+), 5 deletions(-)

diff --git a/field/arena.go b/field/arena.go
index c8d6961..a8d2948 100644
--- a/field/arena.go
+++ b/field/arena.go
@@ -94,6 +94,7 @@ type ArenaStatus struct {
 
 type AllianceStation struct {
 	DsConn *DriverStationConnection
+	Astop  bool
 	Estop  bool
 	Bypass bool
 	Team   *model.Team
@@ -635,7 +636,7 @@ func (arena *Arena) sendDsPacket(auto bool, enabled bool) {
 		dsConn := allianceStation.DsConn
 		if dsConn != nil {
 			dsConn.Auto = auto
-			dsConn.Enabled = enabled && !allianceStation.Estop && !allianceStation.Bypass
+			dsConn.Enabled = enabled && !allianceStation.Estop && !allianceStation.Astop && !allianceStation.Bypass
 			dsConn.Estop = allianceStation.Estop
 			err := dsConn.update(arena)
 			if err != nil {
@@ -741,9 +742,18 @@ func (arena *Arena) handlePlcOutput() {
 func (arena *Arena) handleEstop(station string, state bool) {
 	allianceStation := arena.AllianceStations[station]
 	if state {
-		allianceStation.Estop = true
-	} else if arena.MatchTimeSec() == 0 {
-		// Don't reset the e-stop while a match is in progress.
-		allianceStation.Estop = false
+		if arena.MatchState == AutoPeriod {
+			allianceStation.Astop = true
+		} else {
+			allianceStation.Estop = true
+		}
+	} else {
+		if arena.MatchState != AutoPeriod {
+			allianceStation.Astop = false
+		}
+		if arena.MatchTimeSec() == 0 {
+			// Don't reset the e-stop while a match is in progress.
+			allianceStation.Estop = false
+		}
 	}
 }
diff --git a/field/arena_test.go b/field/arena_test.go
index 9125a2b..a6fcb33 100644
--- a/field/arena_test.go
+++ b/field/arena_test.go
@@ -482,3 +482,68 @@ func TestSetupNetwork(t *testing.T) {
 	assert.Contains(t, writer.String(), "Failed to configure team Ethernet")
 	assert.Contains(t, writer.String(), "Failed to configure team WiFi")
 }
+
+func TestAstop(t *testing.T) {
+	arena := setupTestArena(t)
+
+	arena.Database.CreateTeam(&model.Team{Id: 254})
+	err := arena.assignTeam(254, "R1")
+	assert.Nil(t, err)
+	dummyDs := &DriverStationConnection{TeamId: 254}
+	arena.AllianceStations["R1"].DsConn = dummyDs
+	arena.Database.CreateTeam(&model.Team{Id: 148})
+	err = arena.assignTeam(148, "R2")
+	assert.Nil(t, err)
+	dummyDs = &DriverStationConnection{TeamId: 148}
+	arena.AllianceStations["R2"].DsConn = dummyDs
+
+	arena.AllianceStations["R1"].DsConn.RobotLinked = true
+	arena.AllianceStations["R2"].DsConn.RobotLinked = true
+	arena.AllianceStations["R3"].Bypass = true
+	arena.AllianceStations["B1"].Bypass = true
+	arena.AllianceStations["B2"].Bypass = true
+	arena.AllianceStations["B3"].Bypass = true
+	err = arena.StartMatch()
+	assert.Nil(t, err)
+	arena.Update()
+	arena.MatchStartTime = time.Now().Add(-time.Duration(game.MatchTiming.WarmupDurationSec) * time.Second)
+	arena.Update()
+	assert.Equal(t, AutoPeriod, arena.MatchState)
+	assert.Equal(t, true, arena.AllianceStations["R1"].DsConn.Enabled)
+
+	arena.handleEstop("R1", true)
+	arena.lastDsPacketTime = time.Unix(0, 0) // Force a DS packet.
+	arena.Update()
+	assert.Equal(t, false, arena.AllianceStations["R1"].DsConn.Enabled)
+	assert.Equal(t, true, arena.AllianceStations["R2"].DsConn.Enabled)
+
+	arena.handleEstop("R2", true)
+	arena.lastDsPacketTime = time.Unix(0, 0) // Force a DS packet.
+	arena.Update()
+	assert.Equal(t, false, arena.AllianceStations["R1"].DsConn.Enabled)
+	assert.Equal(t, false, arena.AllianceStations["R2"].DsConn.Enabled)
+
+	arena.handleEstop("R1", false)
+	arena.lastDsPacketTime = time.Unix(0, 0) // Force a DS packet.
+	arena.Update()
+	assert.Equal(t, false, arena.AllianceStations["R1"].DsConn.Enabled)
+	assert.Equal(t, false, arena.AllianceStations["R2"].DsConn.Enabled)
+
+	arena.MatchStartTime = time.Now().Add(-time.Duration(game.MatchTiming.WarmupDurationSec+
+		game.MatchTiming.AutoDurationSec) * time.Second)
+	arena.Update()
+	assert.Equal(t, PausePeriod, arena.MatchState)
+	arena.MatchStartTime = time.Now().Add(-time.Duration(game.MatchTiming.WarmupDurationSec+
+		game.MatchTiming.AutoDurationSec+game.MatchTiming.PauseDurationSec) * time.Second)
+	arena.handleEstop("R2", true)
+	arena.Update()
+	assert.Equal(t, TeleopPeriod, arena.MatchState)
+	assert.Equal(t, true, arena.AllianceStations["R1"].DsConn.Enabled)
+	assert.Equal(t, false, arena.AllianceStations["R2"].DsConn.Enabled)
+
+	arena.handleEstop("R2", false)
+	arena.lastDsPacketTime = time.Unix(0, 0) // Force a DS packet.
+	arena.Update()
+	assert.Equal(t, true, arena.AllianceStations["R1"].DsConn.Enabled)
+	assert.Equal(t, false, arena.AllianceStations["R2"].DsConn.Enabled)
+}