% Initialize parameters
init;

N = 100;
dt = 0.25;

% Continuous System
Ac = [
    0 1         0         0;
    0 0      -K_2         0;
    0 0         0         1;
    0 0 -K_1*K_pp -K_1*K_pd;
];
Bc = [0 0 0 K_1*K_pp]';

nx = size(Ac, 1);
nu = size(Bc, 2);

Ad = eye(nx) + dt * Ac;
Bd = Bc;

Aieq = [diag_repeat([0 0 1 0], N) zeros(N); diag_repeat([0 0 -1 0], N) zeros(N)];
bieq = ones(2*N, 1) * 60*pi / 360;

x0 = [pi, 0, 0, 0]';

l_f = 0;
q = 12;

% Cost function
G1 = diag([2, 0, 0, 0]);
G2 = 2*q;
G = gen_q(G1, G2, N, N);

c = [repmat([2*l_f 0 0 0], 1, N) zeros(1, N)];

% Constraints
Aeq = gen_aeq(Ad, Bd, N, nx, nu);
Beq = [(Ad*x0)' zeros(1, 4*(N-1))]';

% Solve!
z = quadprog(G, c, Aieq, bieq, Aeq, Beq);

x = z(1:4*N); 
u = z(4*N+1:end);

lambda = x(1:4:end);
r = x(2:4:end);
p = x(3:4:end);
pdot = x(4:4:end);
hold on;
plot(lambda, 'DisplayName', 'lambda');
plot(u, 'DisplayName', 'u');
%plot(r, 'DisplayName', 'r');
plot(p, 'DisplayName', 'p');
plot(pdot, 'DisplayName', 'pdot');
legend;
hold off;