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


%%ENTER PROBLEM PARAMETERS%%
A = [0 0]; %# Origin 
B = [1 1.8]; %# center of goal node
C = [0.4 1.5];%[0 1.5]Center of obsticle
Rr=0.12;% 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  FOR PLANNING LEAVE AT 180
ith2=180;  %initial angle theta 2  FOR PLANNING LEAVE AT 180
ith3=180;  %Initial angle theta 3  FOR PLANNING LEAVE AT 180
ith=[ith1 ith2 ith3;
    1 1 1; 1 1 1; 1 1 1; 1 1 1];
r1 = 1.25; %# radius of the SECOND circle
%r1=1.5 will cause the third joint to not be used
%Low values of r1 will make the last joint be used more


%%START CALCULATIONS%%
g=0
G=20;


Bmain=B;












%% HYBRID LAPLACE MAP%%

Cm=10*C+[30 0];
Bm=10*B+[30 0];
M1=zeros(30,60);

M1(Cm(2),Cm(1))=1000000; 
u=Cm(2);
while(u<=30)
    M1(u,Cm(1))=9000;
    u=u+1;
end
M1(Bm(2),Bm(1))=-100000000; %Goal
M1(:,1)=10000;
M1(:,2)=10000;
M1(:,3)=1000;
M1(:,4)=1000;
M1(1,:)=500;
M1(:,60)=500;
M1(30,:)=10000;
M1(29,:)=10000;
M1(28,:)=10000;
M1(27,:)=10000;
M1(26,:)=10000;
M1(25,:)=10000;
M1(24,:)=10000;
M1(23,:)=10000;
M1(2,:)=450;
M1(3,:)=400;
M1(4,:)=350;
M1(5,:)=300;

i=29;
ii=59;%Generate Laplace
tt=1;

    
    while(i>=2)
         while(ii>=2)
              M1(i,ii)=(M1(i+1,ii)+M1(i-1,ii)+M1(i,ii+1)+M1(i,ii-1))/4;
              ii=ii-1;   
         end
     ii=59;
     i=i-1;
    end
    
i=2;
%Find path
%x,y
M1(Bm(2),Bm(1))=-1000000; %Goal
e=2;
x=[-2.5*10+30,-2.4*10+30];
y=[1,2];
while(e<=600)
    
    aa=M1(y(e),x(e)+1);
    bb=M1(y(e),x(e)-1);
    cc=M1(y(e)+1,x(e));
    dd=M1(y(e)-1,x(e));
    ff=M1(y(e)+1,x(e)+1);
    gg=M1(y(e)-1,x(e)+1);
    hh=M1(y(e)+1,x(e)-1);
    jj=M1(y(e)-1,x(e)-1);
    ee=M1(y(e),x(e));
    e=e+1;
    
    if(aa<bb)&&(aa<cc)&&(aa<dd)%&&(aa<ff)&&(aa<gg)&&(aa<hh)&&(aa<jj)%&&(aa<ee);
        x(e)=x(e-1)+1;
        y(e)=y(e-1);
        
        M1(y(e),x(e-1))=10000000;
      
    end
    if(bb<aa)&&(bb<cc)&&(bb<dd)%&&(bb<ff)&&(bb<gg)&&(bb<hh)&&(bb<jj)%&&(bb<ee);
        x(e)=x(e-1)-1;
        y(e)=y(e-1);
        M1(y(e-1),x(e-1))=10000000;
    end
    if(cc<aa)&&(cc<bb)&&(cc<dd)%&&(cc<ff)&&(cc<gg)&&(cc<hh)&&(cc<jj)%&&(cc<ee);
        x(e)=x(e-1);
        y(e)=y(e-1)+1;
        M1(y(e-1),x(e-1))=10000000;
    end
    if(dd<aa)&&(dd<bb)&&(dd<cc)%&&(dd<ff)&&(dd<gg)&&(dd<hh)&&(dd<jj)%&&(dd<ee);
        x(e)=x(e-1);
        y(e)=y(e-1)-1;
        M1(y(e-1),x(e-1))=10000000;
    end

    if(ee==-1000000)
        break
    end
end
plot(x,y)

li=[0]
mm=1;
while(mm<length(x)/10)
    li(mm)=mm*10;
    mm=mm+1
end

%%SELECTING GOALS!!!%%
B1=[x(12)/10-3 y(12)/10];
B2=[x(22)/10-3 y(22)/10];
B3=[x(32)/10-3 y(32)/10];
B4=[x(42)/10-3 y(42)/10];



Goals=[B1;B2;B3;B4;B]








lp=1;

%% SOLUTIONS!!!!$$
while(lp<=5)
    B=Goals(lp,:);



%%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))
 





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
ith(lp+1,:)=[th1 th2 th3];
N=50;
theta1=[ith(lp,1):(th1-ith(lp,1))/N:th1];
theta2=[ith(lp,2):(th2-ith(lp,2))/N:th2];
theta3=[ith(lp,3):(th3-ith(lp,3))/N:th3];
n=1;
frameTime= 0.05; % units: s
figure('Name','Animation')
hold on
cla
 while (n<N);
     cla
     fill([3 -3 -3 3],[-3 -3 0 0],'y')
     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(Bmain(1),Bmain(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 -3 3])
     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
     if (y1(n)<0)
         disp('Impact joint 1 with ground')
         G=5;
         break
     end
     if(y2(n)<0)
         disp('Impact joint 2 with ground')
         G=5;
         break
     end
     if(y3(n)<0)
         disp('Impact joint 3 with ground')
         G=5;
         break
     end
     n=n+1
     pause(.04)
     clc
 
end
 disp('Done')
 plot(x1,y1,x2,y2,x3,y3)

lp=lp+1;
end