%......Nullstilling.....

duN=zeros(N*dimu,1);
uN=zeros(N*dimu,1);
dumaxVektor=0; duminVektor=0;

%....Singulærverdier.....

As=[A-eye(dimx),B; C,zeros(dimy,dimu)];
xus=inv(As)*[zeros(dimx,1); yr];

xs=xus(1:dimx);
us=xus(dimx+1:dimx+dimu);

%...........initiering......

du=u-us;
dx=x-[xs xs];

dumax=(umax-us);
dumin=(umin-us);


usVektor=us;
for i=1:N-1
    usVektor=[usVektor; us];
end


%kaller opp lagHEF for å lage H, E og F
[H,E,F]=lagHEF(A,B,C,Q,R,S,N);

%......Begrensningsmatriser.....
%Setter begrensningene til u dirkete inn i quadprogrutinen

for i=1:N
    dumaxVektor(i*dimu-dimu+1:i*dimu)=dumax;
    duminVektor(i*dimu-dimu+1:i*dimu)=dumin;
end

 for i=1:N                                 %W, begrensning i pådragsendring
     for j=1:N
         
         indx=i-j;
         if indx==0, m=1;
         elseif indx==1, m=-1;
         else m=0;
         end
         W(dimy*i-dimy+1:dimy*i,dimu*j-dimu+1:dimu*j) = m*eye(dimu);
     end
     
 end

for i=1:j2              %Begrensningen på utgangen
   for j=1:N
      % D er null for i+j1-j 
      indx = i-j;
      if indx < 0, m = 0;
      else m = 1;
      end
      D(dimy*i-dimy+1:dimy*i,dimu*j-dimu+1:dimu*j) = m*C*A^(i-j)*B;
  end
end

G=[W;-W;D(j1*dimy-dimy+1:j2*dimy,:);-D(j1*dimy-dimy+1:j2*dimy,:)];

%......Hovedløkken......
t=2*dt;
for k=kStart:kStart+Z   %regulerer fra y(kStart+1) til y(kStart+Z+1)
    
     
    %Begrensningene på pådragsendringen
    w1=Dumax+u(:,k-1)-us;
    w2=Dumin+u(:,k-1)-us;
    for i=1:N-1
        w1=[w1;Dumax];
        w2=[w2;Dumin];
    end
    
    %Begrensningene på utgangen, må trekke fra D*us senere pga D*u=d1; D*(u-us)=d1-D*us
    d1=0;  d2=0;
    for i=1:j2
        d1(i*dimy-dimy+1:i*dimy,1)=ymax-C*A^(i)*x(:,k);
        d2(i*dimy-dimy+1:i*dimy,1)=ymin-C*A^(i)*x(:,k);
    end

    g=[w1;-w2;d1(j1*dimy-dimy+1:j2*dimy)-D(j1*dimy-dimy+1:j2*dimy,:)*usVektor;
        -d2(j1*dimy-dimy+1:j2*dimy)+D(j1*dimy-dimy+1:j2*dimy,:)*usVektor];
    
    %.......QP-Problemet.........
    
    dx(:,k)=x(:,k)-xs;
    c=E*dx(:,k)-F*du(:,k-1);
    duN(:,k)=quadprog(H,c,G,g,[],[],duminVektor,dumaxVektor,[],options); %QP-algoritme
    du(:,k)=duN(1:dimu,k);
    u(:,k)=du(:,k)+us;              %legger til singulæreverdien av pådraget
    ur(:,k)=u(:,k)+ur0;       %lagrer de virkelige pådragene
    xtemp=x(:,k)+xr0;
    
    k
    %Her kommer det inn en forstyrrelse ved tidsskritt k
        if k==2
             data.n3=0.04*1.2;
             %data.q1=0.00025*1.2;
           
         end

    %Her sendes pådraget til den ulineære modellen, som så beregner utgangen med
    %Eulerintegrasjon
     while t<dt*(k+1)
          h=0.005;
          t=t+h;
          f=feval(@trykktank_modell,t,xtemp,ur(:,k));
          xtemp=xtemp+h*f(:);
      end
     x(:,k+1)=xtemp-xr0;
      
    y(:,k+1)=C*x(:,k+1);
    yre(:,k+1)=y(:,k+1)+yre0; %lagrer de virkelige utgangene

end

