#!/usr/bin/env python3
import numpy as np
from lasprs.filter import Biquad, SeriesBiquad, BiquadBank


def test_biquad1():
    """
    Test if manual unit biquad matches response for manual input biquad.
    """
    input_ = np.array([1.0, 0, 0, 0, 0, 0, 0])

    b = Biquad(np.array([1., 0, 0, 1, 0, 0]))
    b2 = Biquad.unit()
    out1 = b.filter(input_)
    out2 = b2.filter(input_)

    expected_output = input_
    assert(np.linalg.norm(out1 - out2) == 0.)
    assert(np.linalg.norm(out1 - expected_output) == 0.)

def test_seriesbiquad():
    """
    Test if manual unit biquad matches response for manual input biquad.
    """
    input_ = np.array([1.0, 0, 0, 0, 0, 0, 0])

    f1 = [1., 0, 0, 1, 0, 0]
    f2 = [1., 0, 0, 1, 0, 0]
    f = f1+f2

    # Two biquads in series
    b = SeriesBiquad(np.array(f))

    # Single one
    b2 = SeriesBiquad.unit()

    out1 = b.filter(input_)
    out2 = b2.filter(input_)

    expected_output = input_
    assert(np.linalg.norm(out1 - out2) == 0.)
    assert(np.linalg.norm(out1 - expected_output) == 0.)

def test_biquadbank():
    """
    See if two filters with half gain produce the output=input
    """
    input_ = np.array([1.0, 0, 0, 0, 0, 0, 0])
    f1 = [1., 0, 0, 1, 0, 0]
    f2 = [1., 0, 0, 1, 0, 0]
    filters = np.array([f1, f2]).T
    bank = BiquadBank(filters)
    bank.set_gains([0.5, 0.5])
    filtered = bank.filter(input_)
    assert(bank.len() == 2)
    assert(np.linalg.norm(input_ - filtered) == 0)


if __name__ == '__main__':
    test_biquad1()
    test_seriesbiquad()
    test_biquadbank()