clc;        %clear console
clear;      %clear workspace
close all;  %close all figures
pkg load control %remove if using MATLAB

Cf = 10e-3;  %10mF
L  = 5e-3;    %5mH

A = [0 1/Cf;   %system matrix
     -1/L 0];
     
B = [0;1/L];  %input matrix

C = [1 0];    %output matrix
D = [0];      %feedthrough matrix

K = [0 0];    %feedback gain
N = 1;        %prefilter

%**********************************************************
%************************ Tasks ***************************
% insert your code at "..."

%%task a) set the simulation parameters
sys_ol  = ss(A,B,C,D);
x0      = [10,0];    %initial state
Tf      = 0.2;      %simulation time
u_ol    = 0;                           

figure('Name','open loop','NumberTitle','off');
simulate_system(sys_ol,x0,K,N,u_ol,Tf);

%%task b) design an LQR-controller
Q = [1 0;
      0 0.1];                  
R = 1;
K = lqr(A,B,Q,R,[]);

%%task c) set up closed loop system and simulate
A_cl    = A-B*K;
sys_cl  = ss(A_cl,B,C,D);   

figure('Name','closed loop','NumberTitle','off');   %create new plot window
simulate_system(sys_cl,x0,K,N,0,Tf);

%task d) simulate closed loop system with additional input
w = 100;    %we call the input w to distinguish from u

figure('Name','closed loop additional input','NumberTitle','off');;   %create new plot window
simulate_system(sys_cl,x0,K,N,w,Tf);

%task e)
NxNu = inv([A B;
            C 0])*[0;0;1];
Nx = NxNu(1:2,1);
Nu = NxNu(3,1);
N = Nu+K*Nx;

%alternative way to get N (not covered in lecture)
%N   = inv(C*inv(B*K-A)*B);

w   = 100;
wN  = w*N;

figure('Name','closed loop with reference tracking','NumberTitle','off');
simulate_system(sys_cl,x0,K,N,wN,Tf);