clear all; clc;
%% TIM HAINES ARM PLANNER%%
%%CSE 599 Personal Robotics%%


%%ENTER PROBLEM PARAMETERS%%
A = [0 0]; %# Origin 
B = [0 2.0]; %# center of goal node
C = [0 1.5];%Center of obsticle
Rr=0.126;% Radius of obsticle
l1 = 1.00; %# Length of base member
l2=1.00; %Length of second member
l3=0.5; %length of third member

ith1=180;  %initial angle theta 1
ith2=180;  %initial angle theta 2
ith3=180;  %Initial angle theta 3
r1 = 1.25; %# radius of the SECOND circle


%%START CALCULATIONS%%
g=0
G=20;
%%Determines the location of first arm%%
c = norm(A-B) %# distance between circles
cosAlpha = (l1^2+c^2-r1^2)/(2*l1*c);
u_AB = (B - A)/c; %# unit vector from first to second center
pu_AB = [u_AB(2), -u_AB(1)]; %# perpendicular vector to unit vector

%# use the cosine of alpha to calculate the length of the
%# vector along and perpendicular to AB that leads to the
%# intersection point
inter1 = A + u_AB * (l1*cosAlpha) + pu_AB * (l1*sqrt(1-cosAlpha^2))
inter2 = A + u_AB * (l1*cosAlpha) - pu_AB * (l1*sqrt(1-cosAlpha^2))


%%Determines the location of the second and third arm Using inter1 %%
D=inter1;
d = norm(D-B) %# distance between circles
cosAlpha1 = (l2^2+d^2-l3^2)/(2*l2*d);
u_AB1 = (B - D)/d; %# unit vector from first to second center
pu_AB1 = [u_AB1(2), -u_AB1(1)]; %# perpendicular vector to unit vector

%# use the cosine of alpha to calculate the length of the
%# vector along and perpendicular to AB that leads to the
%# intersection point
inter3 = D + u_AB1 * (l1*cosAlpha1) + pu_AB1 * (l1*sqrt(1-cosAlpha1^2))
inter4 = D + u_AB1 * (l1*cosAlpha1) - pu_AB1 * (l1*sqrt(1-cosAlpha1^2))

%%Determines the location of the second and third arm Uwing inter 2%%
D=inter2;
d = norm(D-B) %# distance between circles
cosAlpha1 = (l2^2+d^2-l3^2)/(2*l2*d);
u_AB1 = (B - D)/d; %# unit vector from first to second center
pu_AB1 = [u_AB1(2), -u_AB1(1)]; %# perpendicular vector to unit vector

%# use the cosine of alpha to calculate the length of the
%# vector along and perpendicular to AB that leads to the
%# intersection point
inter5 = D + u_AB1 * (l1*cosAlpha1) + pu_AB1 * (l1*sqrt(1-cosAlpha1^2))
inter6 = D + u_AB1 * (l1*cosAlpha1) - pu_AB1 * (l1*sqrt(1-cosAlpha1^2))
 
% % %Plot Solution 1
% figure(1)
% line([A(1) inter1(1)] ,[A(2) inter1(2)],[0 0],'Marker','.','LineStyle','-')
% line([inter1(1) inter3(1)],[inter1(2) inter3(2)],[0 0],'Marker','.','LineStyle','-')
% line([inter3(1) B(1)],[inter3(2) B(2)],[0 0],'Marker','.','LineStyle','-')
% 
% %Plot Solution 2
% figure(2)
% line([A(1) inter1(1)] ,[A(2) inter1(2)],[0 0],'Marker','.','LineStyle','-')
% line([inter1(1) inter4(1)],[inter1(2) inter4(2)],[0 0],'Marker','.','LineStyle','-')
% line([inter4(1) B(1)],[inter4(2) B(2)],[0 0],'Marker','.','LineStyle','-')
% 
% %Plot Solution 3
% figure(3)
% line([A(1) inter2(1)] ,[A(2) inter2(2)],[0 0],'Marker','.','LineStyle','-')
% line([inter2(1) inter5(1)],[inter2(2) inter5(2)],[0 0],'Marker','.','LineStyle','-')
% line([inter5(1) B(1)],[inter5(2) B(2)],[0 0],'Marker','.','LineStyle','-')
% 
% %Plot Solution 4
% figure(4)
% line([A(1) inter2(1)] ,[A(2) inter2(2)],[0 0],'Marker','.','LineStyle','-')
% line([inter2(1) inter6(1)],[inter2(2) inter6(2)],[0 0],'Marker','.','LineStyle','-')
% line([inter6(1) B(1)],[inter6(2) B(2)],[0 0],'Marker','.','LineStyle','-')





while(G>1)
g=g+1    
sol=g;%Enter 1 2 3 4 for which solution interection you would like
%%Determine Angles%%
G=0;
if(sol==1)
end
if(sol==2)
    inter3=inter4;
end
if(sol==3)
    inter1=inter2;
    inter3=inter5;
end
if (sol==4)
    inter1=inter2;
    inter3=inter6;
end
if(sol>4)
    disp('No Colision free path')
    break
end

if(B(2)>=inter3(2))&&(inter3(2)>=inter1(2))
    th1=acosd(inter1(1))
    th2=acosd(inter3(1)-inter1(1))
    th3=acosd((B(1)-inter3(1))/l3)
