%%
close all
clc
clear
%%
load('ABCD.mat');

% Generación de trayectoria
tm=0.01;
textra=0.2;
% [qref1 vref1 aref1 t1]=trapez(0, 1.04, 1.8, 1.95, tm, textra);
% [qref2 vref2 aref2 t2]=trapez(0, 0.35, 1, 1, tm, 0.48);

% trapezoidal orden 4
[qref1 vref1 aref1 jerk jerk_prim t3] = trapez4( 0, 1, 0.5, 2, 10, 500, tm,1.028); %trapez4( 0, 1, 0.5, 3, 20, 800, tm,1.2);
[qref2 vref2 aref2 jerk4 jerk_prim4 t4] = trapez4( 0, 0.5, 0.5, 2, 10, 500, tm,2.028);

%Matrices Q R S
Q = diag([10, 10, 10, 10, 10, 10, 10, 10]); %diag([5,5,5,5,5,5,5,5]);
P = diag([1000,1000,1000,1000,1000,1000,1000,1000]);
R = [0.0001 0; 0 0.001];

x0=[qref1(1);qref2(1);qref1(1);qref2(1);vref1(1);vref2(1);vref1(1);vref2(1)]; % Partimos desde el origen

sys = ss(A,B,C,D);
dt=0.01;
sys_d = c2d(sys,dt,'zoh');

%y = step(sys,5)
%hold
%y2 = step(sys_d,5)

t1 = 0:(5/218):5;
t2 = 0:(5/50):5;

A = sys_d.A;
B = sys_d.B;
C = sys_d.C;
D = sys_d.D;

k=length(qref1);

S=C'*P*C;
v = zeros(8, k);
Kv = zeros(2,8,k-1);
K = zeros(2,8,k-1);

N=k;
%r=[1; 0];
v(:,k)=C'*P*[qref1(k);qref2(k);qref1(k);qref2(k);vref1(k);vref2(k);vref1(k);vref2(k)];

%% Solucion de la ecuacion de Riccati
for k=N-1:-1:1
    K(:,:,k)=inv(R+B'*S*B)*B'*S*A;
    Kv(:,:,k)= inv(R+B'*S*B)*B';
    S=A'*S*(A-B*K(:,:,k))+ C'*Q*C;
%     S=(A-B*K(:,:,k))'*S*(A-B*K(:,:,k))+Q+K(:,:,k)'*R*K(:,:,k);
    v(:,k)=(A-B*K(:,:,k))'*v(:,k+1)+C*Q*[qref1(k);qref2(k);qref1(k);qref2(k);vref1(k);vref2(k);vref1(k);vref2(k)];    
end

%Simulacion del sistema realimentado
x(:,1)=x0;

suma_par1 = 0;
suma_par2 = 0;

% r=ones(N);

%load('ABCD_sin_gravedad.mat');
for k=1:N-1
    x(:,k+1) = A*x(:,k)-B*K(:,:,k)*x(:,k)+B*Kv(:,:,k)*v(:,k+1);
    u(:,k)=-K(:,:,k)*x(:,k)+Kv(:,:,k)*v(:,k+1);
    suma_par1  = suma_par1 + (u(1,k))^2;
    suma_par2  = suma_par2 + (u(2,k))^2;
end

par_medio1 = sqrt((suma_par1/(N-1)))
par_medio2 = sqrt((suma_par2/(N-1)))

ecuad(qref1,x(1,:)') % Error cuadrático medio posición eslbón 1.  
ecuad(vref1,x(5,:)') % Error cuadrático medio velocidad eslabón 1.

ecuad(qref1,x(2,:)') % Error cuadrático medio posición eslbón 2.  
ecuad(vref1,x(6,:)') % Error cuadrático medio velocidad eslabón 2.


t =0:dt:(N-1)*dt;

figure
set(gca,'fontsize',12);
hold on;
plot(t,x(1,:));
plot(t,x(3,:)), title('Seguimiento de trayectoria (Posición eslabón 1)')
plot(t,qref1)
grid, legend('q1','theta1','qref1','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)')
ylabel('Posición (rad)')

figure
set(gca,'fontsize',12);
hold on;
plot(t,x(5,:));
plot(t,x(7,:)), title('Seguimiento de trayectoria (Velocidad eslabón 1)')
plot(t,vref1)
grid, legend('qprim1','thetaprim1','vref1','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)')
ylabel('Velocidad (rad/s)')

figure
set(gca,'fontsize',12);
hold on;
plot(t,x(2,:));
plot(t,x(4,:)), title('Seguimiento de trayectoria (Posición eslabón 2)')
plot(t,qref2)
grid, legend('q2','thet2','qref2','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)')
ylabel('Posición (rad)')

figure
set(gca,'fontsize',12);
hold on;
plot(t,x(6,:));
plot(t,x(8,:)), title('Seguimiento de trayectoria (Velocidad eslabón 2)')
plot(t,vref2)
grid, legend('qprim2','thetaprim2','vref2','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)')
ylabel('Posición (rad)')

figure
plot(t(1:N-1),u), title('Acción de control'), legend('u1','u2','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)')
grid

figure, plot(x(1,:),x(5,:)), grid, title('Velocidad1/Posición1');
xlabel('Posición (rad)')
ylabel('Velocidad (rad/s)')
set(gca,'fontsize',12);
hold on;
figure, plot(x(2,:),x(6,:)), grid, title('Velocidad2/Posición2');
xlabel('Posición (rad)')
ylabel('Velocidad (rad/s)')
set(gca,'fontsize',12);
hold on;
