function rllqr(Acont,Bcont,Ts,x1,Q,R,L0,sigma_e,Nls,N)
% A function that performs an reinforcement learning LQ control
% experiment with a system with two states and one input
% 
% Acont  : The continuous-time A matrix (2x2)
% Bcont  : The continuous-time B matrix (2x1)
% Ts     : The sampling time (positive scalar)
% x1     : The initial state vector (2x1)
% Q      : The first weighting matrix from the LQ criterion (2x2 symmetric)
% R      : The second weighting matrix from the LQ criterion (1x1)
% L0     : The initial state feedback matrix (2x1)
% sigma_e: The standard deviation of the probing noise (positive scalar)
% Nls    : The length of each data collection step (positive integer)
% N      : The total number of samples in the experiment (positive integer > Nls)

Ccont=eye(2);
Dcont=zeros(2,1);
G=ss(Acont,Bcont,Ccont,Dcont);
Gd=c2d(G,Ts);
A=Gd.A;
B=Gd.B;
n=2; %Number of states
m=1; %Number of inputs

Nsteps=floor(N/Nls); 
time=Ts*(0:N);
%----------------------------------------------------------
%----------------------------------------------------------

x=zeros(n,N+1);
x(:,1)=x1;
us=zeros(m,N+1);
Ls=zeros(m*n,Nsteps+1);
Ls(:,1)=L0(:);
thetahat=zeros((n+m)*(n+m+1)/2,1);
Phi=zeros(Nls,(n+m)*(n+m+1)/2);
Y=zeros(Nls,1);

for j=0:Nsteps-1
    for k=1:Nls
        t=j*Nls+k;
        L=reshape(Ls(:,j+1),m,n);
        us(:,t)=-L*x(:,t)+sigma_e*randn(m,1);
        x(:,t+1)=A*x(:,t)+B*us(:,t);
    
        u=us(:,t);
        up=-L*x(:,t+1);
        x1=x(1,t);
        x2=x(2,t);
        xp1=x(1,t+1);
        xp2=x(2,t+1);
        phi=0.5*[x1^2 2*x1*x2 x2^2 2*u*x1 2*u*x2 u^2]';
        phip=0.5*[xp1^2 2*xp1*xp2 xp2^2 2*up*xp1 2*up*xp2 up^2]';
        Phi(k,:)=phi'-phip';
        Y(k)=0.5*(x(:,t)'*Q*x(:,t)+u'*R*u);
    end
    thetahat=Phi\Y;
    Suu=thetahat(6);
    Sux=[thetahat(4) thetahat(5)];
    Ls(:,j+2)=inv(Suu)*Sux;
end
    
[Popt,EG,Lopt]=dare(A,B,Q,R);
Sopt=[A'*Popt*A+Q A'*Popt*B;B'*Popt*A B'*Popt*B+R] %Theoretical S

Shat_final=[thetahat(1) thetahat(2) thetahat(4);
           thetahat(2) thetahat(3) thetahat(5);
           thetahat(4) thetahat(5) thetahat(6)] %Final S estimate

figure(1)
clf
subplot(2,1,1)
timeshort=Ts*Nls*(0:Nsteps);
plot(timeshort,Lopt(1)*ones(1,Nsteps+1),'g--','LineWidth',2)
hold on
[tz,L1z]=zohsignal(timeshort,Ls(1,:));
plot(tz,L1z,'k','LineWidth',1)
title('Feedback gain l_1')
legend('optimal','estimated')
axis([0 timeshort(end) 1.1*min([Ls(1,:) -1 Lopt(1)]) 1.1*max([Ls(1,:) 1 Lopt(1)])])
subplot(2,1,2)
plot(timeshort,Lopt(2)*ones(1,Nsteps+1),'g--','LineWidth',2)
hold on
[tz,L2z]=zohsignal(timeshort,Ls(2,:));
plot(tz,L2z,'k','LineWidth',1)
title('Feedback gain l_2')
legend('optimal','estimated')
axis([0 timeshort(end) 1.1*min([Ls(2,:) -1 Lopt(2)]) 1.1*max([Ls(2,:) 1 Lopt(2)])])

figure(2)
clf
subplot(2,1,1)
plot(time,x(1,:),'k','LineWidth',1)
title('State x_1')
subplot(2,1,2)
plot(time,x(2,:),'k','LineWidth',1)
title('State x_2')

figure(3)
clf
plot(time,us,'k','LineWidth',1)
title('Input u')


%-------------------------------------------------------------
function [tzoh,yzoh]=zohsignal(t,y)
N=length(y);
Ts=t(2)-t(1);
tzoh=zeros(2*N,1);
yzoh=zeros(2*N,1);

for k=1:N
tzoh(2*k-1)=t(k);
tzoh(2*k)=t(k)+Ts;
yzoh(2*k-1)=y(k);
yzoh(2*k)=y(k);
end