# Phase 5: Proximity Algorithm - Research

**Researched:** 2026-03-22
**Domain:** Embedded sensor algorithm porting (C firmware to C++17 driver)
**Confidence:** HIGH

## Summary

Phase 5 ports the Beyond 2 firmware's proximity detection algorithm (`prox_control.c::prox_update()`) into a standalone C++ class (`ProximityAlgorithm`) that runs inside the HID reader thread. The algorithm is well-defined: calibration offset subtraction, sample validation (100-16383 range), 16-sample circular buffer moving average, and hysteresis-based person detection. All parameters come from the existing `CalibrationData` struct already parsed in Phase 4.

The firmware reference implementation is 65 lines of straightforward C. The C++ port is a faithful translation -- same math, same constants, same edge cases -- wrapped in a class with thread-safe accessors. The main engineering challenge is not algorithmic complexity but correctness: matching the firmware behavior exactly, handling thread safety between the reader thread and the main RunFrame thread, and properly resetting state on USB reconnect.

**Primary recommendation:** Create a `ProximityAlgorithm` class in `src/hid/proximity_algorithm.h/.cpp` that takes calibration parameters at construction/reset, processes one sample at a time via `ProcessSample(uint16_t raw_distance)`, and exposes `GetPersonDetected()` as an atomic bool. HidDevice owns the instance and calls it per HID report in ReaderThreadFunc. DeviceProvider reads person_detected in RunFrame.

<user_constraints>
## User Constraints (from CONTEXT.md)

### Locked Decisions
- Algorithm runs inside the HidDevice reader thread, processing each HID sample immediately as it arrives
- New `ProximityAlgorithm` class in separate file (`src/hid/proximity_algorithm.h/.cpp`) -- clean separation from HID I/O, independently testable
- HidDevice owns a ProximityAlgorithm instance, feeds it samples in the reader thread
- HidDevice exposes `GetPersonDetected()` as atomic bool -- same pattern as `GetProxDistance()`
- DeviceProvider reads `GetPersonDetected()` in RunFrame, compares to `m_bProximity`, calls `SetHmdProximity()` on change
- All SteamVR API calls stay on the main thread (RunFrame); reader thread only computes
- Moving average buffer initialized to all zeros, always divides by 16 -- matches firmware exactly
- `person_detected` defaults to false on driver startup (headset assumed off-head)
- On USB reconnect: reset moving average buffer to zeros and `person_detected` to false (same as fresh startup)
- Calibration is re-read on reconnect (Phase 4 decision), so stale samples are never mixed with new calibration
- Pipe `status` command extended with: `averaged_prox=X detected=true/false eff_thresh=Y samples=N`
- No new pipe commands -- existing `proximity on/off` continues to work for manual override
- Algorithm logs state transitions to SteamVR driver log

### Claude's Discretion
- ProximityAlgorithm class interface design (method signatures, internal data structures)
- Circular buffer implementation details (array + index vs std::array)
- Atomic bool implementation for person_detected (std::atomic<bool> vs std::atomic<int>)
- Exact log message formatting
- Whether averaged_prox is exposed as atomic or via mutex with other algorithm state

### Deferred Ideas (OUT OF SCOPE)
None -- discussion stayed within phase scope
</user_constraints>

<phase_requirements>
## Phase Requirements

| ID | Description | Research Support |
|----|-------------|-----------------|
| PROX-01 | Driver computes proximity distance by subtracting programmed_cal offset from raw prox_distance (clamping to 0 if raw <= cal) | Firmware lines 110-114: exact subtraction and clamping logic documented below |
| PROX-02 | Driver validates raw samples against MIN_PROX_VALUE (100) and MAX_PROX_VALUE (16383) bounds before processing | Firmware line 123: validation gate before moving average insertion |
| PROX-03 | Driver maintains 16-sample moving average of calibrated proximity distance values | Firmware lines 126-136: circular buffer with fixed /16 division |
| PROX-04 | Driver detects person_detected = true when averaged value >= (threshold + user_trim + hysteresis) | Firmware lines 148-149: detection transition |
| PROX-05 | Driver detects person_detected = false when averaged value <= (threshold + user_trim - hysteresis) | Firmware lines 141-142: release transition |
| PROX-06 | Driver clamps effective threshold (threshold + user_trim) to >= 0 to handle negative user_trim values | Firmware lines 95-100: int32 temp_threshold clamped to 0 |
</phase_requirements>

