%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                          %
% mainRic.m  solves "general" LQR problem  %
%                                          %
%                                          %
%                                          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global ti tf Tper

global A B C Q0 Qref R S

global p tp 

% The System matrices

A=[-0.089  -2.19  0  0.319  0  0 ; 0.076  -0.217  -0.166  0  0  0 ;-0.602  0.327  -0.975  0  0  0 ;0  0.15  1  0  0  0 ;0  1  0  0  0  0 ;1  0  0  0  2.19  0]
B=[0  0.0327 ; 0.0264  -0.151 ; 0.227  0.0636 ; 0  0 ; 0  0 ; 0  0]
C=[0  0  0  0  0 1];

% The Cost functional


Q0=eye(6).*500;
xTref=[0 0 0 0 0 10]';
g0=-Q0*xTref;


Qref=20

R=eye(2).*1;
S=zeros(6,2);

% Start and final time

ti=0;
tf=10;

% boundary values

p1=PtoX(Q0,g0,0);

x0=zeros(6,1);

% Integrate the Riccati equations
[tp,p] = ode45('fRic',[ti tf],[p1]);

% Integrate the system equations
[tx,x] = ode45('fSys',[ti tf],[x0]);



clear u
% ReCalculate the control (for plotting)

for i=1:length(x)

t=tx(i);
tRev=tf-(t-ti);

pRicc_t=(interp1(tp,p,tRev))';

n=length(A);
[P,q,c]=XtoP(pRicc_t,n);

x_t=x(i,:)';

Vx=2*(P*x_t+q);

u_t=(-1/2)*inv(R)*(B'*Vx+2*S'*x_t);
u(:,i)=u_t;

end

subplot(3,1,1)
plot(tx,x(:,6),'-.',tx,5*(1-cos(1*pi*tx/tf)),'-')
ylabel('feet')
legend('y','ref')

subplot(3,1,2)
plot(tx,u(1,:),'-.',tx,u(2,:),'-')
ylabel('rad*10^{-2}')
legend('da','dr')


subplot(3,1,3)
plot(tx,x(:,4),'-.',tx,x(:,5),'-')
ylabel('rad*10^{-2}')
xlabel('time (s)')
legend('Roll','Yaw')