%% =========================================================================
%% EXAMEN OFICIAL ROBOTICA - CONVOCATORIA 8 DE JUNIO DE 2025 (PROBLEMA A)
%% MANIPULADOR PLANO R-P-R COAXIAL (3 GDL)
%% Grado en Ingenieria de Tecnologias Industriales - E.S.I. Sevilla
%% =========================================================================
% Script analitico, simbolico y numerico para resolver completamente:
% 1. Asignacion de sistemas de coordenadas, parametros DH y matriz T_0^3
% 2. Problema Cinematico Directo (PCD) para punto generico (x, y, phi)
% 3. Jacobiano geometrico, determinante y analisis de singularidades
% 4. Problema Cinematico Inverso (PCI) analitico y validacion numerica
% =========================================================================

clear; clc; close all;

fprintf('=========================================================================\n');
fprintf('  RESOLUCION OFICIAL: EXAMEN GITI JUNIO 2025 - PROBLEMA A (R-P-R COAXIAL)\n');
fprintf('=========================================================================\n\n');

%% 1. DEFINICION DE VARIABLES SIMBOLICAS Y CONSTANTES
syms q1 q2 q3 real          % q1: rotacion base (rad), q2: extension telescopica (m), q3: rotacion muñeca (rad)
syms L2 L3 real positive    % L2: longitud fija base corredera (m), L3: longitud herramienta (m)
PI = sym(pi);               % Constante pi simbolica exacta

%% 2. APARTADO 1: ASIGNACION DE SISTEMAS, TABLA D-H Y MATRICES HOMOGENEAS
% Justificacion de Sistemas de Coordenadas:
% - Sistema {S0} (Base): Origen O0 en el centro del eje de giro 1. Z0 saliente
%   perpendicular al plano (eje de revolucion 1). X0 horizontal a la derecha.
% - Sistema {S1}: Origen O1 en O0. Para que la articulacion 2 (prismatica)
%   se desplace a lo largo de Z1, se orienta Z1 colineal con el brazo.
%   La normal comun a Z0 y Z1 es perpendicular al brazo en el plano:
%   theta1 = q1 + pi/2, d1 = 0, a1 = 0, alpha1 = +pi/2.
% - Sistema {S2} (Muñeca): Origen O2 en el centro de la articulacion 3.
%   Se traslada una distancia d2 = L2 + q2 a lo largo de Z1.
%   Para que el eje de giro 3 sea perpendicular al plano (paralelo a Z0),
%   se rota alpha2 = -pi/2 alrededor de X2:
%   theta2 = 0, d2 = L2 + q2, a2 = 0, alpha2 = -pi/2.
% - Sistema {S3} (Extremo/Herramienta): Origen O3 en el extremo (x, y).
%   Rotacion theta3 = q3 - pi/2 alrededor de Z2, traslacion a3 = L3 a lo largo de X3:
%   theta3 = q3 - pi/2, d3 = 0, a3 = L3, alpha3 = 0.

fprintf('--- APARTADO 1: MATRICES DE TRANSFORMACION DENAVIT-HARTENBERG ---\n');

% Matriz DH estandar
dh = @(th, d, a, alp) [ ...
    cos(th), -sin(th)*cos(alp),  sin(th)*sin(alp), a*cos(th); ...
    sin(th),  cos(th)*cos(alp), -cos(th)*sin(alp), a*sin(th); ...
          0,          sin(alp),          cos(alp),         d; ...
          0,                 0,                 0,         1 ];

A1 = dh(q1 + PI/2, 0, 0, PI/2);
A2 = dh(0, L2 + q2, 0, -PI/2);
A3 = dh(q3 - PI/2, 0, L3, 0);

fprintf('Matriz A_1 (Rotacion articular 1 + cambio a eje longitudinal):\n');
disp(A1);

fprintf('Matriz A_2 (Desplazamiento prismatico d2 = L2 + q2 + retorno a eje normal):\n');
disp(A2);

