import controller


def test_interp():
    table = (
        (5, 10),
        (6, 15),
        (8, 20),
    )
    # Below gives minimum
    assert controller.interp(4, table) == 10
    # Above gives maximum
    assert controller.interp(8.1, table) == 20
    # Between is interpolated
    assert controller.interp(5.5, table) == 12.5
    assert controller.interp(7, table) == 17.5


def test_pid_kp():
    p = controller.PID(kp=5)
    # u = Kp * err
    assert p.step(0, 10) == -50


def test_pid_ki():
    p = controller.PID(ki=4, windup_limit=100)
    p.step(0, 10)
    # u = Ki * err over 0.5s
    assert p.step(0.5, 10) == -20
    assert p.step(0.7, 20) == -40 * 0.5 + -80 * 0.2
    # Tops out at Ki * windup_limit
    assert p.step(10, 10) == -400
    assert p.step(30, -10) == 400
    # Disabling clears the integral
    p.enabled = False
    assert p.step(31, -10) == 0


def test_pid_ff():
    ff = (
        (5, 10),
        (10, 20),
        (15, 40),
    )
    p = controller.PID(ff=ff)
    # u = interp(ff, pv)
    assert p.step(1, 12) == 28


def test_deadband():
    p = controller.PID(kp=1, deadband=(0.5, 1.0))
    # No deadband below limit
    assert p.step(0, 0.4) == -0.4
    assert p.step(0, -0.4) == 0.4
    # Deadband above limit
    assert p.step(0, 0.6) == -1.6
    assert p.step(0, -0.6) == 1.6


def test_lpf():
    f = controller.LPF()

    for _ in range(20):
        print(round(f.step(10), 3))


test_lpf()