"""
A filter design example
"""
import numpy as np
import cvxpy as cp


if __name__ == '__main__':
    x = cp.Variable(2)
    u = cp.Parameter(3)
    u.value = np.array([-2, -3, 5])
    P = np.array([[1, -.5], [-.5, 2]])
    quadratic = cp.quad_form(x, P)
    objective = cp.Minimize(quadratic - x[0])
    A = np.array([[1, 2], [1, -4], [-1, -1]])
    constraints = [A @ x <= u]
    problem = cp.Problem(objective, constraints)
    problem.solve()
    p00 = problem.value
    l0 = constraints[0].dual_value[0]
    l1 = constraints[0].dual_value[1]
    print('First u:', u.value)
    print('First x:', x.value)
    print('Dual variables:', constraints[0].dual_value)

    # Now solve for perturbed versions
    perturbations = [-0.1, 0, 0.1]
    for d1 in perturbations:
        for d2 in perturbations:
            u.value[0] = -2 + d1
            u.value[1] = -3 + d2
            problem.solve()
            print('d1, d2:', d1, d2)
            print('u:', u.value)
            print('x:', x.value)
            print('pred:', p00 - l0*d1 - l1*d2)  # Not sure
            print(problem.value)
