clear all;close;clc;

% Longitudes de los eslabones del robot (en cm)

l1 = 0.25;
l2 = 0.25;
l3 = 0.25;
q1=0;
q2=0;
q3=0;
% Coordenadas del punto objetivo (en cm)
t=0:0.01:30;
h=.35;
k=.35;
K1=5;
K2=2;
w=1/3.195;
r=0.01;
px=h+r.*(16.*sin(w*t).*sin(w*t).*sin(w*t));
py=k+r.*(13*cos(w*t)-5*cos(2*w*t)-2*cos(3*w*t)-cos(4*w*t));

% t = linspace(0, 2*pi, 1000);

% px = 0.4 + 0.2*cos(t).^3;
% py = 0.2 + 0.2*sin(t).^3;

plot(px,py);
hold on; grid on;
clear t;
t=linspace(0,15,length(py)); %% Modifique

% Calcular el radio y el ángulo phi

kappa = atan2(py, px);
D = sqrt(px.^2 + py.^2); % Hipotenusa

cx3 = px - l3.*cos(kappa);
cy3 = py - l3.*sin(kappa);
c = sqrt(cx3.^2 + cy3.^2);

gamma=acos(((l1^2)+(l2^2)-c.^2)./(2*l1*l2));
kappa = atan2(py,px);

q2d=-gamma+pi; % Codo abajo

beta=acos(((l1^2)+(c.^2)-l2^2)./(2.*l1.*c));
q1d=kappa - beta;

q3d = kappa - q1d - q2d;

% q1d=real(q1d);
% q2d=real(q2d);
% q3d=real(q3d);
% Ajustar los ángulos al rango de -pi a pi
% q1d = mod(q1d + pi, 2*pi) - pi;
% q2d = mod(q2d + pi, 2*pi) - pi;
% q3d = mod(q3d + pi, 2*pi) - pi;

% Convertir a grados
q1=q1d;
q2=q2d;
q3=q3d;

% Cinemática directa
x1 = l1*cos(q1);
y1 = l1*sin(q1);
x2 = x1 + l2*cos(q1 + q2);
y2 = y1 + l2*sin(q1 + q2);
x3 = x2 + l3*cos(q1 + q2 + q3);
y3 = y2 + l3*sin(q1 + q2 + q3);

% Animación del robot

P1 = [0;0];
P2 = [x1;y1];
P3 = [x2;y2];
P4 = [x3;y3];
% Animacion del robot
f = figure;
set(f,'color','w');
title('Controlador JSC - SCARA');
xlabel('X (cm)');
ylabel('X (cm)');
axis('equal');
s=max(l1+l2+l3);
axis([-1.5*s 1.5*s -1.5*s 1.5*s]);
grid on

% Graficar la trayectoria del efector final

hold on;
plot(P2(1,:),P2(2,:),'color','y','LineWidth',2);
hold on;
plot(P3(1,:),P3(2,:),'color','r','LineWidth',2);
hold on;
plot(P4(1,:),P4(2,:),'color','g','LineWidth',2);
% Definir el espacio de trabajo
t1=0:0.01:2*pi;
w=pi;
cx1=(l1+l2+l3)*cos(w*t1);
cy1=(l1+l2+l3)*sin(w*t1);
cx2=abs(l1-(l2+l3))*cos(w*t1);
cy2=abs(l1-(l2+l3))*sin(w*t1);
plot(cx1,cy1,'color',...
[0.6,0.6,0.6],'LineWidth',2);
% hold on;
% plot(cx2,cy2,'color',...
% [0.6,0.6,0.6],'LineWidth',2);
% hold on;

% Animacion del robot final

for i=1:length(t);
% Dibujar los nodos
P1_c=viscircles(P1',0.05);
P2_c=viscircles(P2(:,i)',0.05);
P3_c=viscircles(P3(:,i)',0.05);
P4_c=viscircles(P4(:,i)',0.05);
% Dibujar los eslabones
eslabon1=line([P1(1) P2(1,i)],...
[P1(2) P2(2,i)],...
'color','b','LineWidth',1.5);
eslabon2=line([P2(1,i) P3(1,i)],...
[P2(2,i) P3(2,i)],...
'color','b','LineWidth',1.5);
eslabon3=line([P3(1,i) P4(1,i)],...
 [P3(2,i) P4(2,i)],...
'color','b','LineWidth',1.5);
% Retardo
pause(1e-4);
% stop motion
if i<length(t);
delete(eslabon1);
delete(eslabon2);
delete(eslabon3);
delete(P1_c);
delete(P2_c);
delete(P3_c);
delete(P4_c);
end

end