% Project 2
% ENGR 3590
% Alexandru Popoviciu

%clc
%clear all
%close all

function [t3o,t4o,t3c,t4c,pxo,pyo,pxc,pyc]=fourBarSim(r1,r2,r3,r4,rp,beta,t20,t2f,alpha,n)
% define link lengths (known constants)
r1 = 10;    % ground
r2 = 6;     % input
r3 = 8;     % coupler
r4 = 10;    % output
rp = 4;     % triangle
beta = 30*pi/180;   % triangle angle (deg to rad)

% define input angles
t20 = 0;    % initial angle (deg to rad)
t2f = 360*pi/180;   % final angle (deg to rad)

% define simulation parameters
alpha = 0;      % 0 = no sim, 1 = open 4-bar sim, 2 = crossed 4-bar sim
n = 30;     % number of elements in each vector


theta1 = linspace(0,360,n)*pi/180;

    if alpha==0
        for i=1:30

            A = 2*r4*(r1-r2*cos(t20));
            B = -2*r2*r4*sin(t20);
            C = r3^2-r2^2-r4^2-r1^2+2*r2*r1*cos(t20);

            u4 = (B+(A^2+B^2-C^2)^.5)/(C+A);
            t4o = 2*atan(u4);
            t4c = -2*atan(u4);

            c3 = (-r2*cos(t20)+r4*cos(t4o)+r1)/r3;
            s3 = (-r2*sin(t20)+r4*sin(t4o))/r3;
            t3o = atan2(s3,c3);
            t3c = -atan2(s3,c3);

            pxo = r2*cos(t20)+rp*cos(beta+t3o);
            pyo = r2*sin(t20)+rp*sin(beta+t3o);

        end
    end
end