## Standard Stack

### Core
| Library | Version | Purpose | Why Standard |
|---------|---------|---------|--------------|
| C++17 STL | N/A | std::array, std::atomic, std::mutex | Already used by project, no external deps needed |

### Supporting
No new libraries required. The algorithm is pure arithmetic on integers with no external dependencies.

## Architecture Patterns

### Recommended Project Structure
```
src/
  hid/
    proximity_algorithm.h    # NEW: ProximityAlgorithm class declaration
    proximity_algorithm.cpp  # NEW: ProximityAlgorithm implementation
    hid_device.h             # MODIFIED: add ProximityAlgorithm member + GetPersonDetected()
    hid_device.cpp           # MODIFIED: feed samples to algorithm in ReaderThreadFunc
    user_signature.h         # UNCHANGED: CalibrationData struct (algorithm input)
    user_signature.cpp       # UNCHANGED
  driver/
    device_provider.h        # UNCHANGED
    device_provider.cpp      # MODIFIED: RunFrame reads person_detected, status command extended
```

### Pattern 1: Single-Sample Processing (faithful firmware port)

**What:** `ProximityAlgorithm::ProcessSample(uint16_t raw_distance)` mirrors firmware `prox_update()` exactly: subtract cal, validate range, insert into circular buffer, compute average, apply hysteresis.

**When to use:** Called once per HID periodic report in the reader thread.

**Firmware algorithm (lines 91-156 of prox_control.c):**
```cpp
// Step 1: Compute effective threshold (PROX-06)
int32_t temp_threshold = (int32_t)threshold + (int32_t)user_trim;
uint16_t trimmed_threshold = (temp_threshold < 0) ? 0 : (uint16_t)temp_threshold;

// Step 2: Calibration subtraction (PROX-01)
uint16_t calibrated;
if (raw_distance <= programmed_cal)
    calibrated = 0;
else
    calibrated = raw_distance - programmed_cal;

// Step 3: Validate raw sample (PROX-02) -- NOTE: uses raw, not calibrated
if (raw_distance >= MIN_PROX_VALUE && raw_distance <= MAX_PROX_VALUE) {

    // Step 4: Insert calibrated value into circular buffer (PROX-03)
    buffer[write_index] = calibrated;
    write_index = (write_index + 1) % 16;

    // Step 5: Compute moving average (PROX-03)
    uint32_t sum = 0;
    for (int i = 0; i < 16; i++) sum += buffer[i];
    uint32_t averaged = sum / 16;

    // Step 6: Hysteresis comparison (PROX-04, PROX-05)
    if (person_detected) {
        if (averaged <= (trimmed_threshold - hysteresis))
            person_detected = false;
    } else {
        if (averaged >= (trimmed_threshold + hysteresis))
            person_detected = true;
    }
}
```

### Pattern 2: Thread-Safe State Exposure

**What:** ProximityAlgorithm computes in the reader thread; results exposed via atomics for lock-free reads from RunFrame.

