clc 
clear

syms L1 L2 q1 q2 a b R

[q1,q2] = solve('L1*sin(q1) + L2*cos(q2-q1) = a - R*cos(q2 - q1)','-L1*cos(q1) - L2*sin(q2-q1) = b + R*sin(q2 - q1)' , 'q1', 'q2');

q1_simplificada = simple(q1);
q2_simplificada = simple(q2); 


%%%%%%%%%%%%%%%%%%       PRUEBAS         %%%%%%%%%%%%%%%%%%%%%%%%%%

% LAS PRUEBAS SE REALIZAN EQUIVALENTEMENTE A COMO ES EL PROGRAMA EN C++
% ES DECIR: SE PASA LA POSICION (centro) DEL SREF DE LA BASE AL SREF 1,
% SE CALCULA LA POSICION DE APROX, Y SE VULEVE A PASAR ESTA ULTIMA AL SREF
% DE LA BASE

% Sustituyendo tal que entremos un poco por encima y muy a la izquierda:
% OBJETO EN EL PUNTO 0.24,-0.28 RESPECTO AL SREF DE LA BASE
centy=0.24-0.026; % seria 0.24 y -0.28 respecto a la base y centy centz respecto al Sref 1 --> SE PONE ASI PARA QUE SEA IGUAL QUE EL PROGRAMA EN C++
centz=-0.28+0.055; %Luego se vuleve a pasar a la base al final en el vpa()
disp('1) OBJETO EN EL PUNTO 0.24,-0.28 RESPECTO AL SREF DE LA BASE:')
disp('q1:')
var=subs(q1_simplificada,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03]) % subs() sustituye los valores simbolicos
q1_selec=var(1);
disp('q2:')
var=subs(q2_simplificada,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03])
q2_selec=var(1);
y= a + R * cos(pi + q1_selec - q2_selec);
z= b + R * sin(pi + q1_selec - q2_selec);
disp('Y:')
vpa(0.026 + subs(y,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03]))   % vpa() para que muestre el resultado en decimales
disp('Z:')
vpa(-0.055 + subs(z,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03]))   

disp('-----------------------------------------------------')


% Sustituyendo tal que entremos por encima y muy poco a la izquierda:
% OBJETO EN EL PUNTO 0.03,-0.37 RESPECTO AL SREF DE LA BASE
centy=0.03-0.026;
centz=-0.37+0.055;
disp('2) OBJETO EN EL PUNTO 0.03,-0.37 RESPECTO AL SREF DE LA BASE:')
disp('q1:')
var=subs(q1_simplificada,[L1,L2,a,b,R],[0.2078,0.17566,centy,centz,0.03]) % subs() sustituye los valores simbolicos
q1_selec=var(1);
disp('q2:')
var=subs(q2_simplificada,[L1,L2,a,b,R],[0.2078,0.17566,centy,centz,0.03])
q2_selec=var(1);
y= a + R * cos(pi + q1_selec - q2_selec);
z= b + R * sin(pi + q1_selec - q2_selec);
disp('Y:')
vpa(0.026 + subs(y,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03]))   % vpa() para que muestre el resultado en decimales
disp('Z:')
vpa(-0.055 + subs(z,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03]))   

disp('-----------------------------------------------------')



% Sustituyendo tal que entremos por encima y muy poco a la izquierda:
% OBJETO EN EL PUNTO 0,-0.38 RESPECTO AL SREF DE LA BASE
centy=0-0.026;
centz=-0.38+0.055;
disp('3) OBJETO EN EL PUNTO 0,-0.38 RESPECTO AL SREF DE LA BASE:')
disp('q1:')
var=subs(q1_simplificada,[L1,L2,a,b,R],[0.2078,0.17566,centy,centz,0.03]) % subs() sustituye los valores simbolicos
q1_selec=var(2);
disp('q2:')
var=subs(q2_simplificada,[L1,L2,a,b,R],[0.2078,0.17566,centy,centz,0.03])
q2_selec=var(2);
disp('y<=0  --> Ahora las soluciones correctas de q1 y q2 pasan a ser las de la 2ª ecuacion (var[2])')
y= a + R * cos(pi + q1_selec - q2_selec);
z= b + R * sin(pi + q1_selec - q2_selec);
disp('Y:')
vpa(0.026 + subs(y,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03]))   % vpa() para que muestre el resultado en decimales
disp('Z:')
vpa(-0.055 + subs(z,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03]))   

disp('-----------------------------------------------------')

% Sustituyendo tal que entremos por encima y muy poco a la izquierda:
% OBJETO EN EL PUNTO -0.01,-0.37 RESPECTO AL SREF DE LA BASE
centy=-0.01-0.026;
centz=-0.37+0.055;
disp('4) OBJETO EN EL PUNTO -0.01,-0.37 RESPECTO AL SREF DE LA BASE:')
disp('q1:')
var=subs(q1_simplificada,[L1,L2,a,b,R],[0.2078,0.17566,centy,centz,0.03]) % subs() sustituye los valores simbolicos
q1_selec=var(2);
disp('q2:')
var=subs(q2_simplificada,[L1,L2,a,b,R],[0.2078,0.17566,centy,centz,0.03])
q2_selec=var(2);
disp('y<=0  --> Ahora las soluciones correctas de q1 y q2 pasan a ser las de la 2ª ecuacion (var[2])')
y= a + R * cos(pi + q1_selec - q2_selec);
z= b + R * sin(pi + q1_selec - q2_selec);
disp('Y:')
vpa(0.026 + subs(y,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03]))   % vpa() para que muestre el resultado en decimales
disp('Z:')
vpa(-0.055 + subs(z,[L1,L2,a,b,R],[0.209,0.178,centy,centz,0.03]))   

disp('-----------------------------------------------------')
