From d0276f846e47fc010c99988465d98efe857dea22 Mon Sep 17 00:00:00 2001
From: Squid5678 <tyagiaalind@gmail.com>
Date: Mon, 15 Jan 2024 02:42:59 +0000
Subject: [PATCH 01/12] temp marking

---
 soccer/src/soccer/strategy/agent/position/defense.cpp | 6 ++++++
 soccer/src/soccer/strategy/agent/position/defense.hpp | 1 +
 2 files changed, 7 insertions(+)

diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index 5a63c70db94..c8cfc632881 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -61,6 +61,8 @@ Defense::State Defense::update_state() {
             if (check_is_done()) {
                 next_state = IDLING;
             }
+        case MARKING:
+            break;
     }
 
     return next_state;
@@ -123,6 +125,10 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
             planning::MotionCommand{"path_target", current_location_instant, face_ball};
         intent.motion_command = face_ball_cmd;
         return intent;
+    } else if (current_state_ == MARKING) {
+        /*
+        
+        */
     }
 
     return std::nullopt;
diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp
index 86ee80e50f3..1a6e2217bdf 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.hpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.hpp
@@ -63,6 +63,7 @@ class Defense : public Position {
         RECEIVING,     // physically intercepting the ball from a pass
         PASSING,       // physically kicking the ball towards another robot
         FACING,        // turning to face the passing robot
+        MARKING
     };
 
     State update_state();

From 0512589bd27d6c24b5022e7834f4af56277e3767 Mon Sep 17 00:00:00 2001
From: jvogt23 <91633361+jvogt23@users.noreply.github.com>
Date: Tue, 16 Jan 2024 21:30:40 -0500
Subject: [PATCH 02/12] Added scaffolding for marker

---
 .../strategy/agent/position/defense.cpp       |  9 ++---
 .../soccer/strategy/agent/position/marker.cpp | 35 +++++++++++++++++++
 .../soccer/strategy/agent/position/marker.hpp | 24 +++++++++++++
 3 files changed, 64 insertions(+), 4 deletions(-)
 create mode 100644 soccer/src/soccer/strategy/agent/position/marker.cpp
 create mode 100644 soccer/src/soccer/strategy/agent/position/marker.hpp

diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index c8cfc632881..b3cf2705d7f 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -1,8 +1,10 @@
 #include "defense.hpp"
+#include "marker.hpp"
 
 namespace strategy {
 
-Defense::Defense(int r_id) : Position(r_id, "Defense") {}
+Defense::Defense(int r_id) : Position(r_id, "Defense") {
+if (this->robot_id_ == 4) current_state_ = MARKING;}
 
 std::optional<RobotIntent> Defense::derived_get_task(RobotIntent intent) {
     current_state_ = update_state();
@@ -126,9 +128,8 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
         intent.motion_command = face_ball_cmd;
         return intent;
     } else if (current_state_ == MARKING) {
-        /*
-        
-        */
+        Marker marker{(u_int8_t) this->robot_id_};
+        return marker.get_task(intent, last_world_state_, this->field_dimensions_);
     }
 
     return std::nullopt;
diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
new file mode 100644
index 00000000000..8aa9ede8aa0
--- /dev/null
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -0,0 +1,35 @@
+#include "marker.hpp"
+
+namespace strategy {
+Marker::Marker(u_int8_t robot_id) {
+    this->target_robot_id = robot_id;
+}
+
+std::optional<RobotIntent> Marker::get_task(RobotIntent intent,
+                                            const WorldState* world_state, FieldDimensions field_dimensions) {
+    auto ball_position = world_state->ball.position;
+
+    u_int8_t cntr = 0;
+    u_int8_t target_id = 0;
+    for (u_int8_t other_id = 0; other_id < kNumShells; other_id++) {
+        if (world_state->get_robot(false, other_id).visible) {
+            if (cntr == target_robot_id) {
+                target_id = other_id;
+                break;
+            }
+            cntr++;
+        }
+    }
+
+    // face ball
+    planning::PathTargetFaceOption face_option = planning::FaceBall{};
+    // planning::LinearMotionInstant goal{
+    //     (world_state->ball.position - ball_position) * factor + ball_position,
+    //     rj_geometry::Point{0.0, 0.0}};
+    auto target_position = (world_state->get_robot(false, target_id).pose.position() - ball_position) * factor + ball_position;
+    planning::LinearMotionInstant goal{target_position, rj_geometry::Point{0.0, 0.0}};
+    
+    intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, false};
+    return intent;
+}
+}  // namespace strategy
\ No newline at end of file
diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp
new file mode 100644
index 00000000000..2928a382034
--- /dev/null
+++ b/soccer/src/soccer/strategy/agent/position/marker.hpp
@@ -0,0 +1,24 @@
+#pragma once
+#include <rclcpp/rclcpp.hpp>
+
+#include <rj_msgs/msg/world_state.hpp>
+
+#include "position.hpp"
+#include "rj_geometry/point.hpp"
+#include "role_interface.hpp"
+#include <rj_msgs/msg/robot_state.hpp>
+
+
+namespace strategy {
+class Marker : public RoleInterface {
+private:
+    const double factor = 0.5;
+    u_int8_t target_robot_id = 0;
+
+public:
+    Marker(u_int8_t robot_id);
+    ~Marker() = default;
+    virtual std::optional<RobotIntent> get_task(RobotIntent intent,
+                                                const WorldState* const world_state, FieldDimensions field_dimensions) override;
+};
+}  // namespace strategy

From c1282ea616dc5e8f207d08dcd0c9970634841203 Mon Sep 17 00:00:00 2001
From: jvogt23 <91633361+jvogt23@users.noreply.github.com>
Date: Sun, 21 Jan 2024 21:30:54 -0500
Subject: [PATCH 03/12] Made marker follow robot and face ball

---
 soccer/src/soccer/CMakeLists.txt              |  1 +
 .../strategy/agent/position/defense.cpp       | 22 +++++++++----
 .../strategy/agent/position/defense.hpp       |  3 ++
 .../soccer/strategy/agent/position/marker.cpp | 32 ++++++-------------
 .../soccer/strategy/agent/position/marker.hpp |  3 +-
 5 files changed, 30 insertions(+), 31 deletions(-)

diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt
index f0fbf689c1b..42413708786 100644
--- a/soccer/src/soccer/CMakeLists.txt
+++ b/soccer/src/soccer/CMakeLists.txt
@@ -82,6 +82,7 @@ set(ROBOCUP_LIB_SRC
     strategy/agent/position/defense.cpp
     strategy/agent/position/waller.cpp
     strategy/agent/position/seeker.cpp
+    strategy/agent/position/marker.cpp
     strategy/agent/position/goal_kicker.cpp
     strategy/agent/position/penalty_player.cpp
         )
diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index b3cf2705d7f..c89e9e157cb 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -1,10 +1,9 @@
 #include "defense.hpp"
-#include "marker.hpp"
+
 
 namespace strategy {
 
-Defense::Defense(int r_id) : Position(r_id, "Defense") {
-if (this->robot_id_ == 4) current_state_ = MARKING;}
+Defense::Defense(int r_id) : Position(r_id, "Defense") {}
 
 std::optional<RobotIntent> Defense::derived_get_task(RobotIntent intent) {
     current_state_ = update_state();
@@ -67,10 +66,18 @@ Defense::State Defense::update_state() {
             break;
     }
 
+    if (robot_id_ == 2) {
+        next_state = MARKING;
+    }
+
     return next_state;
 }
 
 std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
+
+    // if (robot_id_ == 2) {
+    //     SPDLOG_INFO("{} current state of 2", current_state_);
+    // }
     if (current_state_ == IDLING) {
         auto empty_motion_cmd = planning::MotionCommand{};
         intent.motion_command = empty_motion_cmd;
@@ -117,7 +124,11 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
             Waller waller{waller_id_, (int)walling_robots_.size()};
             return waller.get_task(intent, last_world_state_, this->field_dimensions_);
         }
-    } else if (current_state_ = FACING) {
+    } 
+    else if (current_state_ == MARKING) {
+        Marker marker = Marker((u_int8_t) robot_id_);
+        return marker.get_task(intent, last_world_state_, this->field_dimensions_);
+    }else if (current_state_ == FACING) {
         rj_geometry::Point robot_position =
             last_world_state_->get_robot(true, robot_id_).pose.position();
         auto current_location_instant =
@@ -127,9 +138,6 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
             planning::MotionCommand{"path_target", current_location_instant, face_ball};
         intent.motion_command = face_ball_cmd;
         return intent;
-    } else if (current_state_ == MARKING) {
-        Marker marker{(u_int8_t) this->robot_id_};
-        return marker.get_task(intent, last_world_state_, this->field_dimensions_);
     }
 
     return std::nullopt;
diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp
index 1a6e2217bdf..cf8c465309f 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.hpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.hpp
@@ -17,6 +17,7 @@
 #include "rj_geometry/geometry_conversions.hpp"
 #include "rj_geometry/point.hpp"
 #include "waller.hpp"
+#include "marker.hpp"
 
 namespace strategy {
 
@@ -66,6 +67,8 @@ class Defense : public Position {
         MARKING
     };
 
+
+
     State update_state();
 
     std::optional<RobotIntent> state_to_task(RobotIntent intent);
diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
index 8aa9ede8aa0..2430758b96e 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.cpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -7,29 +7,15 @@ Marker::Marker(u_int8_t robot_id) {
 
 std::optional<RobotIntent> Marker::get_task(RobotIntent intent,
                                             const WorldState* world_state, FieldDimensions field_dimensions) {
-    auto ball_position = world_state->ball.position;
-
-    u_int8_t cntr = 0;
-    u_int8_t target_id = 0;
-    for (u_int8_t other_id = 0; other_id < kNumShells; other_id++) {
-        if (world_state->get_robot(false, other_id).visible) {
-            if (cntr == target_robot_id) {
-                target_id = other_id;
-                break;
-            }
-            cntr++;
-        }
-    }
-
-    // face ball
-    planning::PathTargetFaceOption face_option = planning::FaceBall{};
-    // planning::LinearMotionInstant goal{
-    //     (world_state->ball.position - ball_position) * factor + ball_position,
-    //     rj_geometry::Point{0.0, 0.0}};
-    auto target_position = (world_state->get_robot(false, target_id).pose.position() - ball_position) * factor + ball_position;
-    planning::LinearMotionInstant goal{target_position, rj_geometry::Point{0.0, 0.0}};
-    
-    intent.motion_command = planning::MotionCommand{"path_target", goal, face_option, false};
+    rj_geometry::Point targetPoint = 
+        world_state->get_robot(false, 4).pose.position();
+    rj_geometry::Point ballPoint = 
+        world_state->ball.position;
+    rj_geometry::Segment ballToTarget = rj_geometry::Segment(targetPoint, ballPoint);
+    rj_geometry::Segment ballToTargetBiased = 
+        rj_geometry::Segment(ballToTarget.center(), targetPoint);
+    planning::LinearMotionInstant goal{ballToTargetBiased.center(), rj_geometry::Point{0.0, 0.0}};
+    intent.motion_command = planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true};
     return intent;
 }
 }  // namespace strategy
\ No newline at end of file
diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp
index 2928a382034..5ea3e7838cb 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.hpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.hpp
@@ -7,6 +7,7 @@
 #include "rj_geometry/point.hpp"
 #include "role_interface.hpp"
 #include <rj_msgs/msg/robot_state.hpp>