end
if(B(2)<inter3(2))&&(inter3(2)>=inter1(2))
    th1=acosd(inter1(1))
    th2=acosd(inter3(1)-inter1(1))
    th3a=acosd((B(1)-inter3(1))/l3)
    th3=(180-th3a)*2+th3a
end

if(B(2)>=inter3(2))&&(inter3(2)<inter1(2))
    th1=acosd(inter1(1))
    th2a=acosd(inter3(1)-inter1(1))
    th2=(180-th2a)*2+th2a
    th3=acosd((B(1)-inter3(1))/l3)
end
if(B(2)<inter3(2))&&(inter3(2)<inter1(2))
    th1=acosd(inter1(1))
    th2a=acosd(inter3(1)-inter1(1))
    th2=(180-th2a)*2+th2a
    th3a=acosd((B(1)-inter3(1))/l3)
    th3=(180-th3a)*2+th3a
end

N=200;
theta1=[ith1:(th1-ith1)/N:th1];
theta2=[ith2:(th2-ith2)/N:th2];
theta3=[ith3:(th3-ith3)/N:th3];
n=1
frameTime= 0.05; % units: s
figure('Name','Animation')
hold on
 while (n<N);
     cla
     x1(n)=cosd(theta1(n));
     y1(n)=sind(theta1(n));
     x2(n)=cosd(theta2(n))+x1(n);
     y2(n)=sind(theta2(n))+y1(n);
     x3(n)=cosd(theta3(n))/2+x2(n);
     y3(n)=sind(theta3(n))/2+y2(n);
     circle(C(1),C(2),Rr)
     plot(B(1),B(2),'go',x1(n),y1(n),'ro',x2(n),y2(n),'ro',x3(n),y3(n),'ro',[0 x1(n)],[0 y1(n)],'k',[x1(n) x2(n)],[y1(n) y2(n)],'k',[x2(n) x3(n)],[y2(n) y3(n)],'k','LineWidth',3)
     axis([-3 3 0 6])
     title('Arm Position')
     xlabel('x (m)')
     ylabel('y (m)')
     
     if (x1(n)<(C(1)+Rr))&& (x1(n)>(C(1)-Rr))&& (y1(n)<(C(2)+Rr)) && (y1(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Arm 1')
         G=5;
         break
     end
     if (0.25*x1(n)<(C(1)+Rr))&& (0.25*x1(n)>(C(1)-Rr))&& (0.25*y1(n)<(C(2)+Rr)) && (0.25*y1(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Arm 1')
         G=5;
         break
     end
     if (0.5*x1(n)<(C(1)+Rr))&& (0.5*x1(n)>(C(1)-Rr))&& (0.5*y1(n)<(C(2)+Rr)) && (0.5*y1(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Arm1')
         G=5;
         break
     end
     if (0.75*x1(n)<(C(1)+Rr))&& (0.75*x1(n)>(C(1)-Rr))&& (0.75*y1(n)<(C(2)+Rr)) && (0.75*y1(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Amr1')
         G=5;
         break
     end
     if (0.25*(x2(n)-x1(n))+x1(n)<(C(1)+Rr))&& (0.25*(x2(n)-x1(n))+x1(n)>(C(1)-Rr))&& (0.25*(y2(n)-y1(n))+y1(n)<(C(2)+Rr)) && (0.25*(y2(n)-y1(n))+y1(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Arm2')
         G=5;
         break
     end
     if (0.5*(x2(n)-x1(n))+x1(n)<(C(1)+Rr))&& (0.5*(x2(n)-x1(n))+x1(n)>(C(1)-Rr))&& (0.5*(y2(n)-y1(n))+y1(n)<(C(2)+Rr)) && (0.5*(y2(n)-y1(n))+y1(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Arm2')
         G=5;
         break
     end
     if (0.75*(x2(n)-x1(n))+x1(n)<(C(1)+Rr))&& (0.75*(x2(n)-x1(n))+x1(n)>(C(1)-Rr))&& (0.75*(y2(n)-y1(n))+y1(n)<(C(2)+Rr)) && (0.75*(y2(n)-y1(n))+y1(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Arm2')
         G=5;
         break
     end
     if (x2(n)<(C(1)+Rr))&& (x2(n)>(C(1)-Rr))&& (y2(n)<(C(2)+Rr)) && (y2(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Arm2')
         G=5;
         break
     end
     
     if (x3(n)<(C(1)+Rr))&& (x3(n)>(C(1)-Rr))&& (y3(n)<(C(2)+Rr)) && (y3(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Arm3')
         G=5;
         break
     end
     if (0.5*(x3(n)-x2(n))+x2(n)<(C(1)+Rr))&& (0.5*(x3(n)-x2(n))+x2(n)>(C(1)-Rr))&& (0.5*(y3(n)-y2(n))+y2(n)<(C(2)+Rr)) && (0.5*(y3(n)-y2(n))+y2(n)>(C(2)-Rr))
         disp('Impact with Obsticle!!! Arm3')
         G=5;
         break
     end
     n=n+1
     pause(.04)
     clc
 end
end
 disp('Done')
 plot(x1,y1,x2,y2,x3,y3)

