"""Test for complementary filter class
"""
import control
import numpy as np

import kontrol
import kontrol.core.controlutils
import kontrol.frequency_series.noise_models


def test_complementary_filter():
    omega = np.logspace(-2,2,1000)
    f = omega/2/np.pi
    tf_noise1 = control.tf([1/10, 1], [1/0.1, 1]) *10
    tf_noise2 = control.tf([1/0.1, 1], [1/10, 1])

    comp = kontrol.ComplementaryFilter(noise1=tf_noise1, noise2=tf_noise2)
    comp.hinfsynthesis()
    comp.f = f
    noise_super_correct = np.array(
        [
            4.11535263, 4.09580053, 4.07565869, 4.05494934, 4.03369502,
            4.01191847, 3.98964265, 3.96689057, 3.94368531, 3.92004993,
            3.89600741, 3.87158059, 3.84679215, 3.82166452, 3.79621988,
            3.77048008, 3.74446663, 3.71820064, 3.69170283, 3.66499344,
            3.63809228, 3.61101863, 3.5837913 , 3.55642855, 3.5289481 ,
            3.50136712, 3.47370225, 3.44596954, 3.41818448, 3.39036199,
            3.36251644, 3.3346616 , 3.30681071, 3.27897643, 3.2511709 ,
            3.22340569, 3.19569184, 3.16803987, 3.14045979, 3.11296111,
            3.08555284, 3.05824352, 3.0310412 , 3.00395352, 2.97698765,
            2.95015034, 2.92344794, 2.89688638, 2.87047124, 2.8442077 ,
            2.81810059, 2.7921544 , 2.7663733 , 2.74076113, 2.71532142,
            2.69005745, 2.66497216, 2.64006828, 2.61534826, 2.59081431,
            2.5664684 , 2.5423123 , 2.51834757, 2.49457555, 2.4709974 ,
            2.44761412, 2.42442651, 2.40143522, 2.37864077, 2.35604349,
            2.33364362, 2.31144125, 2.28943633, 2.26762874, 2.24601821,
            2.2246044 , 2.20338686, 2.18236505, 2.16153835, 2.14090606,
            2.12046742, 2.10022159, 2.08016766, 2.06030468, 2.04063164,
            2.02114746, 2.00185104, 1.98274122, 1.96381681, 1.94507658,
            1.92651928, 1.90814359, 1.88994821, 1.87193179, 1.85409296,
            1.83643033, 1.81894249, 1.80162804, 1.78448552, 1.76751351,
            1.75071052, 1.73407512, 1.71760581, 1.70130112, 1.68515958,
            1.6691797 , 1.65335998, 1.63769896, 1.62219513, 1.60684702,
            1.59165314, 1.57661202, 1.56172219, 1.54698217, 1.53239051,
            1.51794574, 1.50364642, 1.4894911 , 1.47547836, 1.46160676,
            1.44787489, 1.43428134, 1.42082471, 1.40750363, 1.3943167 ,
            1.38126257, 1.36833988, 1.35554728, 1.34288344, 1.33034704,
            1.31793677, 1.30565133, 1.29348943, 1.28144981, 1.26953119,
            1.25773233, 1.24605199, 1.23448895, 1.22304198, 1.2117099 ,
            1.20049151, 1.18938563, 1.1783911 , 1.16750677, 1.1567315 ,
            1.14606416, 1.13550363, 1.12504882, 1.11469863, 1.10445198,
            1.09430781, 1.08426505, 1.07432268, 1.06447964, 1.05473493,
            1.04508754, 1.03553646, 1.02608071, 1.01671932, 1.00745132,
            0.99827576, 0.9891917 , 0.9801982 , 0.97129435, 0.96247924,
            0.95375197, 0.94511164, 0.93655738, 0.92808833, 0.91970362,
            0.9114024 , 0.90318384, 0.89504711, 0.88699139, 0.87901586,
            0.87111973, 0.86330221, 0.85556252, 0.84789988, 0.84031353,
            0.83280271, 0.82536668, 0.8180047 , 0.81071604, 0.80349998,
            0.7963558 , 0.78928282, 0.78228031, 0.77534761, 0.76848403,
            0.7616889 , 0.75496154, 0.74830131, 0.74170756, 0.73517964,
            0.72871693, 0.72231878, 0.71598458, 0.70971372, 0.70350559])
    assert np.allclose(comp.noise_super[400:600], noise_super_correct, rtol=1e-2)
    # Let's not check the result because of numerical error
    # tf_correct = control.tf([1, 2.08392466e+04, 1.58200377e+04, 2.53883414e+03, 1.16522184e+02], [1, 2.08679038e+04, 1.64005175e+04, 5.55127674e+03, 8.50146586e+02])
    # assert kontrol.core.controlutils.check_tf_equal(
    #     tf_correct, comp[0, 0], allclose_kwargs={"rtol":1e-4})

"""Test for complementary filter class
"""
import control
import numpy as np

import kontrol
import kontrol.core.controlutils
import kontrol.frequency_series.noise_models