**Design:**
```cpp
class ProximityAlgorithm {
public:
    // Configure with calibration data (called from reader thread on connect/reconnect)
    void Reset(const CalibrationData& cal);

    // Process one raw HID sample (called from reader thread)
    void ProcessSample(uint16_t raw_distance);

    // Lock-free reads (called from RunFrame on main thread)
    bool GetPersonDetected() const;

    // Diagnostic snapshot (called from HandlePipeCommand, less frequent)
    struct DiagState {
        uint32_t averaged_prox;
        bool detected;
        uint16_t effective_threshold;
        uint32_t total_samples;
    };
    DiagState GetDiagState() const;

private:
    // Constants
    static constexpr uint16_t kMinProxValue = 100;
    static constexpr uint16_t kMaxProxValue = 16383;
    static constexpr int kAverageLength = 16;

    // Calibration parameters (set on Reset, read only from reader thread)
    uint16_t m_programmedCal = 0;
    uint16_t m_trimmedThreshold = 0;  // pre-computed: clamp(threshold + user_trim, 0)
    uint16_t m_hysteresis = 100;

    // Moving average buffer (written only from reader thread)
    std::array<uint16_t, kAverageLength> m_buffer{};
    int m_writeIndex = 0;

    // Output state
    std::atomic<bool> m_personDetected{false};

    // Diagnostic state (protected by mutex for multi-field consistency)
    mutable std::mutex m_diagMutex;
    uint32_t m_averagedProx = 0;
    uint32_t m_totalSamples = 0;
};
```

### Pattern 3: HidDevice Integration

**What:** HidDevice owns ProximityAlgorithm, resets it on connect/reconnect, feeds it samples.

**Integration points in ReaderThreadFunc:**
```cpp
// After ReadCalibration() on connect/reconnect:
m_proximityAlgorithm.Reset(GetCalibration());

// After parsing prox from '#' report:
uint16_t prox = (buf[4] << 8) | buf[5];
m_lastProxDistance.store(prox, std::memory_order_relaxed);
m_proximityAlgorithm.ProcessSample(prox);  // NEW
```

### Pattern 4: DeviceProvider RunFrame Integration

**What:** RunFrame polls person_detected and calls SetHmdProximity on state change.

```cpp
void DeviceProvider::RunFrame()
{
    PollPipe();

    if (m_pHidDevice && m_pHidDevice->GetConnectionState() == 1)
    {
        bool detected = m_pHidDevice->GetPersonDetected();
        if (detected != m_bProximity)
        {
            SetHmdProximity(detected);
        }
    }
}
```

### Anti-Patterns to Avoid
- **Reinterpreting the firmware algorithm:** The algorithm must be a faithful port. Do not "improve" the math (e.g., using exponential moving average instead of simple moving average). Same subtraction order, same validation on raw (not calibrated), same hysteresis direction.
- **Validating on calibrated value instead of raw:** Firmware line 123 checks `temp_distance` (raw) against MIN/MAX, not the calibrated value. This is subtle but important -- if programmed_cal is large, calibrated values would always be small and might fail validation incorrectly.
- **Computing trimmed_threshold per sample:** The firmware computes it at the top of prox_update() every call. Since calibration params don't change between Reset() calls, pre-compute trimmed_threshold in Reset() for efficiency. This is an acceptable optimization that doesn't change behavior.
- **Forgetting to store calibrated (not raw) in the buffer:** Firmware line 126 stores `proxdata.current_prox_data` (the calibrated value) in the moving average buffer, not `temp_distance` (raw).

## Don't Hand-Roll

| Problem | Don't Build | Use Instead | Why |
|---------|-------------|-------------|-----|
| Circular buffer | Generic ring buffer template | Simple `std::array<uint16_t, 16>` + int index | Only 16 elements, only uint16_t, firmware uses raw array -- keep it simple and matching |
| Thread synchronization | Complex lock-free structures | `std::atomic<bool>` for person_detected, `std::mutex` for diagnostic snapshot | Established project pattern, proven correct |

**Key insight:** This algorithm is intentionally simple. The firmware uses global arrays and plain integers. The C++ port should be similarly straightforward -- just wrapped in a class for encapsulation and testability.

## Common Pitfalls

### Pitfall 1: Wrong Validation Target
**What goes wrong:** Validating the calibrated distance instead of the raw distance against MIN/MAX bounds.
**Why it happens:** It seems logical to validate after calibration subtraction. But firmware validates raw.
**How to avoid:** Line 123 of prox_update() is explicit: `if((temp_distance >= MIN_PROX_VALUE) && (temp_distance <= MAX_PROX_VALUE))` where `temp_distance` is the raw sensor reading.
**Warning signs:** Samples being rejected that shouldn't be, or vice versa.

