function out = fp6(theta)

%Constants
L1 = 3;
L2 = 3*sqrt(2);
L3 = 3;
gamma = pi/4;
p1 = 5;
p2 = 8; %2 poses: p2=4, 8, 9
p3 = 3;
x1 = 5;
x2 = 0;
y2 = 6;

%Computed Constants based on theta
A2 = L3*cos(theta) - x1;
B2 = L3*sin(theta);
A3 = L2*cos(theta + gamma) - x2;
B3 = L2*sin(theta + gamma) - y2;

%Computing N1, N2, D
D = 2*(A2*B3 - B2*A3);
N1 = B3*(p2^2 - p1^2 - A2^2 - B2^2) - B2*(p3^2 - p1^2 - A3^2 - B3^2);
N2 = -A3*(p2^2 - p1^2 - A2^2 - B2^2) + A2*(p3^2 - p1^2 - A3^2 - B3^2);

%Computed Constants Level 3
x = (B3*(p2^2 - p1^2 -A2^2 - B2^2) - B2*(p3^2 - p1^2 - A3^2 - B3^2))/(2*(A2*B3 - B2*A3));
y = (-A3*(p2^2 - p1^2 - A2^2 - B2^2) + A2*(p3^2 - p1^2 - A3^2 - B3^2))/(2*(A2*B3 - B2*A3));

%Final Result
out = N1.^2 + N2.^2 - (p1.^2).*D.^2;

