#include "src/control/lpf.h"
#include "src/math/Utilities.h"

using namespace math;

namespace control {

incrementalLPF::incrementalLPF()
: m_filtered(0)
{
}

float incrementalLPF::filter(float latestValue)
{
    m_filtered = m_filtered*0.95f + latestValue*0.05f;
    return m_filtered;
}

pt1FilterApply4::pt1FilterApply4(float freqCut)
: m_freqCut(freqCut)
, m_RC(1.0f / (2.0f * (float)PI * m_freqCut))
, m_filtered(0.0f)
{
}

float pt1FilterApply4::filter(float input, float dT)
{
    if (dT != 0.0f && m_RC + dT != 0.0f)
    {
        float gain(dT / (m_RC + dT));

        m_filtered += gain * (input - m_filtered);
    }

	return m_filtered;
}


} // namespace control