function [u e_int] = LQR_controller(A,B,C,D,x,x_sp,u0,y0,y,e_int,u)

umin=0;
umax=1;
e=y0-y;

if u<umax && u>umin
    e_int=e_int+e;
end

Bu=B(:,1);
[Nstates Nmeas]=size(Bu);
N=0;
measnr=2;
R=10000*eye(Nmeas);
Q=eye(length(A)+1);

%sys=ss(A,Bu,C(measnr,:),D(measnr,1));
%[K,S,E] = lqi(sys,Q,R,N); K
%u=-(K(1:3)*(x-x_sp)) -(K(4)*(y0-y))  +u0(1);

Zss=zeros(Nstates,Nstates);
Zmm=zeros(Nmeas,Nmeas);
Zsm=zeros(Nstates,Nmeas);
A=[A Zsm;-C(measnr,:) Zmm];
B=[Bu; -D(measnr,1)];
[K,S,E] = lqr(A,B,Q,R,N); K
u=-(K(1:3)*(x-x_sp)) -(K(4)*e_int)  +u0(1);

%% Saturation
if(u(1)>1)
    u=1;
elseif (u(1)<0)
    u=0;
end

end

