#include "PressureMonitor.h"
#include <cmath>

PressureMonitor::PressureMonitor()
    : m_tickCounter(0)
    , m_sampleCount(0)
    , m_bufIdx(0)
    , m_totalTickCount(0)
    , m_timeout(false)
{
    for (int i = 0; i < BUF_SIZE; i++) {
        m_buf[i] = 0.0f;
    }
}

bool PressureMonitor::tick(float realtimePressure, float calibratedPressure)
{
    m_totalTickCount++;

    if (m_totalTickCount >= TIMEOUT_TICKS) {
        m_timeout = true;
        return true;
    }

    if (m_tickCounter != 0) {
        m_tickCounter = (m_tickCounter + 1) % SAMPLE_INTERVAL_TICKS;
        return false;
    }
    m_tickCounter = (m_tickCounter + 1) % SAMPLE_INTERVAL_TICKS;

    if (calibratedPressure <= 0.0f) {
        return false;
    }

    m_buf[m_bufIdx] = realtimePressure;
    m_bufIdx = (m_bufIdx + 1) % BUF_SIZE;
    m_sampleCount++;

    if (m_sampleCount < BUF_SIZE) {
        return false;
    }

    float rangeMin = calibratedPressure - 10.0f;
    float rangeMax = calibratedPressure + 10.0f;

    if (!checkFirst4InRange(rangeMin, rangeMax)) {
        return false;
    }

    float avg = computeTrimmedMean();
    return (avg >= rangeMin && avg <= rangeMax);
}

void PressureMonitor::reset()
{
    m_tickCounter = 0;
    m_sampleCount = 0;
    m_bufIdx = 0;
    m_totalTickCount = 0;
    m_timeout = false;
    for (int i = 0; i < BUF_SIZE; i++) {
        m_buf[i] = 0.0f;
    }
}

bool PressureMonitor::isCurrentlyInRange() const
{
    return (m_sampleCount >= BUF_SIZE);
}

int PressureMonitor::stableTickCount() const
{
    return m_sampleCount;
}

bool PressureMonitor::wasTimeout() const
{
    return m_timeout;
}

const float* PressureMonitor::buffer() const
{
    return m_buf;
}

bool PressureMonitor::checkFirst4InRange(float rangeMin, float rangeMax) const
{
    for (int i = 0; i < 4; i++) {
        float val = m_buf[(m_bufIdx + i) % BUF_SIZE];
        if (val < rangeMin || val > rangeMax) {
            return false;
        }
    }
    return true;
}

float PressureMonitor::computeTrimmedMean() const
{
    float minVal = m_buf[0];
    float maxVal = m_buf[0];
    float sum = 0.0f;

    for (int i = 0; i < BUF_SIZE; i++) {
        float val = m_buf[i];
        sum += val;
        if (val < minVal) minVal = val;
        if (val > maxVal) maxVal = val;
    }

    return (sum - minVal - maxVal) / (float)(BUF_SIZE - 2);
}