From 0a6470a0965decd316e4d5a81bd04d5be5801123 Mon Sep 17 00:00:00 2001
From: Peter Woolery <peter@research.bike>
Date: Mon, 13 Apr 2026 15:10:01 -0700
Subject: [PATCH] feat: CV line-crossing entry/exit detection with tests

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 firmware/lib/cv/cv.cpp            | 19 +++++++-
 firmware/test/test_cv/test_cv.cpp | 73 +++++++++++++++++++++++++++++++
 2 files changed, 91 insertions(+), 1 deletion(-)

diff --git a/firmware/lib/cv/cv.cpp b/firmware/lib/cv/cv.cpp
index ca4186c..7f0be59 100644
--- a/firmware/lib/cv/cv.cpp
+++ b/firmware/lib/cv/cv.cpp
@@ -146,6 +146,23 @@ CVResult cv_process(CVState& state, const uint8_t* frame, uint8_t line_pct) {
         t.missed     = 0;
         state.tracks.push_back(t);
     }
-    // Line crossing check added in Task 6
+    // Line crossing check
+    for (auto& track : state.tracks) {
+        if (track.missed > 0) continue; // only check tracks matched this frame
+        bool now_above = (track.y < line_y);
+        if (now_above != track.above_line) {
+            if (!now_above) {
+                // was above, now below → entry
+                state.entries++;
+                result.entries_delta++;
+            } else {
+                // was below, now above → exit
+                state.exits++;
+                result.exits_delta++;
+            }
+        }
+        track.above_line = now_above;
+    }
+
     return result;
 }
diff --git a/firmware/test/test_cv/test_cv.cpp b/firmware/test/test_cv/test_cv.cpp
index d8aae25..72b9cba 100644
--- a/firmware/test/test_cv/test_cv.cpp
+++ b/firmware/test/test_cv/test_cv.cpp
@@ -82,6 +82,76 @@ void test_tracking_spawns_track_for_new_blob() {
     TEST_ASSERT_FLOAT_WITHIN(5.0f, 20.0f, state.tracks[0].y);
 }
 
+static void make_blob_frame(uint8_t* f, int cx, int cy) {
+    fill_frame(f, 100);
+    for (int y = cy - 12; y <= cy + 12; y++)
+        for (int x = cx - 12; x <= cx + 12; x++)
+            if (y >= 0 && y < CV_H && x >= 0 && x < CV_W)
+                f[y * CV_W + x] = 200;
+}
+
+void test_blob_crossing_line_top_to_bottom_is_entry() {
+    CVState state;
+    cv_init(state);
+
+    // Line at 50% = y=48; step ≤14px per frame to stay within CV_MAX_MOVE
+    uint8_t bg[CV_PIXELS];
+    fill_frame(bg, 100);
+    cv_process(state, bg, 50); // init background
+
+    // Walk blob from y=20 toward line; crossing occurs at y=48 (above→below)
+    // Stop at crossing frame and assert its result
+    int setup[] = {20, 34};
+    for (int i = 0; i < 2; i++) {
+        uint8_t f[CV_PIXELS]; make_blob_frame(f, 48, setup[i]);
+        cv_process(state, f, 50);
+    }
+    uint8_t fcross[CV_PIXELS]; make_blob_frame(fcross, 48, 48);
+    CVResult r = cv_process(state, fcross, 50);
+
+    TEST_ASSERT_EQUAL_INT(1, r.entries_delta);
+    TEST_ASSERT_EQUAL_INT(0, r.exits_delta);
+    TEST_ASSERT_EQUAL_INT(1, state.entries);
+}
+
+void test_blob_crossing_line_bottom_to_top_is_exit() {
+    CVState state;
+    cv_init(state);
+
+    uint8_t bg[CV_PIXELS]; fill_frame(bg, 100);
+    cv_process(state, bg, 50);
+
+    // Walk blob from y=76 toward line; crossing occurs at y=34 (below→above)
+    // Stop at crossing frame and assert its result
+    int setup[] = {76, 62, 48};
+    for (int i = 0; i < 3; i++) {
+        uint8_t f[CV_PIXELS]; make_blob_frame(f, 48, setup[i]);
+        cv_process(state, f, 50);
+    }
+    uint8_t fcross[CV_PIXELS]; make_blob_frame(fcross, 48, 34);
+    CVResult r = cv_process(state, fcross, 50);
+
+    TEST_ASSERT_EQUAL_INT(0, r.entries_delta);
+    TEST_ASSERT_EQUAL_INT(1, r.exits_delta);
+}
+
+void test_no_crossing_same_side_no_count() {
+    CVState state;
+    cv_init(state);
+
+    uint8_t bg[CV_PIXELS]; fill_frame(bg, 100);
+    cv_process(state, bg, 50);
+
+    uint8_t f1[CV_PIXELS]; make_blob_frame(f1, 48, 20); // above line
+    cv_process(state, f1, 50);
+
+    uint8_t f2[CV_PIXELS]; make_blob_frame(f2, 48, 30); // still above line, moved closer
+    CVResult r = cv_process(state, f2, 50);
+
+    TEST_ASSERT_EQUAL_INT(0, r.entries_delta);
+    TEST_ASSERT_EQUAL_INT(0, r.exits_delta);
+}
+
 int main() {
     UNITY_BEGIN();
     RUN_TEST(test_frame_diff_no_change_gives_no_fg);
@@ -89,5 +159,8 @@ int main() {
     RUN_TEST(test_cv_init_clears_state);
     RUN_TEST(test_cv_reset_counts);
     RUN_TEST(test_tracking_spawns_track_for_new_blob);
+    RUN_TEST(test_blob_crossing_line_top_to_bottom_is_entry);
+    RUN_TEST(test_blob_crossing_line_bottom_to_top_is_exit);
+    RUN_TEST(test_no_crossing_same_side_no_count);
     return UNITY_END();
 }