% INIT_MPC.M %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Initialize the single layer MPC made from scratch

init;

clear control.L control.Rl control.S1 control.S2

global mpc

load('mat-files/predmod_mpc1');

mpc.on = 1;

mpc.ts = mpc_ts;
mpc.A = A;
mpc.B = B;
mpc.Bd = Bd;
mpc.C = C;

clear mpc_ts A B Bd C

mpc.N = 50;
mpc.R_bar = matrixstacker(1 * eye(4),mpc.N,0);
mpc.Q_bar = matrixstacker(1000000 * eye(4),mpc.N,0);

poles = 0.1 * linspace(-0.9,0.9,27);
mpc.L = place([mpc.A mpc.Bd; zeros(size(mpc.Bd,2),size(mpc.A,2)) eye(size(mpc.Bd,2))]',[mpc.C zeros(size(mpc.C,1),size(mpc.Bd,2))]',poles)';
clear poles

if (mpc.on == 1)
    disp('### MPC ON ###');
elseif (mpc.on == 2)
    disp('### LQR ON ###');
else
    disp('### MPC / LQR OFF ###');
end