def test_complementary_filter():
    omega = np.logspace(-2,2,1000)
    f = omega/2/np.pi
    tf_noise1 = control.tf([1/10, 1], [1/0.1, 1]) *10
    tf_noise2 = control.tf([1/0.1, 1], [1/10, 1])

    comp = kontrol.ComplementaryFilter(noise1=tf_noise1, noise2=tf_noise2)
    comp.hinfsynthesis()
    comp.f = f
    noise_super_correct = np.array(
        [
            4.11535263, 4.09580053, 4.07565869, 4.05494934, 4.03369502,
            4.01191847, 3.98964265, 3.96689057, 3.94368531, 3.92004993,
            3.89600741, 3.87158059, 3.84679215, 3.82166452, 3.79621988,
            3.77048008, 3.74446663, 3.71820064, 3.69170283, 3.66499344,
            3.63809228, 3.61101863, 3.5837913 , 3.55642855, 3.5289481 ,
            3.50136712, 3.47370225, 3.44596954, 3.41818448, 3.39036199,
            3.36251644, 3.3346616 , 3.30681071, 3.27897643, 3.2511709 ,
            3.22340569, 3.19569184, 3.16803987, 3.14045979, 3.11296111,
            3.08555284, 3.05824352, 3.0310412 , 3.00395352, 2.97698765,
            2.95015034, 2.92344794, 2.89688638, 2.87047124, 2.8442077 ,
            2.81810059, 2.7921544 , 2.7663733 , 2.74076113, 2.71532142,
            2.69005745, 2.66497216, 2.64006828, 2.61534826, 2.59081431,
            2.5664684 , 2.5423123 , 2.51834757, 2.49457555, 2.4709974 ,
            2.44761412, 2.42442651, 2.40143522, 2.37864077, 2.35604349,
            2.33364362, 2.31144125, 2.28943633, 2.26762874, 2.24601821,
            2.2246044 , 2.20338686, 2.18236505, 2.16153835, 2.14090606,
            2.12046742, 2.10022159, 2.08016766, 2.06030468, 2.04063164,
            2.02114746, 2.00185104, 1.98274122, 1.96381681, 1.94507658,
            1.92651928, 1.90814359, 1.88994821, 1.87193179, 1.85409296,
            1.83643033, 1.81894249, 1.80162804, 1.78448552, 1.76751351,
            1.75071052, 1.73407512, 1.71760581, 1.70130112, 1.68515958,
            1.6691797 , 1.65335998, 1.63769896, 1.62219513, 1.60684702,
            1.59165314, 1.57661202, 1.56172219, 1.54698217, 1.53239051,
            1.51794574, 1.50364642, 1.4894911 , 1.47547836, 1.46160676,
            1.44787489, 1.43428134, 1.42082471, 1.40750363, 1.3943167 ,
            1.38126257, 1.36833988, 1.35554728, 1.34288344, 1.33034704,
            1.31793677, 1.30565133, 1.29348943, 1.28144981, 1.26953119,
            1.25773233, 1.24605199, 1.23448895, 1.22304198, 1.2117099 ,
            1.20049151, 1.18938563, 1.1783911 , 1.16750677, 1.1567315 ,
            1.14606416, 1.13550363, 1.12504882, 1.11469863, 1.10445198,
            1.09430781, 1.08426505, 1.07432268, 1.06447964, 1.05473493,
            1.04508754, 1.03553646, 1.02608071, 1.01671932, 1.00745132,
            0.99827576, 0.9891917 , 0.9801982 , 0.97129435, 0.96247924,
            0.95375197, 0.94511164, 0.93655738, 0.92808833, 0.91970362,
            0.9114024 , 0.90318384, 0.89504711, 0.88699139, 0.87901586,
            0.87111973, 0.86330221, 0.85556252, 0.84789988, 0.84031353,
            0.83280271, 0.82536668, 0.8180047 , 0.81071604, 0.80349998,
            0.7963558 , 0.78928282, 0.78228031, 0.77534761, 0.76848403,
            0.7616889 , 0.75496154, 0.74830131, 0.74170756, 0.73517964,
            0.72871693, 0.72231878, 0.71598458, 0.70971372, 0.70350559])
    assert np.allclose(comp.noise_super[400:600], noise_super_correct, rtol=1e-2)
    # Let's not check the result because of numerical error
    # tf_correct = control.tf([1, 2.08392466e+04, 1.58200377e+04, 2.53883414e+03, 1.16522184e+02], [1, 2.08679038e+04, 1.64005175e+04, 5.55127674e+03, 8.50146586e+02])
    # assert kontrol.core.controlutils.check_tf_equal(
    #     tf_correct, comp[0, 0], allclose_kwargs={"rtol":1e-4})

Test for complementary filter class # Let's not check the result because of numerical error # tf_correct = control.tf([1, 2.08392466e+04, 1.58200377e+04, 2.53883414e+03, 1.16522184e+02], [1, 2.08679038e+04, 1.64005175e+04, 5.55127674e+03, 8.50146586e+02]) # assert kontrol.core.controlutils.check_tf_equal( #     tf_correct, comp[0, 0], allclose_kwargs={"rtol":1e-4})