//------------------------------------------------------------------------------
//  グラフィックイコライザで使う biquad フィルタの係数を計算するクラス
//
//  2022/10/19, Copyright (c) 2022 MIKAMI, Naoki
//------------------------------------------------------------------------------

#include "GrEqParamsCalculator.hpp"

namespace Mikami
{
    // 計算した係数を取得する
    //      gDb : 利得（dB単位）
    BiquadGrEq::Coefs GrEqParams::Get(int band, float f0, float gDb, float qVal)
    {
        const float PI = 3.1415926536f;
        BiquadGrEq::Coefs coefs;

        float gSqrt = sqrtf(powf(10, gDb/20.0f));
        float w0 = 2.0f*PI*f0/FS_;

        if ( (band != 0) && (band != BANDS_-1) )    // ピーキング･フィルタ
        {
            float alpha = sinf(w0)/(2.0f*qVal);
            float beta = 1.0f + alpha/gSqrt;
            coefs.a1 = 2.0f*cosf(w0)/beta;
            coefs.a2 = -(1.0f - alpha/gSqrt)/beta;
            coefs.b0 = (1.0f + alpha*gSqrt)/beta;
            coefs.b1 = -coefs.a1;
            coefs.b2 = (1.0f - alpha*gSqrt)/beta;
        }
        else                                        // シェルビング･フィルタ
        {                                           // Q は固定（1/sqrt(2)）
            w0 = (band == 0) ? w0*1.414214f : w0/1.414214f;
            float alpha = sinf(w0)/1.414214f;
            float g_a = sqrtf(gSqrt);
            float cosW0 = cosf(w0);
            float gSqrtP1 = gSqrt+1.0f;
            float gSqrtM1 = gSqrt-1.0f;
            if (band == 0)      // ローシェルビング･フィルタ
            {                   
                float beta = gSqrtP1 + gSqrtM1*cosW0 + 2.0f*g_a*alpha;
                coefs.a1 = 2.0f*(gSqrtM1 + gSqrtP1*cosW0)/beta;
                coefs.a2 = -(gSqrtP1 + gSqrtM1*cosW0 - 2.0f*g_a*alpha)/beta;
                coefs.b0 = gSqrt*(gSqrtP1 - gSqrtM1*cosW0 + 2.0f*g_a*alpha)/beta;
                coefs.b1 = 2.0f*gSqrt*(gSqrtM1 - gSqrtP1*cosW0)/beta;
                coefs.b2 = gSqrt*(gSqrtP1 - gSqrtM1*cosW0 - 2.0f*g_a*alpha)/beta;
            }
            else                // ハイシェルビング･フィルタ
            {
                float beta = gSqrtP1 - gSqrtM1*cosW0 + 2.0f*g_a*alpha;
                coefs.a1 = -2.0f*(gSqrtM1 - gSqrtP1*cosW0)/beta;
                coefs.a2 = -(gSqrtP1 - gSqrtM1*cosW0 - 2.0f*g_a*alpha)/beta;
                coefs.b0 = gSqrt*(gSqrtP1 + gSqrtM1*cosW0 + 2.0f*g_a*alpha)/beta;
                coefs.b1 = -2.0f*gSqrt*(gSqrtM1 + gSqrtP1*cosW0)/beta;
                coefs.b2 = gSqrt*(gSqrtP1 + gSqrtM1*cosW0 - 2.0f*g_a*alpha)/beta;
            }
        }

        return coefs;
    }
}
