% Four-Bar Mechanism Initial Configuration 
% K. Craig 2022

a = 0.4;
b = 1.20;
c = 0.80;
d = 1.0;
theta2 = 40*pi/180;

K1 = d/a;
K2 = d/c;
K3 = (a*a - b*b + c*c + d*d)/(2*a*c);
K4 = d/b;
K5 = (c*c - d*d - a*a - b*b)/(2*a*b);
Q = cos(theta2);
A = K3 - K1 - (K2-1)*Q;
B = -2*sin(theta2);
C = K3 + K1 - (K2+1)*Q;
D = K5 - K1 + (K4+1)*Q;
E = B;
F = K5 + K1 + (K4-1)*Q;
theta4 = 2*atan2(-B - sqrt(B*B - 4*A*C),2*A);
theta3 = 2*atan2(-E - sqrt(E*E - 4*D*F),2*D);
theta4 = theta4*180/pi;
theta3 = theta3*180/pi;
theta4 = 360 + theta4;
theta3 = 360 + theta3;
theta3
theta4