#include "proximity_algorithm.h"
#include "user_signature.h"
#include "../driver/driverlog.h"

ProximityAlgorithm::ProximityAlgorithm(int avgLength)
    : m_averageLength(avgLength)
    , m_buffer(avgLength, 0)
{
}

void ProximityAlgorithm::Reset(const CalibrationData& cal)
{
    m_hysteresis = cal.proximity_hysteresis;

    // Compute trimmed threshold (PROX-06): clamp to >= 0 for negative user_trim
    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);

    std::fill(m_buffer.begin(), m_buffer.end(), 0);
    m_writeIndex = 0;
    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",
              cal.programmed_cal, cal.proximity_threshold,
              cal.proximity_hysteresis, cal.user_trim, m_trimmedThreshold);
}

void ProximityAlgorithm::ProcessSample(uint16_t hid_distance)
{
    // HID value is already calibration-subtracted by firmware (prox_control.c:110-114).
    // Firmware also rejects invalid sensor readings before sending over HID,
    // so no range check needed here.

    // Insert directly into circular buffer (PROX-03)
    m_buffer[m_writeIndex] = hid_distance;
    m_writeIndex = (m_writeIndex + 1) % m_averageLength;

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

    // Step 5: Hysteresis detection (PROX-04, PROX-05)
    bool currentDetected = m_personDetected.load(std::memory_order_relaxed);
    bool newDetected = currentDetected;

    if (currentDetected)
    {
        // Person currently detected -- check if removed
        if (m_trimmedThreshold >= m_hysteresis &&
            averaged <= static_cast<uint32_t>(m_trimmedThreshold - m_hysteresis))
            newDetected = false;
    }
    else
    {
        // Person not detected -- check if placed on
        if (averaged >= static_cast<uint32_t>(m_trimmedThreshold) + m_hysteresis)
            newDetected = true;
    }

    // Step 6: Update state on transition, log change
    if (newDetected != currentDetected)
    {
        m_personDetected.store(newDetected, std::memory_order_relaxed);
        DriverLog("Proximity: person_detected changed to %s (avg=%u, thresh=%u)\n",
                  newDetected ? "true" : "false",
                  averaged, m_trimmedThreshold);
    }

    // Step 7: Update diagnostics
    {
        std::lock_guard<std::mutex> lock(m_diagMutex);
        m_averagedProx = averaged;
        m_totalSamples++;
    }
}

bool ProximityAlgorithm::GetPersonDetected() const
{
    return m_personDetected.load(std::memory_order_relaxed);
}

ProximityAlgorithm::DiagState ProximityAlgorithm::GetDiagState() const
{
    std::lock_guard<std::mutex> lock(m_diagMutex);
    return DiagState{
        m_averagedProx,
        m_personDetected.load(std::memory_order_relaxed),
        m_trimmedThreshold,
        m_totalSamples
    };
}