%% =========================================================================
% EXAMEN ROBÓTICA GITI - PRIMERA CONVOCATORIA (8 JUNIO 2025) - PROBLEMA 1
% =========================================================================
% Cuestiones Teóricas y Aplicadas:
% 1. Programa Generador de Trayectorias Rectas Cartesianas (10 puntos) -> (qr, qdr, qddr)
% 2. Transformación Homogénea T_0_1 en Prisma Triangular (Rampa 4x2x3)
% 3. Demostración del Desacoplo Dinámico por Cajas Reductoras (r >> 1)
% =========================================================================

clear; clc; close all;
fprintf('=================================================================\n');
fprintf('  EXAMEN ROBÓTICA GITI - JUNIO 2025 - PROBLEMA 1 (TEORÍA/CUESTIONES)\n');
fprintf('=================================================================\n\n');

%% CUESTIÓN 1: GENERADOR DE TRAYECTORIAS RECTAS CARTESIANAS (10 PUNTOS)
fprintf('--- CUESTIÓN 1: GENERADOR DE TRAYECTORIAS CARTESIANAS (RECTA 10 PUNTOS) ---\n');

% Definición de puntos cartesianos inicial y final:
pi_cart = [0.2; 0.3; 0.1];
pf_cart = [0.6; -0.1; 0.5];
N = 10; % Número de puntos
T_total = 4.0; % Duración total de la maniobra [s]
t = linspace(0, T_total, N)';

% Perfil de avance temporal s(t) con interpolador cúbico (velocidad inicial y final nulas):
% s(t) = 3*(t/T)^2 - 2*(t/T)^3
tau = t / T_total;
s   = 3*tau.^2 - 2*tau.^3;
sd  = (6*tau - 6*tau.^2) / T_total;
sdd = (6 - 12*tau) / (T_total^2);

% Trayectoria cartesiana de posición, velocidad y aceleración:
P_cart   = zeros(N, 3);
V_cart   = zeros(N, 3);
A_cart   = zeros(N, 3);
for k = 1:N
    P_cart(k, :) = (pi_cart + s(k) * (pf_cart - pi_cart))';
    V_cart(k, :) = (sd(k) * (pf_cart - pi_cart))';
    A_cart(k, :) = (sdd(k) * (pf_cart - pi_cart))';
end

fprintf('10 Puntos Cartesianos interpolados:\n');
disp(table(t, P_cart(:,1), P_cart(:,2), P_cart(:,3), 'VariableNames', {'t_seg', 'X', 'Y', 'Z'}));

% Función para mapear a espacio articular (ejemplo ilustrativo de bucle):
% qr = zeros(N, 3); qdr = zeros(N, 3); qddr = zeros(N, 3);
% for k = 1:N
%     qr(k, :)   = cinematica_inversa(P_cart(k, :));
%     J          = calcular_jacobiano(qr(k, :));
%     qdr(k, :)  = (J \ V_cart(k, :)')';
%     Jdot_qdot  = calcular_termino_aceleracion(qr(k, :), qdr(k, :));
%     qddr(k, :) = (J \ (A_cart(k, :)' - Jdot_qdot))';
% end

%% CUESTIÓN 2: TRANSFORMACIÓN T_0_1 (RAMPA 4x2x3)
fprintf('\n--- CUESTIÓN 2: TRANSFORMACIÓN HOMOGÉNEA T_0_1 (RAMPA) ---\n');

% Geometría de la rampa:
dx = 4; dy = 2; dz = 3;
L_hip = sqrt(dx^2 + dz^2); % 5
cos_theta = dx / L_hip;    % 4/5 = 0.8
sin_theta = dz / L_hip;    % 3/5 = 0.6

% Posición del marco {1} respecto a {0}:
p_01 = [dx; dy; dz];

% Vectores unitarios de los ejes del marco {1} expresados en {0}:
% y1: desciende por la rampa -> [-cos_theta, 0, -sin_theta]'
y1 = [-cos_theta; 0; -sin_theta];
% x1: normal a la rampa hacia el exterior -> [-sin_theta, 0, cos_theta]'
x1 = [-sin_theta; 0; cos_theta];
% z1: eje transversal derecho -> [0; -1; 0]' o [0; 1; 0]' según regla mano derecha
% Para un sistema dextrógiro: z1 = cross(x1, y1)
z1 = cross(x1, y1);

R_01 = [x1, y1, z1];
T_01 = [R_01, p_01;
        0, 0, 0, 1];

fprintf('Vector de traslación p_01: [%.1f, %.1f, %.1f]^T\n', p_01);
fprintf('Matriz de Rotación R_01 (ortonormal, det=%.1f):\n', det(R_01));
disp(R_01);
fprintf('Matriz de Transformación Homogénea T_01:\n');
disp(T_01);

%% CUESTIÓN 3: DEMOSTRACIÓN DEL DESACOPLO DINÁMICO
fprintf('--- CUESTIÓN 3: DEMOSTRACIÓN DEL DESACOPLO DINÁMICO ---\n');
fprintf('Relación de transmisión R = diag(r1, ..., rn) con ri >> 1 (e.g. ri = 100).\n');
fprintf('M_eq = J_m + R^(-1) * M(q) * R^(-1)  -->  diag(J_m1, ..., J_mn).\n');
fprintf('Los términos cruzados caen como 1/(ri*rj) ~ 10^(-4), desacoplando los ejes en SISO.\n');