### Pitfall 2: Integer Underflow on Hysteresis Subtraction
**What goes wrong:** `trimmed_threshold - hysteresis` underflows when threshold is small and hysteresis is large (both uint16_t).
**Why it happens:** Firmware stores trimmed_threshold as uint16_t and hysteresis as uint16_t. Subtracting can wrap around.
**How to avoid:** The firmware doesn't guard against this -- it relies on calibration data being sensible. However, in the C++ port, the comparison `averaged <= (trimmed_threshold - hysteresis)` should be done with care. Since the firmware defaults are threshold=1500, hysteresis=100, underflow is unlikely in practice. But for safety, cast to int32_t before subtraction or document the assumption.
**Warning signs:** person_detected never transitioning to false.

### Pitfall 3: Forgetting Reset on Reconnect
**What goes wrong:** Stale moving average samples from before disconnect mix with new samples after reconnect, causing incorrect detection for up to 8 seconds (16 samples at 500ms).
**Why it happens:** USB reconnect doesn't automatically clear algorithm state.
**How to avoid:** Call `m_proximityAlgorithm.Reset(cal)` immediately after `ReadCalibration()` in the reconnect path. This is already the locked decision.
**Warning signs:** Brief incorrect proximity state after USB replug.

### Pitfall 4: Race Between Reset and ProcessSample
**What goes wrong:** If Reset() and ProcessSample() could be called from different threads, data corruption.
**Why it happens:** Misunderstanding thread ownership.
**How to avoid:** Both Reset() and ProcessSample() are called ONLY from the reader thread. No synchronization needed for the buffer/write_index/calibration members. Only the output (person_detected atomic, diag snapshot mutex) needs cross-thread protection.
**Warning signs:** N/A -- by design both methods are single-threaded on the reader thread.

### Pitfall 5: SetHmdProximity(true) on Startup Conflict
**What goes wrong:** DeviceProvider::Init() currently calls `SetHmdProximity(true)` on startup to register the proximity sensor property. If RunFrame starts reading person_detected=false immediately, it would call `SetHmdProximity(false)` and undo the sensor registration.
**Why it happens:** The current startup call sets proximity=true for sensor registration, but the algorithm starts with person_detected=false.
**How to avoid:** The startup `SetHmdProximity(true)` sets `Prop_ContainsProximitySensor_Bool` which tells SteamVR the HMD HAS a proximity sensor. The RunFrame-driven `SetHmdProximity()` also sets this same property for actual detection. Need to separate "has sensor" (always true) from "person detected" (algorithm-driven). Check if this is already separated or needs splitting. Looking at the current code: `SetHmdProximity(bool on)` sets `Prop_ContainsProximitySensor_Bool` to `on`. This means setting it to `false` would tell SteamVR there's NO proximity sensor, which is wrong. **This needs to be addressed**: RunFrame should NOT call SetHmdProximity(false) to indicate no-person. Instead, it should use a different mechanism or the property semantics need to be reviewed. This is actually a Phase 6 concern (SteamVR integration), but the algorithm phase should be aware that simply toggling the existing property may not be the right approach. For Phase 5, focus on computing person_detected correctly; Phase 6 will wire it to the correct SteamVR API.

## Code Examples

