clc
clear
close all

%% 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);


%% Estados iniciales y finales (deseado)
x0 = [0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0];
xf = [pi/2 0.5 pi/2 0.5 0.0 0.0 0.0 0.0];
dt = 0.01;
%[n m] = size(xf)
N = 150; %100;
% Acción de control inicial
u = zeros(N, 2);

% Matrices de penalización de estados y acción de control
S = diag([50000.0 50000.0 50000.0 50000.0 50000.0 50000.0 50000.0 50000.0]);
% Qf = diag([500000.0 500000.0 500000.0 500000.0 500000.0 500000.0 500000.0 500000.0]);
% Q = Qf;
Q = diag([1.00 1.00 1.00 1.00 1.00 1.00 1.00 1.00]); %zeros(3);
R = [0.0001 0; 0 0.001]; % [0.01 0; 0 0.1];[0.000001 0; 0 0.00001];

% Tiempo de muestreo, numero de iteraciones y condición de convergencia 

iter = 500;
conveg_cond = 0.0001;
%N = n-1;

% Se inicia el algoritmo iLQR
traju = ilqr_control(x0, xf, u, S, Q, R, dt, iter, conveg_cond);
% Se simula con la trayectoria de u obteniada
xtraj = ilqr_sim(x0, traju, dt); 

len = length(xtraj);
timescale = 0:1:len-1;
timescale = dt*timescale;

%Referencia
ref1 = xf(1,1)*ones(1,length(timescale));
ref2 = xf(1,2)*ones(1,length(timescale));

% Posición eslabón 1
figure;
set(gca,'fontsize',12);
hold on;
plot(timescale, xtraj(:,1));
plot(timescale, xtraj(:,3)), title('Trayectoria seguida (Posición eslabón 1)')
plot(timescale, ref1)
grid, legend('q1','theta1','ref','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad)');

% Posición eslabón 2
figure;
set(gca,'fontsize',12);
hold on;
plot(timescale, xtraj(:,2));
plot(timescale, xtraj(:,4)), title('Trayectoria seguida (Posición eslabón 2)')
plot(timescale, ref2)
grid, legend('q2','theta2','ref2','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad)');

% Velocidad eslabón 1
figure;
set(gca,'fontsize',12);
hold on;
plot(timescale, xtraj(:,5));
plot(timescale, xtraj(:,7)), title('Trayectoria seguida (Velocidad eslabón 1)')
grid, legend('q-prim1','theta-prim1','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad/s)');

% Velocidad eslabón 2
figure;
set(gca,'fontsize',12);
hold on;
plot(timescale, xtraj(:,6));
plot(timescale, xtraj(:,8)), title('Trayectoria seguida (Velocidad eslabón 2)')
grid, legend('q-prim2','theta-prim2','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad/s)');

% Acción de control
figure;
set(gca,'fontsize',12);
hold on;
plot(timescale(1:N),traju), title('Acción de control'), legend('u1','u2','Location','southeast','Orientation','vertical');
grid;
xlabel('Tiempo (s)');
ylabel('Par (Nm)');

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

