Ts = 0.1;
u_max=20;
u_min=20;


sys=zpk([],[1 1 1],1);
ds=c2d(sys, Ts);
[M,N,C,D]=ssdata(ds);

%horizont
h=50;

V = zeros(h,3);
S = zeros(h);
%for i=1:1:h
%   V(i,:) = C*M^(i-1);
%   for j=i:-1:1
%      if i~=j
%         S(i,i-j)=C*M^(j-1)*N;
%      end;   
%      if i==j
%         S(i,j)=D;
%      end;      
%end;   
%end;

for i=1:h
   S(i,i)=D;
   V(i,:)=C*M^(i-1);
   for j=1:i-1
      S(i,j)=C*M^(i-j-1)*N;
   end
end

Q=100*eye(h);
R=eye(h);
x=[1 1 1]';
x1=[1 1 1]';
r=zeros(h,1);

y=zeros(h);
u=zeros(h);

A=zeros(4,h);
b=zeros(4,1);

j=1;
for i=1:2:4
   A(i,j)=1;
   A(i+1,j)=-1;
   b(i,1)=u_max;
   b(i+1)=(-1)*u_min;
   j=j+1;
end;

run = 2;

if run 
   
   W=S'*Q*S+R;
for i=1:100
   up=-inv(S'*Q*S+R)*S'*Q*(V*x-r);
   y(i,:)=(C*x+D*up(1))';
   x=M*x+N*up(1);
   
   if run==2   
   
   w=S'*Q*(V*x1-r);
   U=quadprog(W,w,A,b);
   x1=M*x1+N*U(1);
   u(i)=C*x1+D*U(1);   
end;

end;


end;