### ProximityAlgorithm::ProcessSample (core algorithm)
```cpp
// Faithful port of prox_control.c prox_update() lines 91-156
void ProximityAlgorithm::ProcessSample(uint16_t raw_distance)
{
    // Calibration subtraction (PROX-01)
    uint16_t calibrated;
    if (raw_distance <= m_programmedCal)
        calibrated = 0;
    else
        calibrated = raw_distance - m_programmedCal;

    // Validate raw sample (PROX-02) -- firmware checks raw, not calibrated
    if (raw_distance < kMinProxValue || raw_distance > kMaxProxValue)
        return;  // reject invalid sample, don't update buffer or detection

    // Insert calibrated value into moving average buffer (PROX-03)
    m_buffer[m_writeIndex] = calibrated;
    m_writeIndex = (m_writeIndex + 1) % kAverageLength;

    // Compute moving average (PROX-03)
    uint32_t sum = 0;
    for (int i = 0; i < kAverageLength; i++)
        sum += m_buffer[i];
    uint32_t averaged = sum / kAverageLength;

    // Hysteresis-based detection (PROX-04, PROX-05)
    bool detected = m_personDetected.load(std::memory_order_relaxed);
    if (detected) {
        // Release: averaged drops below threshold - hysteresis
        if (averaged <= static_cast<uint32_t>(m_trimmedThreshold) - m_hysteresis)
            detected = false;
    } else {
        // Detect: averaged rises above threshold + hysteresis
        if (averaged >= static_cast<uint32_t>(m_trimmedThreshold) + m_hysteresis)
            detected = true;
    }

    if (detected != m_personDetected.load(std::memory_order_relaxed))
    {
        m_personDetected.store(detected, std::memory_order_relaxed);
        // Log state transition (DriverLog is thread-safe)
        DriverLog("Proximity: %s (avg=%u, thresh=%u)\n",
                  detected ? "person detected" : "person removed",
                  averaged, m_trimmedThreshold);
    }

    // Update diagnostic state
    {
        std::lock_guard<std::mutex> lock(m_diagMutex);
        m_averagedProx = averaged;
        m_totalSamples++;
    }
}
```

### ProximityAlgorithm::Reset
```cpp
void ProximityAlgorithm::Reset(const CalibrationData& cal)
{
    m_programmedCal = cal.programmed_cal;
    m_hysteresis = cal.proximity_hysteresis;

    // Compute trimmed threshold with clamping (PROX-06)
    int32_t temp = static_cast<int32_t>(cal.proximity_threshold)
                 + static_cast<int32_t>(cal.user_trim);
    m_trimmedThreshold = (temp < 0) ? 0 : static_cast<uint16_t>(temp);

    // Reset buffer to zeros (matches firmware prox_init)
    m_buffer.fill(0);
    m_writeIndex = 0;

    // Reset detection state
    m_personDetected.store(false, std::memory_order_relaxed);

    {
        std::lock_guard<std::mutex> lock(m_diagMutex);
        m_averagedProx = 0;
        m_totalSamples = 0;
    }

    DriverLog("Proximity: Algorithm reset (cal=%u, thresh=%u, hyst=%u, trim=%d, eff_thresh=%u)\n",
              m_programmedCal, cal.proximity_threshold, m_hysteresis,
              cal.user_trim, m_trimmedThreshold);
}
```

### Extended Status Command
```cpp
// In HandlePipeCommand("status"):
if (m_pHidDevice)
{
    auto diag = m_pHidDevice->GetAlgorithmDiag();
    CalibrationData cal = m_pHidDevice->GetCalibration();
    snprintf(response, sizeof(response),
        "proximity=%s hid=%s prox_raw=%u cal=%u thresh=%u hyst=%u trim=%d "
        "averaged_prox=%u detected=%s eff_thresh=%u samples=%u",
        m_bProximity ? "true" : "false",
        hidStateStr,
        m_pHidDevice->GetProxDistance(),
        cal.programmed_cal,
        cal.proximity_threshold,
        cal.proximity_hysteresis,
        cal.user_trim,
        diag.averaged_prox,
        diag.detected ? "true" : "false",
        diag.effective_threshold,
        diag.total_samples);
}
```

## State of the Art

| Old Approach | Current Approach | When Changed | Impact |
|--------------|------------------|--------------|--------|
| Firmware-only prox detection | PC-side algorithm replication | This project | Enables SteamVR standby/wake without firmware changes |
| Raw threshold comparison | Moving average + hysteresis | Firmware design | Prevents flicker near threshold boundary |

## Open Questions

