//--------------------------------------------------------------
// 縦続形 IIR フィルタの構成要素として使う 2 次の IIR フィルタ
//      b0 は 1 と仮定している
//
//  2022/10/17, 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; };

    // デフォルト･コンストラクタ
    //      係数は構造体 Ceofs で与える
    Biquad(const Coefs ck = (Coefs){0, 0, 0, 0})
        : a1_(ck.a1), a2_(ck.a2), b1_(ck.b1), b2_(ck.b2),
          un1_(0), un2_(0) {}       

    // 係数を個別に与えるコンストラクタ
    Biquad(float a1, float a2, float b1, float b2)
        : a1_(a1), a2_(a2), b1_(b1), b2_(b2), un1_(0), un2_(0) {}

    virtual ~Biquad() {}

    // 2 次のフィルタを実行する
    float Execute(float xn)
    {
        float un = xn + a1_*un1_ + a2_*un2_;
        float yn = un + b1_*un1_ + b2_*un2_;
    
        un2_ = un1_;
        un1_ = un;
    
        return yn;
    }

    // 係数を設定する
    void SetCoefs(const Coefs ck)
    {
        a1_ = ck.a1;
        a2_ = ck.a2;
        b1_ = ck.b1;
        b2_ = ck.b2;
    }

    // 内部変数（遅延器）のクリア
    void Clear() { un1_ = un2_ = 0; }

private:
    float a1_, a2_, b1_, b2_;   // フィルタの係数
    float un1_, un2_;           // 遅延器

    // コピー･コンストラクタ禁止
    Biquad(const Biquad&);
};
#endif  // IIR_BIQUAD_HPP
