%% Question 7
%==========================================================================
% Setup MPC controller
%==========================================================================

%sp=zeros(tf,3);         % setpoint trajectory

N=10;                   % prediction horizon
M=3;                    % control horizon

Q = diag([1 0.001 1]);  % state penalty
Pf = Q;                 % terminal state penalty
R = 0.01*eye(m);         % control penalty

x0=[0.01;1;0.1];

tf_vec = 0:1:tf;

p_vec = ones(nd,tf).*d';

XsUs = zeros(n+m,tf);

xe_est = zeros(n+nd,tf);
x_ = zeros(n,tf);
d_ = zeros(nd,tf);

xs = zeros(n,tf);
us = zeros(m,tf);

xd = zeros(n,tf);
ud = zeros(m,tf);

x = zeros(n,tf);
x(:,1) = x0;

u = zeros(m,tf);
y = zeros(p,tf);


%==========================================================================
% Simulation
%==========================================================================
    
% Simulation
    
    for k = 1:tf
        
        y(:,k) = C*x(:,k) + Cd*p_vec(:,k);
        %=============================
        % Calculate steady state target
        %=============================
        XsUs(:,k) = Mss*d_(:,k);
        
        xs(:,k) = XsUs(1:n,k);
        us(:,k) = XsUs(n+1:end,k);
        
        %=============================
        % Solve the QP
        %=============================
        xd(:,k) = x_(:,k) - xs(:,k);
        
        [ud(:,k), ~] = RHC(A,B,Q,R,Pf,N,M,xd(:,k));
        
        u(:,k) = ud(:,k)+ us(:,k);
        %=============================
        % Update the observer state
        %=============================
        xe_est(:,k+1)= Ae*xe_est(:,k) + Be*u(:,k) + Le*(y(:,k)-Ce*xe_est(:,k));
        
        x_(:,k+1) = xe_est(1:n,k+1);
        d_(:,k+1) = xe_est(n+1:end,k+1);
        
        %=============================
        % Update the process state
        %=============================
        
        x(:,k+1) = A*x(:,k) + B*u(:,k) + Bd*p_vec(:,k);
        
        %=============================        
        % Store current variables in log 
        %=============================

   end % simulation loop