%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%											%  
%   This program computes the left dyad components of a 4-bar			        %  
%   mechanism for motion or path generation with specified ground pivots. 		%  
% 											%  
%  CONTRAINTS:										%  
%    theta_1 theta_2  (theta's will be beta's for path generation, and alpha's for      %  
%		       motion genertation)						%  
%											%  
%  GIVEN:										%  
%    R1 R2 R3	      (will be broken into R1x, R1y, R2x, R2y, R3x, R3y)                %  									  
%											%  
%  LEFT SIDE UNKNOWNS:									%  
%    Wx Wy 										%  
%    Zx Zy										%  
%    lamba_1 lambda_2   (lamba's will be alpha's for path generation, and beta's for    %  
%			 motion generation)						%  
%											%   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%       

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INPUT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
theta2=input('Enter the first contraint angle: ');
	if theta2<0   % Get positive value for first constraint angle
		theta2 = theta2 + 360.0;
	end
theta2 = theta2 * (pi/180);

theta3=input('Enter the second constraint angle: ');
	if theta3<0   % Get positive value for second constraint angle
		theta3 = theta3 + 360.0;
	end
theta3 = theta3 * (pi/180);

R1=input('Enter the magnitude of R1: ');
R1_angle=input('Enter the angle of R1: ');
R1_angle = R1_angle * (pi/180);
R1x = R1*cos(R1_angle);
R1y = R1*sin(R1_angle);

R2=input('Enter the magnitude of R2: ');
R2_angle=input('Enter the angle of R2: ');
R2_angle=R2_angle * (pi/180);
R2x = R2*cos(R2_angle);
R2y = R2*sin(R2_angle);

R3=input('Enter the magnitude of R3: ');
R3_angle=input('Enter the angle of R3: ');
R3_angle=R3_angle * (pi/180);
R3x = R3*cos(R3_angle);
R3y = R3*sin(R3_angle);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% END INPUT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SOLVING FOR D'S %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
D1x = (R3x * cos(theta2)) - (R3y * sin(theta2)) - (R2x * cos(theta3)) + (R2y * sin(theta3));
D1y = (R3x * sin(theta2)) + (R3y * cos(theta2)) - (R2x * sin(theta3)) - (R2y * cos(theta3));
theta_D1 = atan2(D1y, D1x);
D1 = sqrt((D1x^2) + (D1y^2));

D2x = -R3x + (R1x * cos(theta3)) - (R1y * sin(theta3));
D2y = -R3y + (R1x * sin(theta3)) + (R1y * cos(theta3)); 
theta_D2 = atan2(D2y, D2x);
D2 = sqrt((D2x^2) + (D2y^2));

D3x = R2x - (R1x * cos(theta2)) + (R1y * sin(theta2));
D3y = R2y - (R1x * sin(theta2)) - (R1y * cos(theta2));
theta_D3 = atan2(D3y, D3x);
D3 = sqrt((D3x^2) + (D3y^2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%% GET FIRST UNKNOWN ANGLE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lambda_2 = theta_D1 - theta_D2 + ( acos((D3^2 - D1^2 - D2^2)/(2*D1*D2)) );
fprintf('              D1: %f angle %f. \n',D1, theta_D1 * (180/pi));
fprintf('              D2: %f angle %f. \n',D2, theta_D2 * (180/pi));
fprintf('              D3: %f angle %f. \n',D3, theta_D3 * (180/pi));

if lambda_2 == theta2
	fprintf('Trivial angle: %f. \n', lambda_2 * (180/pi));
	lambda_2 = theta_D1 - theta_D2 - ( acos((D3^2 - D1^2 - D2^2)/(2*D1*D2)) );
	fprintf('     lambda_2: %f. \n', lambda_2 * (180/pi));
else
	trivial2 = theta_D1 - theta_D2 - ( acos((D3^2 - D1^2 - D2^2)/(2*D1*D2)) );
	fprintf('Trivial angle: %f. \n', trivial2 * (180/pi));
	fprintf('     lambda_2: %f. \n', lambda_2 * (180/pi));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fprintf('                               \n');
disp('Press Enter to go on...');pause
clc

%%%%%%%%%%%%%%%%%%%%%% GET SECOND UNKNOWN ANGLE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lambda_3=atan2(-D1*sin(theta_D1)-D2*sin(theta_D2+lambda_2),-D1*cos(theta_D1)-D2*cos(theta_D2+lambda_2))-theta_D3;
fprintf('lambda_3: %f. \n', lambda_3 * (180/pi));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%% SOLVE FOR UNKNOWN COMPONENTS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
coeff = zeros(4,4);		% allocating memory for coeffecient matrix or (A)
coeff = [1, 0, 1, 0; 0, 1, 0, 1; 
   cos(theta2), -sin(theta2), cos(lambda_2), -sin(lambda_2); 
   sin(theta2), cos(theta2), sin(lambda_2), cos(lambda_2)];

unknowns = zeros(4,1);		% allocating memory for unknowns matrix or (X)

rhs = zeros(4,1);		% allocating memory for constants matrix or (B)
rhs = [R1x; R1y; R2x; R2y];

unknowns = coeff\rhs;		% taking inverse of (A) and multiplying by (B)

W = sqrt(unknowns(1)^2 + unknowns(2)^2);
W_angle = atan2(unknowns(2),unknowns(1)) * (180/pi);

Z = sqrt(unknowns(3)^2 + unknowns(4)^2);
Z_angle = atan2(unknowns(4),unknowns(3)) * (180/pi);

fprintf('***********************************************\n');
fprintf('* RESULTS:					\n');
fprintf('*						\n');
fprintf('*	lambda_2: %f				\n', lambda_2 * (180/pi));
fprintf('*	lambda_3: %f				\n', lambda_3 * (180/pi));
fprintf('*						\n');
fprintf('*	      D1: %f angle %f			\n', D1, theta_D1 * (180/pi));
fprintf('*            D2: %f angle %f			\n', D2, theta_D2 * (180/pi));
fprintf('*	      D3: %f angle %f			\n', D3, theta_D3 * (180/pi));
fprintf('*						\n');
fprintf('*	       W: %f angle %f			\n', W, W_angle);
fprintf('* 	       Z: %f angle %f			\n', Z, Z_angle);
fprintf('* 						\n');
fprintf('***********************************************\n');
fprintf('\n');
fprintf('Wx: %f \n', unknowns(1));
fprintf('Wy: %f \n', unknowns(2));
fprintf('Zx: %f \n', unknowns(3));
fprintf('Zy: %f \n', unknowns(4));





 



		



