h=0.1;
s=tf('s');
z = tf('z',h);
w0=4;
z0=0.1;
g = 1/(s+1)*w0^2/(s^2+2*w0*z0*s+w0^2); 

[y,t] = step(g,0:h:10);
%wv = 2*pi*logspace(-3,1,1000);
%frg = squeeze(freqresp(g,wv));
%figure(1)
%semilogx(wv,20*log10(abs(frg)))
%grid on
%figure(2)
%plot(t,y)


gd = c2d(g,h);
%figure(1)
%bode(gd)
%figure(2)

u1 = (t>4.5);

y1=lsim(gd,u1,t);
N=length(y1);
%stairs(t,y1);

%wr=4;
%gref = wr^2/(s^2 + 2*wr*s + wr^2);
%grefd = c2d(gref,h);
%[y2,t2] = step(grefd,10);
%hold on
yr = max(0,min(1,-5:h:5))';

figure(1)
stairs(t,yr,'k');

itermax = 25;
u = zeros(N,itermax);
e = zeros(N,itermax);
y = zeros(N,itermax);
u(:,1) = u1;
y(:,1) = y1;
e(:,1) = yr - y(:,1);

delta = 6;
k=1;

wf=20;
%lp = wf^2/(s^2+2*0.99*wf*s+wf^2);
lp = wf/(s+wf);
lpd = c2d(lp,h);
[b,a]=tfdata(lpd,'v');

for iter = 1:itermax-1
    figure(1)
    stairs(t,yr,'k','Linewidth',1.2);
    hold on
    stairs(t,y(:,iter),'r','Linewidth',1.2);
    stairs(t,u(:,iter),'b','Linewidth',1.2);
    hold off
    u(:,iter+1) = filtfilt(b,a,u(:,iter) + k*[e(delta+1:end,iter) ; zeros(delta,1)]);
    y(:,iter+1) = lsim(gd,u(:,iter+1),t);
    e(:,iter+1) = yr - y(:,iter+1);
    axis([0 10 -0.2 1.6])
    pause
end
legend('yd','y','u')
figure(10)
sigma(1-k*z^delta*gd)
hold on
sigma(lpd*lpd*(1-k*z^delta*gd))
grid on

% 
%  figure(3)
%  plot(t,yr,'k');
%  hold on
%  plot(t,y(:,iter),'r--');
%  plot(t,u(:,iter),'b--');
% normerror = norm(e(:,itermax))
