clear;
close all;

A=[0 1 0 0;0 0 1 0;0 0 0 1;1e-11 -24010 -2539 -54];
B=[0;0;0;31940];
C=[1 0 0 0];
D=0;

[M,N,C,D]=ssdata(c2d(ss(A,B,C,D),0.01,'zoh'));

ref=0;tp=40;
V=matV(tp,M,C);
S=matS(tp,M,N,C,D);
modS=zeros(tp,tp);
modS(1:1:5,1)=1;
modS(6:1:10,2)=1;
modS(11:1:15,3)=1;
modS(16:1:40,4)=1;
modS;
S=S*modS;

Q=eye(tp)*100;
R=eye(tp);
H=S'*Q*S+R;
H=(H+H')/2;
x=[0;0;0;0];
W=zeros(tp,tp);
z=zeros(tp,1);

figure(1);
subplot(2,2,1),hold;
%subplot(2,2,[3,4]),hold;
%subplot(2,2,4),hold;

ref=[zeros(1,20),0.6*ones(1,150),ones(1,150)*(-0.6),zeros(1,300)];
chn=ones(tp,1);

for i=1:500,
   j=((V*x-ref(i))'*Q*S)';
   uopt=modS*qp(H,j,W,z,-chn,chn);
   y=C*x+D*uopt(1);
   uopth(i)=uopt(1);
   yhist(i)=y;
   
   ypred=V*x+S*uopt;
   

   subplot(2,2,1),plot(i,ref(i),'b');
   subplot(2,2,2), plot(1:1:i,uopth,'b',i:1:(i+tp-1),uopt,'r');
   %subplot(2,2,3),plot(i,x,'.');
   subplot(2,2,[3,4]),plot(1:1:i,yhist,'b',i:1:(i+tp-1),ypred,'r');
   drawnow;
   x=M*x+N*uopt(1);

end;
%plot(y);
%figure;
%plot(uopth);