%% EXAMEN JULIO 2025 - PROBLEMA 2 (PROBLEMA A): RESOLUCIÓN COMPLETA
% Asignatura: Robótica (GITI - Escuela Superior de Ingenieros de Sevilla)
%
% Robot Plano R-P-R:
%   - Articulación 1: Rotación q1 en la base
%   - Articulación 2: Prismática q2 a lo largo de corredera perpendicular a L1
%   - Articulación 3: Rotación q3 en muñeca con herramienta L3

clear; clc;

fprintf('====================================================================\n');
fprintf('  EXAMEN JULIO 2025 - PROBLEMA 2: MANIPULADOR PLANO R-P-R           \n');
fprintf('====================================================================\n\n');

syms L1 L3 q1 q2 q3 real
PI = sym(pi);

%% 1. PARÁMETROS D-H Y MATRICES HOMOGÉNEAS
% Función elemental D-H estándar
dh = @(theta, d, a, alpha) [
    cos(theta), -cos(alpha)*sin(theta),  sin(alpha)*sin(theta), a*cos(theta);
    sin(theta),  cos(alpha)*cos(theta), -sin(alpha)*cos(theta), a*sin(theta);
             0,             sin(alpha),             cos(alpha),            d;
             0,                      0,                      0,            1
];

% Matrices elementales
A1 = dh(q1, 0, L1, PI/2);
A2 = dh(0, q2, 0, -PI/2);
A3 = dh(q3, 0, L3, 0);

% Matriz homogénea resultante T03
T03 = simplify(A1 * A2 * A3);

fprintf('1. MATRIZ DE TRANSFORMACIÓN RESULTANTE T_0^3:\n');
disp(T03);

%% 2. POSICIÓN HOME (q1=0, q2=0, q3=0)
T_HOME = simplify(subs(T03, [q1, q2, q3], [0, 0, 0]));
p_HOME = T_HOME(1:3, 4);

fprintf('POSICIÓN HOME (q1=0, q2=0, q3=0):\n');
fprintf('  x_HOME = %s\n', char(p_HOME(1)));
fprintf('  y_HOME = %s\n', char(p_HOME(2)));
fprintf('  z_HOME = %s\n\n', char(p_HOME(3)));

%% 3. PROBLEMA CINEMÁTICO DIRECTO (PCD)
x = simplify(T03(1, 4));
y = simplify(T03(2, 4));
phi = simplify(q1 + q3);

fprintf('2. PROBLEMA CINEMÁTICO DIRECTO (PCD):\n');
fprintf('  x(q1, q2, q3)   = %s\n', char(x));
fprintf('  y(q1, q2, q3)   = %s\n', char(y));
fprintf('  phi(q1, q2, q3) = %s\n\n', char(phi));

%% 4. JACOBIANO Y SINGULARIDADES
p_op = [x; y; phi];
q_vars = [q1; q2; q3];

J = jacobian(p_op, q_vars);
det_J = simplify(det(J));

fprintf('3. JACOBIANO DEL MANIPULADOR J(q):\n');
disp(J);
fprintf('DETERMINANTE DEL JACOBIANO:\n');
fprintf('  det(J) = %s\n', char(det_J));
fprintf('SINGULARIDAD:\n');
fprintf('  det(J) = 0 <==> q2 = 0 (Muñeca sobre el casquillo de corredera)\n\n');

%% 5. PROBLEMA CINEMÁTICO INVERSO (PCI) Y COMPROBACIÓN NUMÉRICA
fprintf('4. PROBLEMA CINEMÁTICO INVERSO (PCI):\n');
L1_val = 0.6;
L3_val = 0.4;

% Punto objetivo de prueba
x_des   = 0.8731;
y_des   = 0.2534;
phi_des = deg2rad(75);

fprintf('Probando PCI para el objetivo: [x=%.4f, y=%.4f, phi=%.2f deg]\n', ...
    x_des, y_des, rad2deg(phi_des));

% Paso 1: Desacoplo de muñeca
xw = x_des - L3_val * cos(phi_des);
yw = y_des - L3_val * sin(phi_des);
rw2 = xw^2 + yw^2;

if rw2 < L1_val^2
    error('Punto fuera de rango: rw^2 < L1^2');
end

% Las 2 soluciones (+ y -)
signos = [1, -1];
for s = 1:2
    sgn = signos(s);
    q2_sol = sgn * sqrt(rw2 - L1_val^2);
    q1_sol = atan2(q2_sol * xw + L1_val * yw, L1_val * xw - q2_sol * yw);
    q3_sol = phi_des - q1_sol;
    
    % Comprobación cinemática directa
    x_check = L1_val * cos(q1_sol) + q2_sol * sin(q1_sol) + L3_val * cos(q1_sol + q3_sol);
    y_check = L1_val * sin(q1_sol) - q2_sol * cos(q1_sol) + L3_val * sin(q1_sol + q3_sol);
    phi_check = q1_sol + q3_sol;
    
    err = norm([x_check - x_des, y_check - y_des, phi_check - phi_des]);
    
    fprintf('  Solución %d (signo %+d):\n', s, sgn);
    fprintf('    q1 = %8.2f deg\n', rad2deg(q1_sol));
    fprintf('    q2 = %8.4f m\n', q2_sol);
    fprintf('    q3 = %8.2f deg\n', rad2deg(q3_sol));
    fprintf('    Error cinemático: %.2e m\n\n', err);
end