x_6 = (-1 * pi):0.1:pi;
y_6 = gamli2(x_6);

finalFunc = @(x) (((3*sqrt(2)).*sin(x + pi/4) - 6).*(4.^2 - 5.^2 - (3.*cos(x) - 5).^2 - (3.*sin(x)).^2) - (3.*sin(x)).*(3.^2 - 5.^2 - ((3*sqrt(2)).*cos(x + pi/4) - 0).^2 - ((3*sqrt(2)).*sin(x + pi/4) - 6).^2)).^2 + (-1*((3*sqrt(2)).*cos(x + pi/4) - 0).*(4.^2 - 5.^2 - (3.*cos(x) - 5).^2 - (3.*sin(x)).^2) + (3.*cos(x) - 5).*(3.^2 - 5.^2 - ((3*sqrt(2)).*cos(x + pi/4) - 0).^2 - ((3*sqrt(2)).*sin(x + pi/4) - 6).^2)).^2 - 5.^2 * (2 * ((3.*cos(x) - 5).*((3*sqrt(2)).*sin(x + pi/4) - 6) - (3.*sin(x)).*((3*sqrt(2)).*cos(x + pi/4) - 0))).^2;
uncty = 10.^(-14);

root1 = bisect(finalFunc, 1, 1.6, uncty);
root2 = bisect(finalFunc, 1.6, 2, uncty);


figure(1)
plot(x_6,y_6,'k')
grid on
hold on
plot(root1,finalFunc(root1), 'bo')
plot(root2,finalFunc(root2), 'bo')

[x_r1, y_r1] = xy_values(root1)
[x_r2, y_r2] = xy_values(root2)

figure(2)
%Root 1
subplot(1,2,1)
plot([x_r1 (x_r1+(l3.*cos(root1))) (x_r1+(l2.*cos(root1 + gamma))) x_r1],[y_r1 (y_r1+(l3.*sin(root1))) (y_r1+(l2.*sin(root1 + gamma))) y_r1],'r'); hold on
plot([0 x1 x2],[0 0 y2],'bo');
plot([0 x_r1], [0 y_r1],'b');
plot([5 (x_r1+(l3.*cos(root1)))], [0 (y_r1+(l3.*sin(root1)))],'b');
plot([0 (x_r1+(l2.*cos(root1 + gamma)))], [6 (y_r1+(l2.*sin(root1 + gamma)))],'b');
plot(x_r1, y_r1,'bo');
grid on
xlabel('x')
ylabel('y')
title('Theta 1')
hold off;

root1_p1lengd = sqrt((x_r1).^2 + (y_r1).^2)
root1_p2lengd = sqrt((x1-(x_r1+(l3.*cos(root1)))).^2 + ((y_r1+(l3.*sin(root1)))).^2)
root1_p3lengd = sqrt((x2-(x_r1+(l2.*cos(root1 + gamma)))).^2 + (y2-(y_r1+(l2.*sin(root1 + gamma)))).^2)

%Root 2
subplot(1,2,2)
plot([x_r2 (x_r2+(l3.*cos(root2))) (x_r2+(l2.*cos(root2 + gamma))) x_r2],[y_r2 (y_r2+(l3.*sin(root2))) (y_r2+(l2.*sin(root2 + gamma))) y_r2],'r'); hold on
plot([0 x1 x2],[0 0 y2],'bo');
plot([0 x_r2], [0 y_r2],'b');
plot([5 (x_r2+(l3.*cos(root2)))], [0 (y_r2+(l3.*sin(root2)))],'b');
plot([0 (x_r2+(l2.*cos(root2 + gamma)))], [6 (y_r2+(l2.*sin(root2 + gamma)))],'b');
plot(x_r2, y_r2,'bo');
grid on
xlabel('x')
ylabel('y')
title('Theta 2')
hold off;

root2_p1lengd = sqrt((x_r2).^2 + (y_r2).^2)
root2_p2lengd = sqrt((x1-(x_r2+(l3.*cos(root2)))).^2 + ((y_r2+(l3.*sin(root2)))).^2)
root2_p3lengd = sqrt((x2-(x_r2+(l2.*cos(root2 + gamma)))).^2 + (y2-(y_r2+(l2.*sin(root2 + gamma)))).^2)