#パターン1: BiQuadモデル可変ローパスフィルタ
struct BiQuadLPF
    a::Vector
    b::Vector
    uz::Vector
    yz::Vector

    function BiQuadLPF()
        a = [1.0, 0.0, 0.0]
        b = [0.0, 0.0, 0.0]
        uz = [0.0, 0.0]
        yz = copy(uz)
        new(a, b, uz, yz)
    end
end

function setCoefficients(sys::BiQuadLPF, fc, Q, fs)
    ωc = 2π*fc/fs
    α = sin(ωc)/2Q
    a1 = 1 + α
    a2 = -2*cos(ωc)/a1
    a3 = (1 - α)/a1
    b1 = (1 - cos(ωc))/2.0/ a1
    b2 = b1*2
    b3 = b1
    sys.a[2] = a2
    sys.a[3] = a3
    sys.b[1] = b1
    sys.b[2] = b2
    sys.b[3] = b3
end

function process(sys::BiQuadLPF, u)
    y = sys.b[1]*u
    for i in [1, 2]
        y += sys.uz[i] * sys.b[i+1] - sys.yz[i] * sys.a[i+1]
    end
    
    sys.uz[2] = sys.uz[1]
    sys.uz[1] = u
    sys.yz[2] = sys.yz[1]
    sys.yz[1] = y
    
    return y
end

#mは，カットオフ周波数を時間変化させるモジュレーション信号
function process!(sys::BiQuadLPF, u::Vector, m::Vector, f0, Q, fs)
    @assert length(u) == length(m)
    
    for i in 1:length(u)
        fc = f0 + m[i]
        setCoefficients(sys, fc, Q, fs)
        u[i] = process(sys, u[i])
    end
end

function process(sys::BiQuadLPF, u::Vector, m::Vector, f0, Q, fs)
    uc = copy(u)
    process!(sys, uc, m, f0, Q, fs)
    
    return uc
end

#パターン2: StateVariableモデル可変ローパスフィルタ
struct StateVariableLPF
    s::Vector

    StateVariableLPF() = new([0.0, 0.0])
end

function process(sys::StateVariableLPF, u, g, Q)
    den = 1 + g/Q + g^2
    G1 = g/den
    S1 = (sys.s[1] - g*sys.s[2])/den
    G2 = g*G1
    S2 = g*S1 + sys.s[2]
    
    ξ = u - 1.0/Q * (G1*u + S1) - (G2*u + S2)
    v = g*ξ
    y1 = v + sys.s[1]
    sys.s[1] = v + y1
    
    v = g*y1
    y2 = v + sys.s[2]
    sys.s[2] = v + y2
    
    return y2
end

function process!(sys::StateVariableLPF, u::Vector, m::Vector, f0, Q, fs)
    @assert length(u) == length(m)
    
    for i in 1:length(u)
        fc = f0 + m[i]
        g = π*fc/fs
        u[i] = process(sys, u[i], g, Q)
    end
end

function process(sys::StateVariableLPF, u::Vector, m::Vector, f0, Q, fs)
    uc = copy(u)
    process!(sys, uc, m, f0, Q, fs)
    
    return uc
end

#オーディオファイルIOパッケージ
using LibSndFile

#サンプルオーディオデータ
audio = load("./Resource/SynthSample.wav")

#ワウエフェクトのパラメータ設定
fs = audio.samplerate
f0 = 1000.0
Q = 3.0

#モジュレーション信号の生成
fm = 7.5
A = 600.0
n = 0:length(audio.data[:, 1])-1
m = A*sin.(2π*fm / fs * n)

#BiQuadフィルタによるワウ
bqf = [BiQuadLPF(), BiQuadLPF()]
faudio = convert(Array{Float64}, audio.data)
@time left = process(bqf[1], faudio[:, 1], m, f0, Q, fs)
@time right = process(bqf[2], faudio[:, 2], m, f0, Q, fs)
SampledSignals.SampleBuf([left right], fs)

#StateVariableフィルタによるワウ
svf = [StateVariableLPF(), StateVariableLPF()]
faudio = convert(Array{Float64}, audio.data)
@time left = process(svf[1], faudio[:, 1], m, f0, Q, fs)
@time right = process(svf[2], faudio[:, 2], m, f0, Q, fs)
SampledSignals.SampleBuf([left right], fs)