clear all;

%lets define the lengths are vectors

pi =4.0*atan(1.0);
r1=3.2;
r2=1.7;
r3=1.7;
r4=1;
r5=1;
p4=1.6;
p1=4.0;
h2 =1;
%guess theta values
theta2 = 0.0;
theta3 = 3*pi/2;
theta4 = 5*pi/4;
theta5 = pi/2;
%guess h and f values

f3 = 1;
h3 = 1;
h4 = 2;
h5 = 3;
f3p = 1;
h3p = 1;
h4p = 1;
h5p = 1;
%enumerate matrices
x0=[theta3;theta4;theta5];
hf =[f3;h3;h4;h5];
hf_prime = [f3p;h3p;h4p;h5p];
%check
i=1;
j=1;
while j ~= 50 % unit conversion
error = 100;
while error >3
L1=r2*cos(theta2)+r3*cos(theta3)+r4*cos(theta4)-r5*cos(theta5)-r1;
L2=r2*sin(theta2)+r3*sin(theta3)+r4*sin(theta4)-r5*sin(theta5);
GC = theta4-theta3+pi/2;
Equations = [L1;L2;GC];

PartialT3= [-r3*sin(theta3); r3*cos(theta3);-1];

PartialT4= [-r4*sin(theta4); r4*cos(theta4);1];
PartialT5= [ r5*sin(theta5);-r5*cos(theta5);0];
Partials = [PartialT3 PartialT4 PartialT5];

Derivative1_2= [cos(theta3) -r3*sin(theta3) -r4*sin(theta4)

r5*sin(theta5);
sin(theta3) r3*cos(theta3) r4*cos(theta4)
-r5*cos(theta5);
0 -1 1 0;
0 0 p4 p1-p4];

Result_hf= [r2*sin(theta2); -r2*cos(theta2); 0; 0];

k=i
if i ==1
k=2;
end;
 Result_hfprime= [r2*cos(theta2)*h2^2+2*f3(k-1)*sin(theta3)*h3(k-
1)+r3*cos(theta3)*h3(k-1)^2+r4*cos(theta4)*h4(k-1)^2+r5*cos(theta5)*h5(k-1)^2;
-r2*cos(theta2)*h2^2-2*f3(k-1)*cos(theta3)*h3(k-
1)+r3*sin(theta3)*h3(k-1)^2+r4*sin(theta4)*h4(k-1)^2-r5*sin(theta5)*h5(k-1)^2;
0;
0];
%solve

x=x0-inv(Partials)*Equations;
hf =inv(Derivative1_2)*Result_hf;
hf_prime =inv(Derivative1_2)*Result_hfprime;

error = norm(x-x0)+L1-L2;
x0=x;
theta3=x(1);
theta4=x(2);

f3(i)= hf(1);
h3(i)= hf(2);
h4(i)= hf(3);
h5(i)= hf(4);
f3_prime(i)= hf_prime(1);
h3_prime(i)= hf_prime(2);
h4_prime(i)= hf_prime(3);
h5_prime(i)= hf_prime(4);

T2degrees(i)=theta2*180/pi;
T3degrees(i)=theta3*180/pi;
T4degrees(i)=theta4*180/pi;
theta2=theta2+3.6*pi/180; %increment by 3.6 degrees
j=j+1;
i=i+1;
end;
end

figure('Name','f3,h3,h4,h5 Vs Theta2');
plot(T2degrees,f3,'x',T2degrees,h3,'.',T2degrees,h4,'O',T2degrees,h5,'*');
figure('Name','f3 prime,h3 prime,h4 prime,h5 prime Vs Theta2');
plot(T2degrees,f3_prime,'x',T2degrees,h3_prime,'.',T2degrees,h4_prime,'O',T2degrees
,h5_prime,'*');