%% =========================================================================
%% PLANTILLA MAESTRA DE RESOLUCION DE EXAMENES DE ROBOTICA (GITI)
%% Escuela Tecnica Superior de Ingenieria - Universidad de Sevilla
%% =========================================================================
% INSTRUCCIONES RAPIDAS:
% 1. Modifica la Tabla DH en la Seccion 1 segun el robot del enunciado.
% 2. Ejecuta la seccion (Ctrl + Enter) para obtener T_0^n, PCD y HOME.
% 3. Ejecuta la Seccion 2 para obtener el Jacobiano y Singularidades.
% 4. Ejecuta la Seccion 3 para la Dinamica o Seccion 4 para Control PID.
% =========================================================================

clear; clc; close all;

%% =========================================================================
%% SECCION 1: CINEMATICA DIRECTA (DH, T_0^n, PCD, HOME)
%% =========================================================================
fprintf('--- SECCION 1: CINEMATICA DIRECTA (DH Y PCD) ---\n');

% 1. Variables simbolicas reales
syms q1 q2 q3 L1 L2 L3 real
PI = sym(pi);

% 2. Introducir aqui la Tabla de Denavit-Hartenberg del examen:
% dh_matrix(theta_i, d_i, a_i, alpha_i)
% (Ejemplo: Robot R-P-R Coaxial de Junio 2025 P2)
A1 = dh_matrix(q1,0,L1,PI/2);
A2 = dh_matrix(0,q2,0,-PI/2);
A3 = dh_matrix(q3,0,L3,0);

% 3. Matriz resultante
T03 = simplify(A1 * A2 * A3);
disp('Matriz Homogenea Resultante T_0^3:');
disp(T03);

% 4. Posicion Cartesiana y Orientacion del Extremo
p = T03(1:3, 4);
x_raw = p(1);
y_raw = p(2);
z_raw = p(3);

% Extraer orientacion angular phi respecto a la horizontal:
% Viene dada por el bloque de rotacion 2D: R = [cos(phi), -sin(phi); sin(phi), cos(phi)]
phi = simplify(atan2(T03(2,1), T03(1,1)));

% 5. Compactacion automatica factorizada (como pide el tribunal)
x = compactar_expresion(x_raw, [cos(q1), sin(q1)]);
y = compactar_expresion(y_raw, [sin(q1), cos(q1)]);
z = simplify(z_raw);

fprintf('\nEcuaciones del PCD Compactadas:\n');
fprintf('  x(q)   = %s\n', char(x));
fprintf('  y(q)   = %s\n', char(y));
fprintf('  z(q)   = %s\n', char(z));
fprintf('  phi(q) = %s\n\n', char(phi));

% 6. Comprobacion obligatoria en Posicion HOME (q1=0, q2=0, q3=0)
T_HOME = simplify(subs(T03, [q1, q2, q3], [0, 0, 0]));
disp('Posicion y Matriz en HOME (q=0):');
disp(T_HOME);


%% =========================================================================
%% SECCION 2: JACOBIANO Y ANALISIS DE SINGULARIDADES
%% =========================================================================
fprintf('\n--- SECCION 2: JACOBIANO Y SINGULARIDADES ---\n');

% Vector operacional (planar: [x; y; phi]; espacial: [x; y; z])
X_op = [x; y; phi];
q_vars = [q1; q2; q3];

% Calculo directo con la funcion del toolkit
[J, det_J, sing] = calcular_jacobiano_y_singularidades(X_op, q_vars);


%% =========================================================================
%% SECCION 3: PROBLEMA CINEMATICO INVERSO (PCI PLANAR)
%% =========================================================================
fprintf('\n--- SECCION 3: PROBLEMA CINEMATICO INVERSO (PCI) ---\n');

% Desacoplo de la muñeca para el robot planar
% (x, y, phi dados -> encontrar q1, q2, q3)
xw = x - L3 * cos(phi);
yw = y - L3 * sin(phi);

fprintf('Posicion de la muñeca desacoplada:\n');
fprintf('  xw = %s\n', char(simplify(xw)));
fprintf('  yw = %s\n', char(simplify(yw)));

% Para robot R-P-R coaxial:
% q1 = atan2(yw, xw)
% q2 = sqrt(xw^2 + yw^2) - L2
% q3 = phi - q1


%% =========================================================================
%% SECCION 4: CONTROL PID Y SINTONIZACION ANALITICA
%% =========================================================================
fprintf('\n--- SECCION 4: SINTONIZACION CONTROLADOR PID ---\n');

% Datos nominales del examen
J_eje1 = 2.525; % kg*m^2
wn_1   = 3.0;   % rad/s
xi_1   = 1.0;   % critico
Kt_1   = 10.0;  % N*m/A

[Kp, Kd, Ki, Kp_i, Kd_i, Ki_i] = sintonizar_pid(J_eje1, wn_1, xi_1, Kt_1);