+#include <rj_geometry/segment.hpp>
 
 
 namespace strategy {
@@ -18,7 +19,7 @@ class Marker : public RoleInterface {
 public:
     Marker(u_int8_t robot_id);
     ~Marker() = default;
-    virtual std::optional<RobotIntent> get_task(RobotIntent intent,
+    std::optional<RobotIntent> get_task(RobotIntent intent,
                                                 const WorldState* const world_state, FieldDimensions field_dimensions) override;
 };
 }  // namespace strategy

From 6a14ea78a09d69d3e8d9cf17f5dfff138d5bef01 Mon Sep 17 00:00:00 2001
From: jvogt23 <91633361+jvogt23@users.noreply.github.com>
Date: Tue, 23 Jan 2024 21:23:11 -0500
Subject: [PATCH 04/12] Added automatic target selection

---
 .../strategy/agent/position/defense.cpp       | 41 ++++++++++++++-----
 .../strategy/agent/position/defense.hpp       |  9 +++-
 .../soccer/strategy/agent/position/marker.cpp | 39 ++++++++++++++----
 .../soccer/strategy/agent/position/marker.hpp | 12 +++++-
 4 files changed, 81 insertions(+), 20 deletions(-)

diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index c89e9e157cb..d900792400e 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -3,7 +3,9 @@
 
 namespace strategy {
 
-Defense::Defense(int r_id) : Position(r_id, "Defense") {}
+Defense::Defense(int r_id) : 
+    Position(r_id, "Defense"), 
+    marker_{static_cast<uint8_t>(r_id)} {}
 
 std::optional<RobotIntent> Defense::derived_get_task(RobotIntent intent) {
     current_state_ = update_state();
@@ -31,6 +33,8 @@ Defense::State Defense::update_state() {
 
     switch (current_state_) {
         case IDLING:
+            if (robot_id_ == 2) 
+            SPDLOG_INFO("IN IDLING");
             break;
         case JOINING_WALL:
             send_join_wall_request();
@@ -63,11 +67,25 @@ Defense::State Defense::update_state() {
                 next_state = IDLING;
             }
         case MARKING:
+            if (marker_.get_target() == -1 || marker_.target_out_of_bounds(world_state)) {
+                SPDLOG_INFO("TRANSITION INTO IDLING FROM MARKING");
+                next_state = ENTERING_MARKING;
+            }
             break;
-    }
-
-    if (robot_id_ == 2) {
-        next_state = MARKING;
+        case ENTERING_MARKING:
+            SPDLOG_INFO("IN ENTERING MARKING");
+            int target_id = marker_.choose_target(world_state);
+            if (target_id == -1) {
+                SPDLOG_INFO("THE ID IS -1, ENTERING IDLING");
+                next_state = ENTERING_MARKING;
+            } else {
+                next_state = MARKING;
+            }
+        
+        // if (robot_id_ == 2 && has_already_run_enter_mark == false) {
+        //     next_state = ENTERING_MARKING;
+        //     has_already_run_enter_mark = true;
+        // }
     }
 
     return next_state;
@@ -124,11 +142,10 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
             Waller waller{waller_id_, (int)walling_robots_.size()};
             return waller.get_task(intent, last_world_state_, this->field_dimensions_);
         }
-    } 
-    else if (current_state_ == MARKING) {
-        Marker marker = Marker((u_int8_t) robot_id_);
-        return marker.get_task(intent, last_world_state_, this->field_dimensions_);
-    }else if (current_state_ == FACING) {
+    } else if (current_state_ == MARKING) {
+        //Marker marker = Marker((u_int8_t) robot_id_);
+        return marker_.get_task(intent, last_world_state_, this->field_dimensions_);
+    } else if (current_state_ == FACING) {
         rj_geometry::Point robot_position =
             last_world_state_->get_robot(true, robot_id_).pose.position();
         auto current_location_instant =
@@ -138,6 +155,10 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
             planning::MotionCommand{"path_target", current_location_instant, face_ball};
         intent.motion_command = face_ball_cmd;
         return intent;
+    } else if (current_state_ == ENTERING_MARKING) {
+        auto empty_motion_cmd = planning::MotionCommand{};
+        intent.motion_command = empty_motion_cmd;
+        return intent;
     }
 
     return std::nullopt;
diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp
index cf8c465309f..7254e8434f9 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.hpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.hpp
@@ -43,6 +43,9 @@ class Defense : public Position {
     void revive() override;
 
 private:
+
+    //REMOVE REMOVE REMOVE REMOVE REMOVE REMOVE REMOVE PLEASE FOR THE LOVE OF GOD REMOVE
+    bool has_already_run_enter_mark {false};
     int move_ct_ = 0;
 
     /**
@@ -64,7 +67,8 @@ class Defense : public Position {
         RECEIVING,     // physically intercepting the ball from a pass
         PASSING,       // physically kicking the ball towards another robot
         FACING,        // turning to face the passing robot
-        MARKING
+        MARKING,
+        ENTERING_MARKING,
     };
 
 
@@ -117,6 +121,9 @@ class Defense : public Position {
     // current state of the defense agent (state machine)
     int get_waller_id();
     State current_state_ = JOINING_WALL;
+
+    int get_marker_target_id();
+    Marker marker_;
 };
 
 }  // namespace strategy
diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
index 2430758b96e..ab1ffd7ba52 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.cpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -2,20 +2,45 @@
 
 namespace strategy {
 Marker::Marker(u_int8_t robot_id) {
-    this->target_robot_id = robot_id;
 }
 
 std::optional<RobotIntent> Marker::get_task(RobotIntent intent,
-                                            const WorldState* world_state, FieldDimensions field_dimensions) {
+    const WorldState* world_state, FieldDimensions field_dimensions) {
+    SPDLOG_INFO("Current target ID of robot: {}", target);
     rj_geometry::Point targetPoint = 
-        world_state->get_robot(false, 4).pose.position();
+        world_state->get_robot(false, target).pose.position();
     rj_geometry::Point ballPoint = 
         world_state->ball.position;
-    rj_geometry::Segment ballToTarget = rj_geometry::Segment(targetPoint, ballPoint);
-    rj_geometry::Segment ballToTargetBiased = 
-        rj_geometry::Segment(ballToTarget.center(), targetPoint);
-    planning::LinearMotionInstant goal{ballToTargetBiased.center(), rj_geometry::Point{0.0, 0.0}};
+    rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); 
+    planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}};
     intent.motion_command = planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true};
     return intent;
 }
+
+int Marker::choose_target(WorldState* ws) {
+
+    for (int i = 0; i < 11; i++) {
+        if (std::fabs(ws->get_robot(false, i).pose.position().x()) < 2.5
+            && ws->get_robot(false, i).pose.position().y() < Y_BOUND
+            && (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) {
+                target = i;
+                return i;
+        }
+    }
+    target = -1;
+    return -1;
+}
+
+bool Marker::target_out_of_bounds(WorldState* ws) {
+    if (target == -1) return true;
+    if (ws->get_robot(false, target).pose.position().y() > Y_BOUND) {
+        target = -1;
+        return true;
+    }
+    return false;
+}
+
+int Marker::get_target() {
+    return target;
+}
 }  // namespace strategy
\ No newline at end of file
diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp
index 5ea3e7838cb..ebc780509c8 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.hpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.hpp
@@ -14,12 +14,20 @@ namespace strategy {
 class Marker : public RoleInterface {
 private:
     const double factor = 0.5;
-    u_int8_t target_robot_id = 0;
+    int target {-1};
+
+    const double Y_BOUND {4.5};
 
 public:
     Marker(u_int8_t robot_id);
     ~Marker() = default;
     std::optional<RobotIntent> get_task(RobotIntent intent,
-                                                const WorldState* const world_state, FieldDimensions field_dimensions) override;
+        const WorldState* const world_state,
+        FieldDimensions field_dimensions) override;
+
+    int choose_target(WorldState* ws);
+    int get_target();
+    bool target_out_of_bounds(WorldState* ws);
+    
 };
 }  // namespace strategy

From 4389f91ab231decae32408c82c899efcdb22d02b Mon Sep 17 00:00:00 2001
From: jvogt23 <91633361+jvogt23@users.noreply.github.com>
Date: Sun, 28 Jan 2024 20:03:19 -0500
Subject: [PATCH 05/12] Added automatic setting of marker, still needs testing

---
 .../soccer/strategy/agent/position/defense.cpp  | 17 ++++++++---------
 .../soccer/strategy/agent/position/defense.hpp  |  3 ---
 .../soccer/strategy/agent/position/marker.cpp   |  1 -
 3 files changed, 8 insertions(+), 13 deletions(-)

diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index d900792400e..458e2428ab5 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -33,8 +33,6 @@ Defense::State Defense::update_state() {
 
     switch (current_state_) {
         case IDLING:
-            if (robot_id_ == 2) 
-            SPDLOG_INFO("IN IDLING");
             break;
         case JOINING_WALL:
             send_join_wall_request();
@@ -42,6 +40,13 @@ Defense::State Defense::update_state() {
             walling_robots_ = {(u_int8_t)robot_id_};
             break;
         case WALLING:
+            //Should remove the robot with the highest ID from a wall
+            //and make them a marker instead.
+            if (this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end())
+                && walling_robots_.size() > 3) {
+                send_leave_wall_request();
+                next_state = ENTERING_MARKING;
+            }
             break;
         case SEARCHING:
             break;
@@ -68,24 +73,18 @@ Defense::State Defense::update_state() {
             }
         case MARKING:
             if (marker_.get_target() == -1 || marker_.target_out_of_bounds(world_state)) {
-                SPDLOG_INFO("TRANSITION INTO IDLING FROM MARKING");
                 next_state = ENTERING_MARKING;
             }
             break;
         case ENTERING_MARKING:
-            SPDLOG_INFO("IN ENTERING MARKING");
             int target_id = marker_.choose_target(world_state);
             if (target_id == -1) {
-                SPDLOG_INFO("THE ID IS -1, ENTERING IDLING");
                 next_state = ENTERING_MARKING;
             } else {
                 next_state = MARKING;
             }
         
-        // if (robot_id_ == 2 && has_already_run_enter_mark == false) {
-        //     next_state = ENTERING_MARKING;
-        //     has_already_run_enter_mark = true;
-        // }
+
     }
 
     return next_state;
diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp
index 7254e8434f9..ea226f2f5f8 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.hpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.hpp
@@ -43,9 +43,6 @@ class Defense : public Position {
     void revive() override;
 
 private:
-
-    //REMOVE REMOVE REMOVE REMOVE REMOVE REMOVE REMOVE PLEASE FOR THE LOVE OF GOD REMOVE
-    bool has_already_run_enter_mark {false};
     int move_ct_ = 0;
 
     /**
diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
index ab1ffd7ba52..f44893a3920 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.cpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -6,7 +6,6 @@ Marker::Marker(u_int8_t robot_id) {
 
 std::optional<RobotIntent> Marker::get_task(RobotIntent intent,
     const WorldState* world_state, FieldDimensions field_dimensions) {
-    SPDLOG_INFO("Current target ID of robot: {}", target);
     rj_geometry::Point targetPoint = 
         world_state->get_robot(false, target).pose.position();
     rj_geometry::Point ballPoint = 

From 296ef4aebfeb4c253da9f708d6a2ab9c5a51cb9b Mon Sep 17 00:00:00 2001
From: jvogt23 <91633361+jvogt23@users.noreply.github.com>
Date: Sun, 28 Jan 2024 20:59:56 -0500
Subject: [PATCH 06/12] cleaning up code from previous commit

---
 .../src/soccer/strategy/agent/position/defense.cpp   | 12 +++++++-----
 .../src/soccer/strategy/agent/position/defense.hpp   |  6 ++++--
 soccer/src/soccer/strategy/agent/position/marker.cpp |  4 ++++
 soccer/src/soccer/strategy/agent/position/marker.hpp |  8 ++++++--
 4 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index 458e2428ab5..619984cd5ed 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -40,10 +40,11 @@ Defense::State Defense::update_state() {
             walling_robots_ = {(u_int8_t)robot_id_};
             break;
         case WALLING:
-            //Should remove the robot with the highest ID from a wall
+            //If a wall is already full,
+            //Remove the robot with the highest ID from a wall
             //and make them a marker instead.
             if (this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end())
-                && walling_robots_.size() > 3) {
+                && walling_robots_.size() > MAX_WALLERS) {
                 send_leave_wall_request();
                 next_state = ENTERING_MARKING;
             }
@@ -141,9 +142,6 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
             Waller waller{waller_id_, (int)walling_robots_.size()};
             return waller.get_task(intent, last_world_state_, this->field_dimensions_);
         }
-    } else if (current_state_ == MARKING) {
-        //Marker marker = Marker((u_int8_t) robot_id_);
-        return marker_.get_task(intent, last_world_state_, this->field_dimensions_);
     } else if (current_state_ == FACING) {
         rj_geometry::Point robot_position =
             last_world_state_->get_robot(true, robot_id_).pose.position();
@@ -155,9 +153,13 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
         intent.motion_command = face_ball_cmd;
         return intent;
     } else if (current_state_ == ENTERING_MARKING) {
+        //Prepares a robot for marking. NOTE: May update to add move to center of field
         auto empty_motion_cmd = planning::MotionCommand{};
         intent.motion_command = empty_motion_cmd;
         return intent;
+    } else if (current_state_ == MARKING) {
+        //Marker marker = Marker((u_int8_t) robot_id_);
+        return marker_.get_task(intent, last_world_state_, this->field_dimensions_);
     }
 
     return std::nullopt;
diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp
index ea226f2f5f8..2064b83f60a 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.hpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.hpp
@@ -45,6 +45,8 @@ class Defense : public Position {
 private:
     int move_ct_ = 0;
 
+    static constexpr int MAX_WALLERS {3};
+
     /**
      * @brief The derived_get_task method returns the task for the defensive robot
      *  to do based on the game situation. The method will continuously look to assign
@@ -64,8 +66,8 @@ class Defense : public Position {
         RECEIVING,     // physically intercepting the ball from a pass
         PASSING,       // physically kicking the ball towards another robot
         FACING,        // turning to face the passing robot
-        MARKING,
-        ENTERING_MARKING,
+        MARKING,       // Following closely to an offense robot
+        ENTERING_MARKING, // Choosing/waiting for a robot to mark
     };
 
 
diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
index f44893a3920..3955f10b45f 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.cpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -18,6 +18,10 @@ std::optional<RobotIntent> Marker::get_task(RobotIntent intent,
 
 int Marker::choose_target(WorldState* ws) {
 
+    //TODO: (James Vogt, github: jvogt23)
+    //If we ever use multiple Markers, they should choose different
+    //robots to track from each other. Logic for this operation must be
+    //added because multiple markers currently mark the same robot.
     for (int i = 0; i < 11; i++) {
         if (std::fabs(ws->get_robot(false, i).pose.position().x()) < 2.5
             && ws->get_robot(false, i).pose.position().y() < Y_BOUND
diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp
index ebc780509c8..0159875b60f 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.hpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.hpp
@@ -11,12 +11,16 @@
 
 
 namespace strategy {
+
+/**
+ * Represents a Marker, a defensive role that targets an enemy robot
+ * and follows it around the field while it is on our side, blocking passes.
+*/
 class Marker : public RoleInterface {
 private:
-    const double factor = 0.5;
     int target {-1};
 
-    const double Y_BOUND {4.5};
+    static constexpr double Y_BOUND {4.5};
 
 public:
     Marker(u_int8_t robot_id);

From 45b855b105bf4d8c4fcfcbfead46c75028930c09 Mon Sep 17 00:00:00 2001
From: "github-actions[bot]"
 <41898282+github-actions[bot]@users.noreply.github.com>
Date: Sun, 28 Jan 2024 21:34:10 -0500
Subject: [PATCH 07/12] Fix Code Style On ja_marker_behavior (#2177)

automated style fixes

Co-authored-by: jvogt23 <jvogt23@users.noreply.github.com>
---
 .../strategy/agent/position/defense.cpp       | 18 ++++----
 .../strategy/agent/position/defense.hpp       | 24 +++++------
 .../soccer/strategy/agent/position/marker.cpp | 41 ++++++++-----------
 .../soccer/strategy/agent/position/marker.hpp | 17 ++++----
 4 files changed, 43 insertions(+), 57 deletions(-)

diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index 619984cd5ed..3aea132b169 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -1,6 +1,5 @@
 #include "defense.hpp"
 
-
 namespace strategy {
 
 Defense::Defense(int r_id) : 
@@ -40,11 +39,11 @@ Defense::State Defense::update_state() {
             walling_robots_ = {(u_int8_t)robot_id_};
             break;
         case WALLING:
-            //If a wall is already full,
-            //Remove the robot with the highest ID from a wall
-            //and make them a marker instead.
-            if (this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end())
-                && walling_robots_.size() > MAX_WALLERS) {
+            // If a wall is already full,
+            // Remove the robot with the highest ID from a wall
+            // and make them a marker instead.
+            if (this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end()) &&
+                walling_robots_.size() > MAX_WALLERS) {
                 send_leave_wall_request();
                 next_state = ENTERING_MARKING;
             }
@@ -84,15 +83,12 @@ Defense::State Defense::update_state() {
             } else {
                 next_state = MARKING;
             }
-        
-
     }
 
     return next_state;
 }
 
 std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
-
     // if (robot_id_ == 2) {
     //     SPDLOG_INFO("{} current state of 2", current_state_);
     // }
@@ -153,12 +149,12 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
         intent.motion_command = face_ball_cmd;
         return intent;
     } else if (current_state_ == ENTERING_MARKING) {
-        //Prepares a robot for marking. NOTE: May update to add move to center of field
+        // Prepares a robot for marking. NOTE: May update to add move to center of field
         auto empty_motion_cmd = planning::MotionCommand{};
         intent.motion_command = empty_motion_cmd;
         return intent;
     } else if (current_state_ == MARKING) {
-        //Marker marker = Marker((u_int8_t) robot_id_);
+        // Marker marker = Marker((u_int8_t) robot_id_);
         return marker_.get_task(intent, last_world_state_, this->field_dimensions_);
     }
 
diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp
index 2064b83f60a..b92f2ffb674 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.hpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.hpp
@@ -9,6 +9,7 @@
 
 #include <rj_msgs/action/robot_move.hpp>
 
+#include "marker.hpp"
 #include "planning/instant.hpp"
 #include "position.hpp"
 #include "rj_common/field_dimensions.hpp"
@@ -17,7 +18,6 @@
 #include "rj_geometry/geometry_conversions.hpp"
 #include "rj_geometry/point.hpp"
 #include "waller.hpp"
-#include "marker.hpp"
 
 namespace strategy {
 
@@ -45,7 +45,7 @@ class Defense : public Position {
 private:
     int move_ct_ = 0;
 
-    static constexpr int MAX_WALLERS {3};
+    static constexpr int MAX_WALLERS{3};
 
     /**
      * @brief The derived_get_task method returns the task for the defensive robot
@@ -59,19 +59,17 @@ class Defense : public Position {
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
 
     enum State {
-        IDLING,        // simply staying in place
-        JOINING_WALL,  // send message to find its place in the wall
-        WALLING,       // participating in the wall
-        SEARCHING,     // moving around on the field to do something
-        RECEIVING,     // physically intercepting the ball from a pass
-        PASSING,       // physically kicking the ball towards another robot
-        FACING,        // turning to face the passing robot
-        MARKING,       // Following closely to an offense robot
-        ENTERING_MARKING, // Choosing/waiting for a robot to mark
+        IDLING,            // simply staying in place
+        JOINING_WALL,      // send message to find its place in the wall
+        WALLING,           // participating in the wall
+        SEARCHING,         // moving around on the field to do something
+        RECEIVING,         // physically intercepting the ball from a pass
+        PASSING,           // physically kicking the ball towards another robot
+        FACING,            // turning to face the passing robot
+        MARKING,           // Following closely to an offense robot
+        ENTERING_MARKING,  // Choosing/waiting for a robot to mark
     };
 
-
-
     State update_state();
 
     std::optional<RobotIntent> state_to_task(RobotIntent intent);
diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
index 3955f10b45f..d313e5badb7 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.cpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -1,33 +1,30 @@
 #include "marker.hpp"
 
 namespace strategy {
-Marker::Marker(u_int8_t robot_id) {
-}
+Marker::Marker(u_int8_t robot_id) {}
 
-std::optional<RobotIntent> Marker::get_task(RobotIntent intent,
-    const WorldState* world_state, FieldDimensions field_dimensions) {
-    rj_geometry::Point targetPoint = 
-        world_state->get_robot(false, target).pose.position();
-    rj_geometry::Point ballPoint = 
-        world_state->ball.position;
-    rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); 
+std::optional<RobotIntent> Marker::get_task(RobotIntent intent, const WorldState* world_state,
+                                            FieldDimensions field_dimensions) {
+    rj_geometry::Point targetPoint = world_state->get_robot(false, target).pose.position();
+    rj_geometry::Point ballPoint = world_state->ball.position;
+    rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f);
     planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}};
-    intent.motion_command = planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true};
+    intent.motion_command =
+        planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true};
     return intent;
 }
 
 int Marker::choose_target(WorldState* ws) {
-
-    //TODO: (James Vogt, github: jvogt23)
-    //If we ever use multiple Markers, they should choose different
-    //robots to track from each other. Logic for this operation must be
-    //added because multiple markers currently mark the same robot.
+    // TODO: (James Vogt, github: jvogt23)
+    // If we ever use multiple Markers, they should choose different
+    // robots to track from each other. Logic for this operation must be
+    // added because multiple markers currently mark the same robot.
     for (int i = 0; i < 11; i++) {
-        if (std::fabs(ws->get_robot(false, i).pose.position().x()) < 2.5
-            && ws->get_robot(false, i).pose.position().y() < Y_BOUND
-            && (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) {
-                target = i;
-                return i;
+        if (std::fabs(ws->get_robot(false, i).pose.position().x()) < 2.5 &&
+            ws->get_robot(false, i).pose.position().y() < Y_BOUND &&
+            (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) {
+            target = i;
+            return i;
         }
     }
     target = -1;
@@ -43,7 +40,5 @@ bool Marker::target_out_of_bounds(WorldState* ws) {
     return false;
 }
 
-int Marker::get_target() {
-    return target;
-}
+int Marker::get_target() { return target; }
 }  // namespace strategy
\ No newline at end of file
diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp
index 0159875b60f..519936cbe71 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.hpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.hpp
@@ -1,37 +1,34 @@
 #pragma once
 #include <rclcpp/rclcpp.hpp>
 
+#include <rj_geometry/segment.hpp>
+#include <rj_msgs/msg/robot_state.hpp>
 #include <rj_msgs/msg/world_state.hpp>
 
 #include "position.hpp"
 #include "rj_geometry/point.hpp"
 #include "role_interface.hpp"
-#include <rj_msgs/msg/robot_state.hpp>
-#include <rj_geometry/segment.hpp>
-
 
 namespace strategy {
 
 /**
  * Represents a Marker, a defensive role that targets an enemy robot
  * and follows it around the field while it is on our side, blocking passes.
-*/
+ */
 class Marker : public RoleInterface {
 private:
-    int target {-1};
+    int target{-1};
 
-    static constexpr double Y_BOUND {4.5};
+    static constexpr double Y_BOUND{4.5};
 
 public:
     Marker(u_int8_t robot_id);
     ~Marker() = default;
-    std::optional<RobotIntent> get_task(RobotIntent intent,
-        const WorldState* const world_state,
-        FieldDimensions field_dimensions) override;
+    std::optional<RobotIntent> get_task(RobotIntent intent, const WorldState* const world_state,
+                                        FieldDimensions field_dimensions) override;
 
     int choose_target(WorldState* ws);
     int get_target();
     bool target_out_of_bounds(WorldState* ws);
-    
 };
 }  // namespace strategy

From 7cdfb6eb4e6e13edbc9fab9b27b37f40495f6ee3 Mon Sep 17 00:00:00 2001
From: jvogt23 <91633361+jvogt23@users.noreply.github.com>
Date: Sun, 3 Mar 2024 21:34:30 -0500
Subject: [PATCH 08/12] Fixed conversations for code style

---
 external/robocup-fshare                       |  2 +-
 .../strategy/agent/position/defense.cpp       |  9 +++---
 .../strategy/agent/position/defense.hpp       |  2 +-
 .../soccer/strategy/agent/position/marker.cpp | 31 +++++++++----------
 .../soccer/strategy/agent/position/marker.hpp | 19 ++++++++----
 5 files changed, 35 insertions(+), 28 deletions(-)

diff --git a/external/robocup-fshare b/external/robocup-fshare
index 634ac6b7955..bf443b29144 160000
--- a/external/robocup-fshare
+++ b/external/robocup-fshare
@@ -1 +1 @@
-Subproject commit 634ac6b79552842f88643dde616db3bb2a95f9bb
+Subproject commit bf443b29144cd0f19b1a1ff31efec0beec0d3ff4
diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index 3aea132b169..d666308a051 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -4,7 +4,7 @@ namespace strategy {
 
 Defense::Defense(int r_id) : 
     Position(r_id, "Defense"), 
-    marker_{static_cast<uint8_t>(r_id)} {}
+    marker_{} {}
 
 std::optional<RobotIntent> Defense::derived_get_task(RobotIntent intent) {
     current_state_ = update_state();
@@ -42,8 +42,8 @@ Defense::State Defense::update_state() {
             // If a wall is already full,
             // Remove the robot with the highest ID from a wall
             // and make them a marker instead.
-            if (this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end()) &&
-                walling_robots_.size() > MAX_WALLERS) {
+            if (walling_robots_.size() > kMaxWallers && 
+                this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end())) {
                 send_leave_wall_request();
                 next_state = ENTERING_MARKING;
             }
@@ -77,7 +77,8 @@ Defense::State Defense::update_state() {
             }
             break;
         case ENTERING_MARKING:
-            int target_id = marker_.choose_target(world_state);
+            marker_.choose_target(world_state);
+            int target_id = marker_.get_target();
             if (target_id == -1) {
                 next_state = ENTERING_MARKING;
             } else {
diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp
index b92f2ffb674..0234fec7d03 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.hpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.hpp
@@ -45,7 +45,7 @@ class Defense : public Position {
 private:
     int move_ct_ = 0;
 
-    static constexpr int MAX_WALLERS{3};
+    static constexpr int kMaxWallers {3};
 
     /**
      * @brief The derived_get_task method returns the task for the defensive robot
diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
index d313e5badb7..582145e2dc4 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.cpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -1,11 +1,11 @@
 #include "marker.hpp"
 
 namespace strategy {
-Marker::Marker(u_int8_t robot_id) {}
+Marker::Marker() {}
 
 std::optional<RobotIntent> Marker::get_task(RobotIntent intent, const WorldState* world_state,
-                                            FieldDimensions field_dimensions) {
-    rj_geometry::Point targetPoint = world_state->get_robot(false, target).pose.position();
+                                            [[maybe_unused]] FieldDimensions field_dimensions) {
+    rj_geometry::Point targetPoint = world_state->get_robot(false, target_).pose.position();
     rj_geometry::Point ballPoint = world_state->ball.position;
     rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f);
     planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}};
@@ -14,31 +14,30 @@ std::optional<RobotIntent> Marker::get_task(RobotIntent intent, const WorldState
     return intent;
 }
 
-int Marker::choose_target(WorldState* ws) {
+void Marker::choose_target(const WorldState* ws) {
     // TODO: (James Vogt, github: jvogt23)
     // If we ever use multiple Markers, they should choose different
     // robots to track from each other. Logic for this operation must be
     // added because multiple markers currently mark the same robot.
-    for (int i = 0; i < 11; i++) {
-        if (std::fabs(ws->get_robot(false, i).pose.position().x()) < 2.5 &&
-            ws->get_robot(false, i).pose.position().y() < Y_BOUND &&
+    for (int i = 0; i < kNumShells; i++) {
+        if (std::fabs(ws->get_robot(false, i).pose.position().x()) < kMarkerFollowCutoff &&
+            ws->get_robot(false, i).pose.position().y() < kYBound &&
             (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) {
-            target = i;
-            return i;
+            target_ = i;
+            return; 
         }
     }
-    target = -1;
-    return -1;
+    target_ = -1;
 }
 
-bool Marker::target_out_of_bounds(WorldState* ws) {
-    if (target == -1) return true;
-    if (ws->get_robot(false, target).pose.position().y() > Y_BOUND) {
-        target = -1;
+bool Marker::target_out_of_bounds(const WorldState* ws) {
+    if (target_ == -1) return true;
+    if (ws->get_robot(false, target_).pose.position().y() > kYBound) {
+        target_ = -1;
         return true;
     }
     return false;
 }
 
-int Marker::get_target() { return target; }
+int Marker::get_target() { return target_; }
 }  // namespace strategy
\ No newline at end of file
diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp
index 519936cbe71..c4d23aa2668 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.hpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.hpp
@@ -17,18 +17,25 @@ namespace strategy {
  */
 class Marker : public RoleInterface {
 private:
-    int target{-1};
+    int target_{-1};
 
-    static constexpr double Y_BOUND{4.5};
+    static constexpr double kYBound{4.5};
+    //Constant - Avoid marking robots that are right on the edge of the field.
+    static constexpr double kMarkerFollowCutoff {2.5};
 
 public:
-    Marker(u_int8_t robot_id);
+    Marker();
     ~Marker() = default;
-    std::optional<RobotIntent> get_task(RobotIntent intent, const WorldState* const world_state,
+    Marker(const Marker& other) = default;
+    Marker(Marker&& other) = default;
+    Marker& operator=(const Marker& other) = default;
+    Marker& operator=(Marker&& other) = default;
+
+    std::optional<RobotIntent> get_task(RobotIntent intent, const WorldState* world_state,
                                         FieldDimensions field_dimensions) override;
 
-    int choose_target(WorldState* ws);
+    void choose_target(const WorldState* ws);
     int get_target();
-    bool target_out_of_bounds(WorldState* ws);
+    bool target_out_of_bounds(const WorldState* ws);
 };
 }  // namespace strategy

From de401e41dec539f8af3bf8a2ad77c6182c018937 Mon Sep 17 00:00:00 2001
From: jvogt23 <91633361+jvogt23@users.noreply.github.com>
Date: Tue, 5 Mar 2024 19:39:47 -0500
Subject: [PATCH 09/12] Made marker.cpp use FieldDimensions instead of
 constants

---
 soccer/src/soccer/strategy/agent/position/defense.cpp |  2 +-
 soccer/src/soccer/strategy/agent/position/marker.cpp  | 11 +++++++----
 soccer/src/soccer/strategy/agent/position/marker.hpp  | 10 ++++++----
 3 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index d666308a051..c8c9574172f 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -4,7 +4,7 @@ namespace strategy {
 
 Defense::Defense(int r_id) : 
     Position(r_id, "Defense"), 
-    marker_{} {}
+    marker_{field_dimensions_} {}
 
 std::optional<RobotIntent> Defense::derived_get_task(RobotIntent intent) {
     current_state_ = update_state();
diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
index 582145e2dc4..fe76a6c30c3 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.cpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -1,7 +1,10 @@
 #include "marker.hpp"
 
 namespace strategy {
-Marker::Marker() {}
+Marker::Marker(FieldDimensions field_dimensions) {
+    this->y_bound = field_dimensions.length() / 2;
+    this->marker_follow_cutoff = field_dimensions.width() / 2;
+}
 
 std::optional<RobotIntent> Marker::get_task(RobotIntent intent, const WorldState* world_state,
                                             [[maybe_unused]] FieldDimensions field_dimensions) {
@@ -20,8 +23,8 @@ void Marker::choose_target(const WorldState* ws) {
     // robots to track from each other. Logic for this operation must be
     // added because multiple markers currently mark the same robot.
     for (int i = 0; i < kNumShells; i++) {
-        if (std::fabs(ws->get_robot(false, i).pose.position().x()) < kMarkerFollowCutoff &&
-            ws->get_robot(false, i).pose.position().y() < kYBound &&
+        if (std::fabs(ws->get_robot(false, i).pose.position().x()) < marker_follow_cutoff &&
+            ws->get_robot(false, i).pose.position().y() < y_bound &&
             (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) {
             target_ = i;
             return; 
@@ -32,7 +35,7 @@ void Marker::choose_target(const WorldState* ws) {
 
 bool Marker::target_out_of_bounds(const WorldState* ws) {
     if (target_ == -1) return true;
-    if (ws->get_robot(false, target_).pose.position().y() > kYBound) {
+    if (ws->get_robot(false, target_).pose.position().y() > y_bound) {
         target_ = -1;
         return true;
     }
diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp
index c4d23aa2668..8f0eb4aa542 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.hpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.hpp
@@ -19,12 +19,14 @@ class Marker : public RoleInterface {
 private:
     int target_{-1};
 
-    static constexpr double kYBound{4.5};
-    //Constant - Avoid marking robots that are right on the edge of the field.
-    static constexpr double kMarkerFollowCutoff {2.5};
+    //Calculated from field dimensions - Prevent the marker from
+    //marking any enemies that are out of range; e. g. on the other
+    //side of the field or the sidelines.
+    float y_bound {0.0f};
+    float marker_follow_cutoff {0.0f};
 
 public:
-    Marker();
+    Marker(FieldDimensions field_dimensions);
     ~Marker() = default;
     Marker(const Marker& other) = default;
     Marker(Marker&& other) = default;

From bbc6281332bb234d97b3106fda36c15a6ce291ef Mon Sep 17 00:00:00 2001
From: Sid Parikh <parikhsid16@gmail.com>
Date: Tue, 5 Mar 2024 20:09:44 -0500
Subject: [PATCH 10/12] update field dims

---
 soccer/src/soccer/strategy/agent/position/marker.cpp | 4 ++++
 soccer/src/soccer/strategy/agent/position/marker.hpp | 4 ++--
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
index fe76a6c30c3..fd39297d74f 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.cpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -8,6 +8,10 @@ Marker::Marker(FieldDimensions field_dimensions) {
 
 std::optional<RobotIntent> Marker::get_task(RobotIntent intent, const WorldState* world_state,
                                             [[maybe_unused]] FieldDimensions field_dimensions) {
+
+    this->y_bound = field_dimensions.length() / 2;
+    this->marker_follow_cutoff = field_dimensions.width() / 2;
+
     rj_geometry::Point targetPoint = world_state->get_robot(false, target_).pose.position();
     rj_geometry::Point ballPoint = world_state->ball.position;
     rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f);
diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp
index 8f0eb4aa542..cd6cf9579d1 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.hpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.hpp
@@ -22,8 +22,8 @@ class Marker : public RoleInterface {
     //Calculated from field dimensions - Prevent the marker from
     //marking any enemies that are out of range; e. g. on the other
     //side of the field or the sidelines.
-    float y_bound {0.0f};
-    float marker_follow_cutoff {0.0f};
+    float y_bound {FieldDimensions::kDefaultDimensions.length() / 2};
+    float marker_follow_cutoff {FieldDimensions::kDefaultDimensions.width() / 2};
 
 public:
     Marker(FieldDimensions field_dimensions);

From e3b19932facf4a0d8b285b1cb86b063653a65a80 Mon Sep 17 00:00:00 2001
From: Sid Parikh <parikhsid16@gmail.com>
Date: Tue, 5 Mar 2024 20:31:34 -0500
Subject: [PATCH 11/12] actually actually fix fshare

---
 external/robocup-fshare | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/external/robocup-fshare b/external/robocup-fshare
index bf443b29144..634ac6b7955 160000
--- a/external/robocup-fshare
+++ b/external/robocup-fshare
@@ -1 +1 @@
-Subproject commit bf443b29144cd0f19b1a1ff31efec0beec0d3ff4
+Subproject commit 634ac6b79552842f88643dde616db3bb2a95f9bb

From 0c207344f4358c5fe8caef8cd05d9467daa14a70 Mon Sep 17 00:00:00 2001
From: Sid Parikh <parikhsid16@gmail.com>
Date: Tue, 5 Mar 2024 20:34:26 -0500
Subject: [PATCH 12/12] pretty-lines

---
 soccer/src/soccer/strategy/agent/position/defense.cpp |  6 ++----
 soccer/src/soccer/strategy/agent/position/defense.hpp |  2 +-
 soccer/src/soccer/strategy/agent/position/marker.cpp  |  3 +--
 soccer/src/soccer/strategy/agent/position/marker.hpp  | 10 +++++-----
 4 files changed, 9 insertions(+), 12 deletions(-)

diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp
index c8c9574172f..7eae69cfaaf 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.cpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.cpp
@@ -2,9 +2,7 @@
 
 namespace strategy {
 
-Defense::Defense(int r_id) : 
-    Position(r_id, "Defense"), 
-    marker_{field_dimensions_} {}
+Defense::Defense(int r_id) : Position(r_id, "Defense"), marker_{field_dimensions_} {}
 
 std::optional<RobotIntent> Defense::derived_get_task(RobotIntent intent) {
     current_state_ = update_state();
@@ -42,7 +40,7 @@ Defense::State Defense::update_state() {
             // If a wall is already full,
             // Remove the robot with the highest ID from a wall
             // and make them a marker instead.
-            if (walling_robots_.size() > kMaxWallers && 
+            if (walling_robots_.size() > kMaxWallers &&
                 this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end())) {
                 send_leave_wall_request();
                 next_state = ENTERING_MARKING;
diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp
index 0234fec7d03..7254f1fe12b 100644
--- a/soccer/src/soccer/strategy/agent/position/defense.hpp
+++ b/soccer/src/soccer/strategy/agent/position/defense.hpp
@@ -45,7 +45,7 @@ class Defense : public Position {
 private:
     int move_ct_ = 0;
 
-    static constexpr int kMaxWallers {3};
+    static constexpr int kMaxWallers{3};
 
     /**
      * @brief The derived_get_task method returns the task for the defensive robot
diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp
index fd39297d74f..ea8924c0f66 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.cpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.cpp
@@ -8,7 +8,6 @@ Marker::Marker(FieldDimensions field_dimensions) {
 
 std::optional<RobotIntent> Marker::get_task(RobotIntent intent, const WorldState* world_state,
                                             [[maybe_unused]] FieldDimensions field_dimensions) {
-
     this->y_bound = field_dimensions.length() / 2;
     this->marker_follow_cutoff = field_dimensions.width() / 2;
 
@@ -31,7 +30,7 @@ void Marker::choose_target(const WorldState* ws) {
             ws->get_robot(false, i).pose.position().y() < y_bound &&
             (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) {
             target_ = i;
-            return; 
+            return;
         }
     }
     target_ = -1;
diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp
index cd6cf9579d1..eb27304401e 100644
--- a/soccer/src/soccer/strategy/agent/position/marker.hpp
+++ b/soccer/src/soccer/strategy/agent/position/marker.hpp
@@ -19,11 +19,11 @@ class Marker : public RoleInterface {
 private:
     int target_{-1};
 
-    //Calculated from field dimensions - Prevent the marker from
-    //marking any enemies that are out of range; e. g. on the other
-    //side of the field or the sidelines.
-    float y_bound {FieldDimensions::kDefaultDimensions.length() / 2};
-    float marker_follow_cutoff {FieldDimensions::kDefaultDimensions.width() / 2};
+    // Calculated from field dimensions - Prevent the marker from
+    // marking any enemies that are out of range; e. g. on the other
+    // side of the field or the sidelines.
+    float y_bound{FieldDimensions::kDefaultDimensions.length() / 2};
+    float marker_follow_cutoff{FieldDimensions::kDefaultDimensions.width() / 2};
 
 public:
     Marker(FieldDimensions field_dimensions);