%% =========================================================================
%% EXAMEN OFICIAL ROBOTICA - CONVOCATORIA 29 DE OCTUBRE DE 2025 (P2)
%% MANIPULADOR PRR (1 PRISMATICA VERTICAL + 2 ROTACIONALES)
%% Grado en Ingenieria de Tecnologias Industriales - E.S.I. Sevilla
%% =========================================================================
% Script analitico, simbolico y numerico para resolver completamente:
% a) Dibujo de ejes y Tabla DH del manipulador
% b) Cinematica Directa T_0^3 y posicion del extremo (x, y, z)
% c) Cinematica Inversa analitica: todas las soluciones posibles
% d) Jacobiano geometrico, determinante y analisis de singularidades
% =========================================================================

clear; clc; close all;

fprintf('=========================================================================\n');
fprintf('  RESOLUCION OFICIAL: EXAMEN GITI OCTUBRE 2025 - PROBLEMA 2 (PRR)\n');
fprintf('=========================================================================\n\n');

%% 1. DEFINICION DE VARIABLES SIMBOLICAS Y CONSTANTES
syms q1 q2 q3 real          % Variables articulares: q1 (m, prism), q2 (rad, rot), q3 (rad, rot)
syms L1 L2 L3 real positive % Longitudes y cotas geometricas (m)
PI = sym(pi);               % Constante simbolica exacta para evitar redondeos

%% 2. APARTADO A: MATRICES DENAVIT-HARTENBERG SEGUN CONVENCION ESTANDAR
% Tabla DH:
% Link 1 (Prismatica vertical along Z0):
%   theta1 = 0,   d1 = L1 + q1,  a1 = 0,   alpha1 = 0
% Link 2 (Rotacional vertical along Z1):
%   theta2 = q2,  d2 = 0,        a2 = L2,  alpha2 = +PI/2
% Link 3 (Rotacional horizontal along Z2):
%   theta3 = q3,  d3 = 0,        a3 = L3,  alpha3 = 0

fprintf('--- APARTADO A: MATRICES DE TRANSFORMACION HOMOGENEA A_i ---\n');

% Matriz DH estandar inline para garantizar ejecucion autosuficiente
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(0, L1 + q1, 0, 0);
A2 = dh(q2, 0, L2, PI/2);
A3 = dh(q3, 0, L3, 0);

fprintf('Matriz A_1 (Eslabon 1, Prismatica):\n');
disp(A1);

fprintf('Matriz A_2 (Eslabon 2, Rotacional Base):\n');
disp(A2);

fprintf('Matriz A_3 (Eslabon 3, Rotacional Elevacion):\n');
disp(A3);

%% 3. APARTADO B: CINEMATICA DIRECTA T_0^3 Y POSICION DEL EXTREMO (x, y, z)
fprintf('\n--- APARTADO B: MATRIZ T_0^3 Y COORDENADAS CARTESIANAS (x, y, z) ---\n');

T02 = simplify(A1 * A2);
T03 = simplify(A1 * A2 * A3);

fprintf('Matriz T_0^3 completa:\n');
disp(T03);

% Extraccion del vector de posicion cartesiana del extremo:
p = simplify(T03(1:3, 4));
x = p(1);
y = p(2);
z = p(3);

fprintf('Ecuaciones de Cinematica Directa (Posicion del extremo):\n');
fprintf('  x(q1, q2, q3) = %s\n', char(x));
fprintf('  y(q1, q2, q3) = %s\n', char(y));
fprintf('  z(q1, q2, q3) = %s\n\n', char(z));

% Comprobacion en la posicion de reposo / HOME: q = [0, 0, 0]'
p_home = subs(p, [q1, q2, q3], [0, 0, 0]);
fprintf('Comprobacion en HOME (q1=0, q2=0, q3=0):\n');
fprintf('  x_home = %s  (Debe ser L2 + L3)\n', char(p_home(1)));
fprintf('  y_home = %s  (Debe ser 0)\n', char(p_home(2)));
fprintf('  z_home = %s  (Debe ser L1)\n\n', char(p_home(3)));

