% Program 3.6
% R-TRR
% Velocity and acceleration analysis
% Derivative method

clear; clc; close;

fprintf('Results \n') ; fprintf('\n');
fprintf('Velocity and acceleration analysis \n');
fprintf('Derivative method \n'); fprintf('\n');

AC = 0.1 ; BC = 0.30 ; xA = 0 ; yA = 0 ; xC = AC ; yC = 0 ; 
n = 30 ; omega = n*pi/30; 

t = sym('t','real') ;

phi = sym('phi(t)') ;
xB = sym('xB(t)') ; 
yB = sym('yB(t)') ;
 
eqB1 = tan(phi) - yB/xB ;
eqB2 = ( xB - xC )^2 + ( yB - yC )^2 - BC^2 ;

sp = {'phi(t)','xB(t)','yB(t)'} ;
np = {pi/4,'xBn','yBn'} ;

eqB1p = subs(eqB1,sp,np) ;
eqB2p = subs(eqB2,sp,np) ;
solBp = solve(eqB1p, eqB2p) ;
xBpositions = eval(solBp.xBn) ; 
yBpositions = eval(solBp.yBn) ;
xB1 = xBpositions(1); xB2 = xBpositions(2) ;
yB1 = yBpositions(1); yB2 = yBpositions(2) ;
if yB1 > 0
    xBp = xB1; yBp = yB1;
else
    xBp = xB2; yBp = yB2;
end
rB = [xBp yBp 0];
fprintf('rB =  [ %g, %g, %g ] (m)\n', rB) ;

fp = {pi/4,xBp,yBp} ;

% velocity of B2=B3
deqB1 = diff(eqB1,t) ;
deqB2 = diff(eqB2,t) ;

sv = {diff('phi(t)',t),diff('xB(t)',t),diff('yB(t)',t)} ;
nv = {omega,'vxB','vyB'} ;

deqB1p=subs(deqB1,sv,nv) ;
deqB1n=subs(deqB1p,sp,fp) ;
deqB2p=subs(deqB2,sv,nv) ;
deqB2n=subs(deqB2p,sp,fp) ;

solvB = solve(deqB1n, deqB2n) ;
vBx = eval(solvB.vxB) ;
vBy = eval(solvB.vyB) ;
fprintf('vB2 = vB3 = [ %g, %g, %g ] (m/s)\n', [vBx vBy 0]) ;

fv = {omega,vBx,vBy} ;

% acceleration of B2=B3
ddeqB1 = diff(deqB1,t) ;
ddeqB2 = diff(deqB2,t) ;

sa = {diff('phi(t)',t,2),diff('xB(t)',t,2),diff('yB(t)',t,2)};
na = {0,'axB','ayB'};

ddeqB1p=subs(ddeqB1,sa,na) ;
ddeqB1n=subs(ddeqB1p,sv,fv) ;
ddeqB1f=subs(ddeqB1n,sp,fp) ;

ddeqB2p=subs(ddeqB2,sa,na) ;
ddeqB2n=subs(ddeqB2p,sv,fv) ;
ddeqB2f=subs(ddeqB2n,sp,fp) ;

solaB = solve(ddeqB1f, ddeqB2f) ;
aBx = eval(solaB.axB) ;
aBy = eval(solaB.ayB) ;
fprintf('aB2 = aB3 = [ %g, %g, %g ] (m/s^2)\n', [aBx aBy 0]);

fa = {0,aBx,aBy};

% angular velocity and acceleration of link 3

phi3 = atan((yB-yC)/(xB-xC)) ; 
phi3n = subs(phi3,sp,fp) ;
fprintf('phi3 = %g (degrees) \n', double(phi3n*180/pi)) ;

dphi3 = diff(phi3,t) ;  
dphi3nn = subs(dphi3,sv,fv) ;
dphi3n  = subs(dphi3nn,sp,fp) ;
fprintf('omega3 = %g (rad/s) \n', double(dphi3n)) ;

ddphi3 = diff(dphi3,t) ; 
ddphi3nnn = subs(ddphi3,sa,fa) ;
ddphi3nn = subs(ddphi3nnn,sv,fv) ;
ddphi3n  = subs(ddphi3nn,sp,fp) ;
fprintf('alpha3 = %g (rad/s^2) \n', double(ddphi3n)) ; 

plot([xA,xBp],[yA,yBp],'r',[xBp,xC],[yBp,yC],'b'),...
text(xA,yA,' A'), text(xBp,yBp,' B'), text(xC,yC,' C'), grid