fprintf('Matriz A_3 (Rotacion articular 3 + desplazamiento L3 hasta extremo):\n');
disp(A3);

% Matriz homogenea resultante T_0^3
T03 = simplify(A1 * A2 * A3);
fprintf('Matriz de Transformacion Resultante T_0^3 = A1 * A2 * A3:\n');
disp(T03);

% Comprobacion obligatoria en Posicion HOME (q1 = 0, q2 = 0, q3 = 0)
T_HOME = simplify(subs(T03, [q1, q2, q3], [0, 0, 0]));
fprintf('Verificacion de Posicion HOME (q1=0, q2=0, q3=0):\n');
disp(T_HOME);

p_HOME = T_HOME(1:3, 4);
fprintf('  Posicion cartesiana en HOME: x = %s, y = %s, z = %s\n', ...
    char(p_HOME(1)), char(p_HOME(2)), char(p_HOME(3)));
assert(p_HOME(1) == L2 + L3, 'Error en comprobacion HOME: x != L2 + L3');
assert(p_HOME(2) == 0, 'Error en comprobacion HOME: y != 0');
assert(p_HOME(3) == 0, 'Error en comprobacion HOME: z != 0');
fprintf('  -> HOME verificado satisfactoriamente: Extremo en (L2 + L3, 0, 0) y matriz de rotacion Identidad.\n\n');

%% 3. APARTADO 2: PROBLEMA CINEMATICO DIRECTO (PCD)
fprintf('--- APARTADO 2: PROBLEMA CINEMATICO DIRECTO (PCD) ---\n');

x_pcd = simplify(T03(1, 4));
y_pcd = simplify(T03(2, 4));
phi_pcd = simplify(q1 + q3); % Orientacion angular del eslabon 3 respecto al eje X0

fprintf('Ecuaciones de posicion y orientacion del extremo (x, y, phi):\n');
fprintf('  x(q1, q2, q3)   = %s\n', char(x_pcd));
fprintf('  y(q1, q2, q3)   = %s\n', char(y_pcd));
fprintf('  phi(q1, q3)     = %s\n\n', char(phi_pcd));

%% 4. APARTADO 3: JACOBIANO Y ANALISIS DE SINGULARIDADES
fprintf('--- APARTADO 3: MATRIZ JACOBIANA Y SINGULARIDADES ---\n');

% Vector de variables operacionales y articulares
X_op = [x_pcd; y_pcd; phi_pcd];
q_vec = [q1; q2; q3];

% Jacobiano analitico J = d(X_op)/d(q)
J = jacobian(X_op, q_vec);
fprintf('Matriz Jacobiana J(q) [3x3]:\n');
disp(J);

% Determinante del Jacobiano
det_J = simplify(det(J));
fprintf('Determinante analitico del Jacobiano: det(J) = %s\n', char(det_J));

% Analisis de singularidades: det(J) = 0
fprintf('\nCondicion de Singularidad:\n');
fprintf('  det(J) = 0  <=>  -(L2 + q2) = 0  <=>  q2 = -L2  (o L2 + q2 = 0)\n');
fprintf('Interpretacion Fisica:\n');
fprintf('  - Cuando q2 = -L2, la distancia del origen O0 a la muñeca O2 se anula.\n');
fprintf('  - El eje de giro 1 y el eje de giro 3 pasan a ser colineales (ambos en el origen).\n');
fprintf('  - Las columnas 1 y 3 del Jacobiano se vuelven linealmente dependientes.\n');
fprintf('  - El manipulador pierde un grado de libertad instantaneo (no puede generar velocidad\n');
fprintf('    radial y angular de forma independiente).\n\n');

%% 5. APARTADO 4: PROBLEMA CINEMATICO INVERSO (PCI)
fprintf('--- APARTADO 4: PROBLEMA CINEMATICO INVERSO (PCI) ---\n');