%% 4. APARTADO D: JACOBIANO GEOMETRICO, DETERMINANTE Y SINGULARIDADES
% Calculamos el Jacobiano de posicion lineal respecto a las 3 variables: J_v = [dp/dq1, dp/dq2, dp/dq3]
fprintf('--- APARTADO D: JACOBIANO GEOMETRICO Y SINGULARIDADES ---\n');

q_vec = [q1; q2; q3];
Jv = jacobian(p, q_vec);

fprintf('Matriz Jacobiana de Velocidad Lineal J_v (3x3):\n');
disp(Jv);

det_Jv = simplify(det(Jv));
fprintf('Determinante del Jacobiano det(J_v):\n');
fprintf('  det(J_v) = %s\n\n', char(det_Jv));

fprintf('ANALISIS ALGEBRAICO Y FISICO DE SINGULARIDADES (det(J_v) = 0):\n');
fprintf('El determinante se factoriza exactamente como:\n');
fprintf('  det(J_v) = L3 * (L2 + L3*cos(q3)) * sin(q3) = 0\n\n');

fprintf('1) SINGULARIDAD DE FRONTERA DEL ESPACIO DE TRABAJO (sin(q3) = 0):\n');
fprintf('   a) q3 = 0 rad (0 deg): Brazo extendido horizontalmente (r_max = L2 + L3).\n');
fprintf('      Eslabones 2 y 3 alineados horizontalmente. Pierde velocidad radial instantanea dr.\n');
fprintf('   b) q3 = pi rad (180 deg): Brazo replegado horizontalmente hacia atras (r_min = |L2 - L3|).\n');
fprintf('      Eslabon 3 doblado sobre el 2. Pierde velocidad radial instantanea dr.\n\n');

fprintf('2) SINGULARIDAD DE CRUCE DEL EJE DE ROTACION DE LA BASE (L2 + L3*cos(q3) = 0):\n');
fprintf('   Ocurre cuando cos(q3) = -L2 / L3 (factible fisicamente si L3 >= L2).\n');
fprintf('   Fisicamente: El extremo cruza exactamente el eje vertical de giro Z0 (r = 0, x=0, y=0).\n');
fprintf('   En este punto, una velocidad angular qd2 no produce velocidad lineal en el extremo (v = w x r = 0).\n\n');

%% 5. APARTADO C: CINEMATICA INVERSA ANALITICA COMPLETA
fprintf('--- APARTADO C: MODELO CINEMATICO INVERSO ANALITICO ---\n');
fprintf('Dado el punto objetivo cartesiano Pe = [x; y; z], determinar [q1; q2; q3]:\n\n');

fprintf('1. Orientacion de la base (q2):\n');
fprintf('   A partir del plano horizontal: x = r*cos(q2), y = r*sin(q2)\n');
fprintf('   q2 = atan2(y, x)   (Rama directa, r > 0)\n\n');

fprintf('2. Angulo de elevacion (q3):\n');
fprintf('   Radio horizontal: r = sqrt(x^2 + y^2) = L2 + L3*cos(q3)\n');
fprintf('   cos(q3) = (sqrt(x^2 + y^2) - L2) / L3\n');
fprintf('   Condicion de existencia en espacio de trabajo: -1 <= cos(q3) <= 1\n');
fprintf('   sin(q3) = +- sqrt(1 - cos(q3)^2)\n');
fprintf('   q3 = atan2(sin(q3), cos(q3))\n\n');
fprintf('   Existen DOS soluciones para q3:\n');
fprintf('     - Rama 1 (Codo Arriba): sin(q3) = +sqrt(1 - cos(q3)^2)  --> q3 >= 0\n');
fprintf('     - Rama 2 (Codo Abajo):  sin(q3) = -sqrt(1 - cos(q3)^2)  --> q3 <= 0\n\n');

