From b5f702b6394cfde6c530ab8b140a32923f70fba2 Mon Sep 17 00:00:00 2001
From: John Hinke <johnh@polymathrobotics.com>
Date: Wed, 29 Apr 2026 04:37:26 -0700
Subject: [PATCH] Adding more unit tests

---
 README.md                                        |  2 ++
 pstop_c/pstop/include/pstop/machine.h            |  2 +-
 pstop_c/pstop/src/pstop/machine.c                | 16 ++++------------
 pstop_c/pstop/src/pstop/protocol.c               |  2 +-
 pstop_c/pstop/src/pstop/pstop_client.c           |  7 -------
 .../pstop/test/src/pstop/machine_timeout_test.c  |  2 ++
 6 files changed, 10 insertions(+), 21 deletions(-)

diff --git a/README.md b/README.md
index bee4fb3..a2dc7b8 100644
--- a/README.md
+++ b/README.md
@@ -69,4 +69,6 @@ mkdir build
 cd build
 cmake ..
 make
+
+./pstop/pstop_test
 ```
diff --git a/pstop_c/pstop/include/pstop/machine.h b/pstop_c/pstop/include/pstop/machine.h
index 6d8dad8..7ae46ff 100644
--- a/pstop_c/pstop/include/pstop/machine.h
+++ b/pstop_c/pstop/include/pstop/machine.h
@@ -77,6 +77,6 @@ typedef struct pstop_machine_t {
 
 void machine_init(pstop_machine_t *machine, pstop_application_t *app, pstop_client_data_t *clients, uint16_t max_clients);
 
-void machine_stop_robot(pstop_machine_t *machine, pstop_client_data_t *client);
+void machine_stop_robot(pstop_machine_t *machine);
 
 #endif /* PSTOP_MACHINE_H */
diff --git a/pstop_c/pstop/src/pstop/machine.c b/pstop_c/pstop/src/pstop/machine.c
index 01e090b..28c2632 100644
--- a/pstop_c/pstop/src/pstop/machine.c
+++ b/pstop_c/pstop/src/pstop/machine.c
@@ -176,7 +176,7 @@ handle_unbond_msg(pstop_machine_t *machine, pstop_client_data_t *client, const p
 
     // if this is the last client then stop the robot
     if((pstop_client_num_active(&(machine->pstops)) == 0U) || (machine->robot_state.robot_state == ROBOT_STATE_STOPPED)) {
-        machine_stop_robot(machine, NULL);
+        machine_stop_robot(machine);
     }
 
     return PSTOP_OK;
@@ -270,8 +270,6 @@ machine_check_heartbeats(pstop_machine_t *machine)
             continue;
         }
 
-        //client->missed_heartbeats_counter++;
-
         // problem! this client hasn't talked to us in a while
         // if we're still within the window of missed heartbeats then we're OK
 
@@ -284,7 +282,7 @@ machine_check_heartbeats(pstop_machine_t *machine)
     }
 
     if(needsStop != 0) {
-        machine_stop_robot(machine, NULL);
+        machine_stop_robot(machine);
         return PSTOP_MISSED_HEARTBEATS;
     }
 
@@ -310,17 +308,11 @@ machine_init(pstop_machine_t *machine, pstop_application_t *app, pstop_client_da
 }
 
 void
-machine_stop_robot(pstop_machine_t *machine, pstop_client_data_t *client)
+machine_stop_robot(pstop_machine_t *machine)
 {
     machine->robot_state.robot_state = ROBOT_STATE_STOPPED;
     machine->robot_state.restart_state = ROBOT_RESTART_STATE_NEED_STOP;
-
-    if(client != NULL) {
-        machine->robot_state.client_stop_id = client->local_client_id;
-    }
-    else {
-        machine->robot_state.client_stop_id = 0U;
-    }
+    machine->robot_state.client_stop_id = 0U;
 
     machine->application->status_cb(PSTOP_STATUS_STOP);
 }
diff --git a/pstop_c/pstop/src/pstop/protocol.c b/pstop_c/pstop/src/pstop/protocol.c
index 10b9456..da0f591 100644
--- a/pstop_c/pstop/src/pstop/protocol.c
+++ b/pstop_c/pstop/src/pstop/protocol.c
@@ -49,7 +49,7 @@ validate_message(pstop_machine_t *machine, pstop_client_data_t *client, const ps
             // too many lost messages
             // clean up this client
             *resp = NULL;
-            machine_stop_robot(machine, NULL);
+            machine_stop_robot(machine);
             return PSTOP_MSG_LOST;
         }
     }
diff --git a/pstop_c/pstop/src/pstop/pstop_client.c b/pstop_c/pstop/src/pstop/pstop_client.c
index b2f928d..db2c2b8 100644
--- a/pstop_c/pstop/src/pstop/pstop_client.c
+++ b/pstop_c/pstop/src/pstop/pstop_client.c
@@ -24,13 +24,6 @@ pstop_client_init(pstop_client_data_t *client)
     client->local_client_id = next_client_id++;
 }
 
-static
-void
-pstop_client_copy(pstop_client_data_t *dest, pstop_client_data_t *src)
-{
-    memcpy(dest, src, sizeof(pstop_client_data_t));
-}
-
 void
 pstop_clients_init(pstop_clients_t *clients)
 {
diff --git a/pstop_c/pstop/test/src/pstop/machine_timeout_test.c b/pstop_c/pstop/test/src/pstop/machine_timeout_test.c
index a8dd531..caa0beb 100644
--- a/pstop_c/pstop/test/src/pstop/machine_timeout_test.c
+++ b/pstop_c/pstop/test/src/pstop/machine_timeout_test.c
@@ -180,6 +180,8 @@ test_bond_stop_timeout_2_missed_timeouts(void)
 
     current_time = 222U; // now two missed heartbeats
     TEST_ASSERT_EQUAL(PSTOP_MISSED_HEARTBEATS, machine.check_heartbeats_cb(&machine));
+    TEST_ASSERT_EQUAL(ROBOT_STATE_STOPPED, machine.robot_state.robot_state);
+    TEST_ASSERT_EQUAL(ROBOT_RESTART_STATE_NEED_STOP, machine.robot_state.restart_state);
 }
 
 void