1. **Underflow safety on hysteresis subtraction**
   - What we know: Firmware uses `trimmed_threshold - hysteresis` as uint16 subtraction. Default values (1500-100=1400) are safe.
   - What's unclear: Whether any production headsets have calibration data where threshold+trim < hysteresis.
   - Recommendation: Add a safety cast to int32_t or document the assumption. Low risk given known defaults.

2. **SetHmdProximity semantics for Phase 6**
   - What we know: Current SetHmdProximity toggles `Prop_ContainsProximitySensor_Bool`. Setting to false means "no sensor" not "person absent".
   - What's unclear: The correct SteamVR property/API for communicating person_detected state changes.
   - Recommendation: Phase 5 computes person_detected correctly. Phase 6 will research the correct SteamVR integration API. For Phase 5, do NOT wire person_detected to SetHmdProximity in RunFrame yet -- just compute and expose it. Let status command show it for validation.

## Validation Architecture

### Test Framework
| Property | Value |
|----------|-------|
| Framework | Manual hardware validation via named pipe CLI |
| Config file | none -- CLI tool already built (beyond_prox_ctl) |
| Quick run command | `beyond_prox_ctl.exe status` |
| Full suite command | Manual test sequence with headset on/off |

### Phase Requirements to Test Map
| Req ID | Behavior | Test Type | Automated Command | File Exists? |
|--------|----------|-----------|-------------------|-------------|
| PROX-01 | Cal subtraction (raw - programmed_cal, clamp 0) | smoke | `beyond_prox_ctl.exe status` -- compare prox_raw vs averaged_prox with known cal | N/A (manual) |
| PROX-02 | Sample validation (100-16383 range) | smoke | Place/remove headset, check `samples` counter increments only for valid reads | N/A (manual) |
| PROX-03 | 16-sample moving average | smoke | `beyond_prox_ctl.exe status` -- watch `averaged_prox` converge over 16 samples | N/A (manual) |
| PROX-04 | person_detected=true when avg >= thresh+hyst | smoke | Place headset on head, verify `detected=true` in status after ~8s | N/A (manual) |
| PROX-05 | person_detected=false when avg <= thresh-hyst | smoke | Remove headset from head, verify `detected=false` in status after ~8s | N/A (manual) |
| PROX-06 | Effective threshold clamped >= 0 | code-review | Verify clamping logic in Reset() matches firmware lines 95-100 | N/A |

### Sampling Rate
- **Per task commit:** `beyond_prox_ctl.exe status` to verify algorithm state fields present
- **Per wave merge:** Full on/off head test cycle with status monitoring
- **Phase gate:** person_detected transitions correctly on real hardware

### Wave 0 Gaps
- [ ] New source files `proximity_algorithm.h/.cpp` must be added to CMakeLists.txt
- [ ] HidDevice needs `GetPersonDetected()` and `GetAlgorithmDiag()` public methods
- [ ] Status response buffer may need to be enlarged (currently 256 bytes, extended response is ~150 chars -- fits)

## Sources

### Primary (HIGH confidence)
- `code_samples/beyond_firmware/src/Devices/prox_control.c` -- exact firmware algorithm, line-by-line reference
- `code_samples/beyond_firmware/src/Devices/prox_control.h` -- constants (MIN_PROX_VALUE=100, MAX_PROX_VALUE=16383), Proximity_T struct
- `src/hid/hid_device.h/.cpp` -- existing reader thread, atomic patterns, CalibrationData integration
- `src/hid/user_signature.h` -- CalibrationData struct with all algorithm input parameters
- `src/driver/device_provider.cpp` -- existing RunFrame, HandlePipeCommand, SetHmdProximity

### Secondary (MEDIUM confidence)
- None needed -- all information comes from project source code

### Tertiary (LOW confidence)
- None

## Metadata

**Confidence breakdown:**
- Standard stack: HIGH -- no external deps, pure C++ arithmetic
- Architecture: HIGH -- patterns established in Phase 4, decisions locked in CONTEXT.md
- Pitfalls: HIGH -- firmware reference is unambiguous, edge cases identified from code review

**Research date:** 2026-03-22
**Valid until:** Indefinite -- firmware algorithm is stable, project patterns established