//--------------------------------------------------------------
//  縦続形 IIR フィルタの構成要素として使う 2 次の IIR フィルタ
//      b0 は 1 と仮定している
//
//  2022/12/22, Copyright (c) 2022 MIKAMI, Naoki
//--------------------------------------------------------------

#include "mbed.h"

#ifndef IIR_BIQUAD_HPP
#define IIR_BIQUAD_HPP

class Biquad
{
public:
    // フィルタの係数に対応する構造体
    struct Coefs { float a1, a2, b1, b2; };

    // コンストラクタ
    explicit Biquad(const Coefs ck = (Coefs){0, 0, 0, 0})
        : a1_(ck.a1), a2_(ck.a2), b1_(ck.b1), b2_(ck.b2),
          vn1_(0), vn2_(0) {}       

    // 2 次のフィルタを実行する
    float Execute(float xn)
    {
        float vn = xn + a1_*vn1_ + a2_*vn2_;
        float yn = vn + b1_*vn1_ + b2_*vn2_;

        vn2_ = vn1_;
        vn1_ = vn;

        return yn;
    }

private:
    float a1_, a2_, b1_, b2_;   // フィルタの係数
    float vn1_, vn2_;           // 単位遅延素子
};
#endif  // IIR_BIQUAD_HPP