fprintf('3. Desplazamiento prismatico vertical (q1):\n');
fprintf('   De la ecuacion vertical: z = L1 + q1 + L3*sin(q3)\n');
fprintf('   q1 = z - L1 - L3*sin(q3)\n');
fprintf('   - Para Rama 1: q1_1 = z - L1 - L3*sin(q3_1)\n');
fprintf('   - Para Rama 2: q1_2 = z - L1 - L3*sin(q3_2)\n\n');

%% 6. COMPROBACION NUMERICA CRUZADA DE LA CINEMATICA INVERSA
fprintf('--- COMPROBACION NUMERICA DE VALIDACION CRUZADA ---\n');
% Valores numericos de prueba:
L1_num = 2.0; L2_num = 3.5; L3_num = 3.0;
x_test = 4.0; y_test = 3.0; z_test = 5.0;

fprintf('Punto objetivo fijado: P_des = [%.2f; %.2f; %.2f] m\n', x_test, y_test, z_test);
fprintf('Parametros: L1=%.2f, L2=%.2f, L3=%.2f m\n\n', L1_num, L2_num, L3_num);

r_test = sqrt(x_test^2 + y_test^2);
cos_q3_num = (r_test - L2_num) / L3_num;

if abs(cos_q3_num) <= 1
    % Rama 1: Codo Arriba
    sin_q3_1 = sqrt(1 - cos_q3_num^2);
    q3_sol1 = atan2(sin_q3_1, cos_q3_num);
    q2_sol1 = atan2(y_test, x_test);
    q1_sol1 = z_test - L1_num - L3_num * sin(q3_sol1);
    
    % Rama 2: Codo Abajo
    sin_q3_2 = -sqrt(1 - cos_q3_num^2);
    q3_sol2 = atan2(sin_q3_2, cos_q3_num);
    q2_sol2 = q2_sol1;
    q1_sol2 = z_test - L1_num - L3_num * sin(q3_sol2);
    
    fprintf('SOLUCION 1 (Codo Arriba):\n');
    fprintf('  q1 = %.4f m\n', q1_sol1);
    fprintf('  q2 = %.4f rad (%.2f deg)\n', q2_sol1, rad2deg(q2_sol1));
    fprintf('  q3 = %.4f rad (%.2f deg)\n', q3_sol1, rad2deg(q3_sol1));
    
    % Verificacion directa Solucion 1:
    x_rec1 = (L2_num + L3_num*cos(q3_sol1))*cos(q2_sol1);
    y_rec1 = (L2_num + L3_num*cos(q3_sol1))*sin(q2_sol1);
    z_rec1 = L1_num + q1_sol1 + L3_num*sin(q3_sol1);
    fprintf('  Verificacion P_rec1: [%.4f, %.4f, %.4f] m  --> Error: %.2e m\n\n', ...
        x_rec1, y_rec1, z_rec1, norm([x_rec1-x_test, y_rec1-y_test, z_rec1-z_test]));
    
    fprintf('SOLUCION 2 (Codo Abajo):\n');
    fprintf('  q1 = %.4f m\n', q1_sol2);
    fprintf('  q2 = %.4f rad (%.2f deg)\n', q2_sol2, rad2deg(q2_sol2));
    fprintf('  q3 = %.4f rad (%.2f deg)\n', q3_sol2, rad2deg(q3_sol2));
    
    % Verificacion directa Solucion 2:
    x_rec2 = (L2_num + L3_num*cos(q3_sol2))*cos(q2_sol2);
    y_rec2 = (L2_num + L3_num*cos(q3_sol2))*sin(q2_sol2);
    z_rec2 = L1_num + q1_sol2 + L3_num*sin(q3_sol2);
    fprintf('  Verificacion P_rec2: [%.4f, %.4f, %.4f] m  --> Error: %.2e m\n\n', ...
        x_rec2, y_rec2, z_rec2, norm([x_rec2-x_test, y_rec2-y_test, z_rec2-z_test]));
else
    fprintf('¡El punto se encuentra fuera del espacio de trabajo!\n');
end

fprintf('=========================================================================\n');
fprintf('  COMPROBACION COMPLETADA EXITOSAMENTE SIN ERRORES\n');
fprintf('=========================================================================\n');