clear variables;
% delete(findall(0,'Type','figure'));
clc;
%%
a = [-0.9646 -0.192 0.169 0.01244];
b = [-1.23 1.547 0.1055 2.812];
dt = 0.0125;   %[s]
t_end = 50;    %[s]
%% Recursive Init
c = t_end/dt;
theta      = zeros(8,c);
theta(:,4) = [0.5; 0.5; 0.5; 0.5;
              0.5; 0.5; 0.5; 0.5];%a(1);a(2);a(3);a(4);b(1);b(2);b(3);b(4)];
P          = 1000*eye(8,8);
phi        = zeros(c,8);
K          = zeros(8,c);
lambda     = 0.99;
%%
u = zeros(1,c);
y = zeros(1,c);
yb = zeros(1,c);
x = zeros(4,c);
rf = zeros(1,c);
ref(   1:c/2) = 3000;   %[mm]
ref(c/2+1:c)  = 1500;   %[mm]
Kf(4) = 1;
xbar = [0;0;0;0];
xdot(:,4) = [1;1;1;1];
%% Start Loop
i = 5;
for t=0:dt:t_end
    [Ao,Bo,Co,Do] = tf2ss([theta(5,i-1) theta(6,i-1) theta(7,i-1) theta(8,i-1) 0],...
                          [1 -theta(1,i-1) -theta(2,i-1) -theta(3,i-1) -theta(4,i-1)]);
    %% Feedback Controller
    pp = [-0.356  -0.328   0.707   0.524];
    F(i,:) = pzplace(Ao,Bo,pp);
    %% Observer
    L = pzplace(Ao',Co',[0.09 0.09 0.09 0.09])'; %0.95
    %% Precompensator
    [num, den] = ss2tf(Ao-Bo*F(i,:),Bo,Co,Do);
    dc1(i) = -dcgain(tf(num, den));%, dt));  %%
    dc2(i) =  Co*inv(eye(4)-Ao)*Bo;     %% Discrete
    dc3(i) = -Co*inv(Ao)*Bo;            %% Continous
    dc4(i) = -1/(Co*inv(Ao-Bo*F(i,:))*Bo);%1050;
    %% Next-State Calculation
    y_diff(i) = y(i-1)-yb(i-1);
    xbar      = Bo*u(i-1) + Ao*xbar + L*y_diff(i);
    %% Input Adaption
    u(i) = (ref(i)*dc3(i)) - (F(i,:)*xbar);  
    %% Plant Modelling - Recursive Analysis
    y(i)       = -a(1)*y(i-1)-a(2)*y(i-2)-a(3)*y(i-3)-a(4)*y(i-4)...
                 +b(1)*u(i-1)+b(2)*u(i-2)+b(3)*u(i-3)+b(4)*u(i-4);
    y(i)       = y(i) + 0.5*((rand()-0.5)*2); 
    yb(i)      = -theta(1,i-1)*y(i-1)-theta(2,i-1)*y(i-2)-theta(3,i-1)*y(i-3)-theta(4,i-1)*y(i-4)...
                 +theta(5,i-1)*u(i-1)+theta(6,i-1)*u(i-2)+theta(7,i-1)*u(i-3)+theta(8,i-1)*u(i-4);
    phi(i,:)   = [-y(i-1) -y(i-2) -y(i-3) -y(i-4) u(i-1) u(i-2) u(i-3) u(i-4)];
    K          = (P*phi(i,:)')/(lambda+(phi(i,:)*P*phi(i,:)'));
    P          = ((eye(8)-K*phi(i,:))*P)*(1/lambda);
    theta(:,i) = theta(:,i-1)+K*(y(i)-phi(i,:)*theta(:,i-1));    
    %%
    if(i~=c)
        i = i+1;
    else
        break;
    end
end
%%
t=0:dt:t_end-dt;
figure(1);
subplot(5,1,1)
    plot(t,theta');
    grid;
    title('Model Coefficients'), 
    axis([0 t_end min(min(theta)) max(max(theta))]),
subplot(5,1,2:3)
    plot(t,ref), hold on;   %%*48
    plot(t,y,'r'), hold off, grid on;
subplot(5,1,4)
    plot(t,u), grid on;
    legend('Ctrl Output');
    axis([0 t_end min(u)-(0.05*max(u)) max(u)+(0.05*max(u))]);
subplot(5,1,5)
    plot(dc4,'b'), grid on, hold on;
    plot(dc1,'r'), grid on, hold on;
    plot(dc2,'c'), grid on, hold on;
    plot(dc3,'g'), grid on, hold off;