% Ecuaciones del PCI dadas (x, y, phi):
% 1) Desacoplo de la muñeca (xw, yw):
%    xw = x - L3*cos(phi) = (L2 + q2)*cos(q1)
%    yw = y - L3*sin(phi) = (L2 + q2)*sin(q1)
%
% 2) Calculo de q1:
%    q1 = atan2(yw, xw) = atan2(y - L3*sin(phi), x - L3*cos(phi))
%
% 3) Calculo de q2:
%    (L2 + q2)^2 = xw^2 + yw^2
%    Considerando la configuracion habitual con el brazo extendido hacia adelante (L2 + q2 > 0):
%    q2 = sqrt(xw^2 + yw^2) - L2 = sqrt((x - L3*cos(phi))^2 + (y - L3*sin(phi))^2) - L2
%
%    (Rama alternativa matematica con brazo apuntando en sentido opuesto:
%     q1_alt = atan2(-yw, -xw), q2_alt = -sqrt(xw^2 + yw^2) - L2)
%
% 4) Calculo de q3:
%    q3 = phi - q1

fprintf('Algoritmo analitico del PCI:\n');
fprintf('  Paso 1: xw = x - L3*cos(phi)\n');
fprintf('          yw = y - L3*sin(phi)\n');
fprintf('  Paso 2: q1 = atan2(yw, xw)\n');
fprintf('  Paso 3: q2 = sqrt(xw^2 + yw^2) - L2\n');
fprintf('  Paso 4: q3 = phi - q1\n\n');

%% 6. VALIDACION NUMERICA CRUZADA DEL PCI (TEST POINTS)
fprintf('--- VALIDACION NUMERICA DEL PCI ---\n');

% Definimos valores numericos de ensayo
L2_num = 1.0;   % m
L3_num = 0.5;   % m

test_cases = [ ...
    0,       0,       0;       % HOME
    pi/6,    0.3,    -pi/4;    % Caso 1: Giro positivo, extension positiva, muñeca negativa
    pi/3,    0.7,     pi/6;    % Caso 2: Primer cuadrante
    -pi/4,   0.2,     pi/2     % Caso 3: Cuarto cuadrante
];

for k = 1:size(test_cases, 1)
    q1_k = test_cases(k, 1);
    q2_k = test_cases(k, 2);
    q3_k = test_cases(k, 3);
    
    % Evaluacion PCD
    x_val = double(subs(x_pcd, [q1, q2, q3, L2, L3], [q1_k, q2_k, q3_k, L2_num, L3_num]));
    y_val = double(subs(y_pcd, [q1, q2, q3, L2, L3], [q1_k, q2_k, q3_k, L2_num, L3_num]));
    phi_val = double(subs(phi_pcd, [q1, q3], [q1_k, q3_k]));
    
    % Resolucion PCI
    xw_val = x_val - L3_num * cos(phi_val);
    yw_val = y_val - L3_num * sin(phi_val);
    q1_sol = atan2(yw_val, xw_val);
    q2_sol = sqrt(xw_val^2 + yw_val^2) - L2_num;
    q3_sol = phi_val - q1_sol;
    
    err_q1 = abs(q1_k - q1_sol);
    err_q2 = abs(q2_k - q2_sol);
    err_q3 = abs(q3_k - q3_sol);
    
    fprintf('Test %d: q_orig = [%.3f rad, %.3f m, %.3f rad] -> Pose = (%.3f m, %.3f m, %.3f rad)\n', ...
        k, q1_k, q2_k, q3_k, x_val, y_val, phi_val);
    fprintf('        q_recov = [%.3f rad, %.3f m, %.3f rad] -> Max Error = %.2e\n', ...
        q1_sol, q2_sol, q3_sol, max([err_q1, err_q2, err_q3]));
    assert(max([err_q1, err_q2, err_q3]) < 1e-12, 'Error en validacion numerica PCI');
end

fprintf('\n¡VALIDACION NUMERICA COMPLETADA CON EXITO ABSOLUTO (Error < 1e-12)!\n');
fprintf('=========================================================================\n');