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

%Matrices Q R P
Q = diag([10,10,10,10,10,10,10,10]);
P = diag([1000,1000,1000,1000,1000,1000,1000,1000]);
R = [0.1 0; 0 0.1];

% Estados iniciales
x0=[0 0.3500 0 0.3500 0 0 0 0]'

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

%step(sys,1)
%hold
%step(sys_d,1)

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

k=450;

S=C'*P*C;
N=k

%% Solucion de la ecuacion de Riccati
for k=N-1:-1:1
    K(:,:,k)=inv(R+B'*S*B)*B'*S*A;
    S=(A-B*K(:,:,k))'*S*(A-B*K(:,:,k))+Q+K(:,:,k)'*R*K(:,:,k);    
end

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

% r=ones(N);
for k=1:N-1
    x(:,k+1) = A*x(:,k)-B*K(:,:,k)*x(:,k);
    u(:,k)=-K(:,:,k)*x(:,k+1);
end

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

figure
plot(t,x(1,:));
hold
plot(t,x(3,:)), title('Posición eslabón 1')
grid, legend('q1','theta1','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)')
ylabel('Posición (rad)')

figure
plot(t,x(5,:));
hold
plot(t,x(7,:)), title('Velocidad eslabón 1')
grid, legend('qprim1','thetaprim1','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)')
ylabel('Velocidad (rad/s)')

figure
plot(t,x(2,:));
hold
plot(t,x(4,:)), title('Posición eslabón 2')
grid, legend('q2','thet2','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)')
ylabel('Posición (rad)')

figure
plot(t,x(6,:));
hold
plot(t,x(8,:)), title('Velocidad eslabón 2')
grid, legend('qprim2','thetaprim2','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)')
ylabel('Velocidad (rad/s)')

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

figure, plot(x(1,:),x(5,:)), grid, title('Velocidad1/Posición1');
figure, plot(x(2,:),x(6,:)), grid, title('Velocidad2/Posición2');
