//------------------------------------------------------------------------
//  双一次ｚ変換によるバタワースフィルタの設計
//      LPF, HPF のみ
//
//  2023/01/08, Copyright (c) 2023 MIKAMI, Naoki
//------------------------------------------------------------------------

#include "BilinearDesignLH.hpp"

namespace Mikami
{
    // 設計の実行
    //      入力
    //          fc: 遮断周波数
    //          pb: 通過域 (LPF or HPF)
    //      出力
    //          c : 縦続形の Biquad 構成の係数
    //          g : 利得定数
    void BilinearDesign::Execute(float fc, Type pb, Coefs c[], float& g)
    {
        Butterworth();
        Bilinear(fc);
        ToCascade(pb);
        GetGain(pb);
        GetCoefs(c, g);
    }

    // バタワース特性の極を取得
    void BilinearDesign::Butterworth()
    {
        float pi_2order = PI_/(2.0f*ORDER_);
        for (int k=0; k<ORDER_/2; k++)
        {
            float theta = (2.0f*k + 1.0f)*pi_2order;
            sP_[k] = Complex(-cosf(theta), sinf(theta));
        }
    }

    // 双一次ｚ変換
    //      fc: 遮断周波数
    void BilinearDesign::Bilinear(float fc)
    {
        float wc = tanf(fc*PI_FS_);
        for (int k=0; k<ORDER_/2; k++)
            zP_[k] = (1.0f + wc*sP_[k])/(1.0f - wc*sP_[k]);
    }

    // 縦続形の Biquad 構成の係数に変換
    void BilinearDesign::ToCascade(Type pb)
    {
        for (int k=0; k<ORDER_/2; k++)
        {
            ck_[k].a1 = 2.0f*real(zP_[k]);          // a1k
            ck_[k].a2 = -norm(zP_[k]);              // a2k
            ck_[k].b1 = (pb == LPF) ? 2.0f : -2.0f; // b1k
            ck_[k].b2 = 1.0f;                       // b2k
        }
    }

    // 利得定数の計算
    void BilinearDesign::GetGain(Type pb)
    {
        float u = (pb == LPF) ? 1.0f : -1.0f;   // u: z の逆数
        gain_ = 1.0f;
        for (int k=0; k<ORDER_/2; k++)
            gain_ = gain_*(1.0f - (ck_[k].a1 + ck_[k].a2*u)*u)/
                          (1.0f + (ck_[k].b1 + ck_[k].b2*u)*u);
    }

    // 係数の取得
    void BilinearDesign::GetCoefs(Coefs c[], float& gain) const
    {
        for (int k=0; k<ORDER_/2; k++) c[k] = ck_[k];
        gain = gain_;
    }
}
