function [upast,A,B]=totalwithNU(DatatoMatlab,upast)
%Initialize upast=ones(13,1) in the command window

global H b fmark w A B ypast uopt yrls urls beta P K

%Function that minimize the cost function subject to constrains
%on the control signal. The control signal is the setpoint to the slave-controller
%Startvalues for the model. Updates by RLS
A=[1  -0.4548 0.0218 -0.5309];
B=[0 0 -0.0235 0.0552 0.0255 0.0414 -0.0058];

%Constants
lamda=0.3; %control-weighting sequense
N1=2;   %minimum costing horizon

%Data from Labview
NU=DatatoMatlab(3); %control horizon
N2=DatatoMatlab(2); %maximum costing horizon
N=N2-N1+1;
ypast=DatatoMatlab(4:16)'; %past output
w=DatatoMatlab(1); %referense trajectory(=setpoint)

%Calculate delta u = u(t)-u(t-1). Used in free response for cost function with 
%delta u as variable. This free response is used for calculated the free response
%for cost function with u as variable.
for t=1:size(upast)-1
   delta_u(t)=upast(t)-upast(t+1);
end
delta_u=delta_u';


%Calculate the Diophantine eq recursivly
[E_rec,F_rec]=diophantine(A,B,N);

%Generate the G1,G2...GN2 polynominal. Used in the free response calc.
for k=1:N
   g(k,:)=(conv(E_rec(k,:),B));
end

%Generate the G matrix
Gprime=diag(g);
G=zeros(N);
for j=1:N
   G(:,j)=[zeros(j-1,1);Gprime(1:N-j+1)];
end

G=G(1:N,1:NU);

%Compute the G1,G2,...GN2 for the free response
gfree=triu(g,1);
m=size(gfree,2);
for p=1:N
   gtemp(p,:)=[gfree(p,p:m) zeros(1,p)]; 
end

gdiff=gtemp(:,1:size(gtemp,2)-1);
gdiff=gdiff(:,1:N);

%The free response
f=[gdiff(:,2:size(gdiff,2))*delta_u(1:size(gdiff,2)-1)+F_rec*ypast(1:size(F_rec,2))];

%Making f and w of the same dimention. Labview poke often more data than 
%necessary.Less values is needed for a shorter control horizon
w=w*ones(size(f,1),1);

%Compute variables for the cost function for u, not deltau
D1=diag(ones(NU,1));
D2=diag(-ones(NU-1,1),-1);
D=D1+D2;
Gmark=G*D
f1=[upast(1);zeros(NU-1,1)];
f2=f-G(:,1)*f1(1);
H=2*(Gmark'*Gmark+lamda*D'*D);
b=2*((f2-w)'*Gmark-lamda*f1'*D);
fmark=(f2-w)'*(f2-w)+lamda*f1'*f1;

%Minimizing the cost function
ustart=7*ones(NU,1);
umin=0.5*ones(NU,1);
umax=18*ones(NU,1);
[u,fval]=fmincon('costfuntotal',ustart,[],[],[],[],umin,umax);
uopt=u(1)+ random('Normal',0,0.25);
upast=[uopt; upast(1:size(upast,1)-1)];

%The RLS part
rlsdata=[ypast(1),uopt];
[A,B]=adaptinGPC(rlsdata);

