function [thdot,thddot,ag,f] = fan_solve(t, th)
%
% NOTE ON USE OF NARGOUT:
% If this function is not called with all of the output arguments, then
% the extra output arguments are not computed -- no sense in finding the
% values if the caller of the function is not going to use them. For 
% example, if this function is passed to ode45 with something like
%        [t, th_num] = ode45('fan_solve',tspan,th_init,options);
% than ode45 will call fan_solve with only one output argument, the
% equivalent of you calling fan_solve like 
%        [thdot] = fan_solve(t_current, th_current);
% In that case, only one output argument has been requested, and matlab
% sets the automatic variable nargout to be equal to 1
%
% INPUT arguments:
% t  --         scalar, input, current time; never used for anything
% th --         vector of length nlink (number of moving links); angle
%               of a link measured counterclockwise from the i-direction 
% OUTPUT arguments:
% given angles of components, find:
% thdot --      vector of length nlink; angular velocities
% thddot --     vector (nlink); angular accelerations
% ag     --     matrix (2,nlink); 2 because two-dimensional vector for
%               the accelerations; linear acceleration of mass centers
% f      --     vector of length 9; 8 forces at pins and 1 unknown moment
%
% GLOBALS that have been assigned in the driver script
global r r2 rg lthg;
%    r --    vector(nlink); length of the link
%    r2 --   matrix(nlink,nlink); "squares" of links, r2(i,j)=r(i)*r(j)
%    rg --   vector(nlink); length from beginning of link to center of mass
%    lthg -- vector(nlink); angle from line of link to line from the
%            beginning of the link to the center of mass (counterclockwise)
global iwk iwu1 iwu2 ium;
%    iwk --  scalar; index for link for which angular velocity is known
%    iwu1 -- scalar; index for first link for which angular velocity is unknown
%    iwu2 -- scalar; index for second link --||-- 
%    ium  -- scalar; index for link to which unknown moment is applied
global w_data;
%    w_data -- vector(nlink); data giving known angular velocity; entry
%              iwk is the only number used in this vector
global gc g_by_gc; 
%    gc      -- scaling used when using English units
%    g_by_gc -- vector(2) giving the direction in which gravity acts
global m ai;
%    m  -- vector(nlink); masses of links
%    ai -- vector(nlink); angular inertia of links
%
nlink = 3; % number of moving links
if (length(th) ~= nlink);
      disp('error, th of wrong length');
      return;
end;
%
% GET UNIT VECTORS:
% vectors nvec are along the links and vectors tvec are perpendicular to
% the links and in the direction of motion of the end of the link when 
% the angular velocity is positive;
%
% nvec -- matrix(2,nlink)
% tvec -- matrix(2,nlink)
%
[nvec, tvec] = linkage_vec(th);
%
% solve for ANGULAR VELOCITIES;
% note that tvec(:,i) and nvec(:,i) are column vectors so that
% A becomes a (2,2) matrix and y becomes a 2-column-vector, or (2,1) matrix
%
A = [r(iwu1)*tvec(:,iwu1) r(iwu2)*tvec(:,iwu2)];
y = [w_data(iwk)*r(iwk)*tvec(:,iwk)];
x = -A\y;
%
% set values in thdot
%
thdot = zeros(3,1);
thdot(iwk)  = w_data(iwk);
thdot(iwu1) = x(1);
thdot(iwu2) = x(2);
%
% end of calculation of angular velocities
%
if (nargout > 1);
%
% find ANGULAR ACCELERATIONS
%
% get squares of angular velocities
thdot2 = thdot.*thdot;
% thdot2 -- vector(nlink)
%
A = [r(iwu1)*tvec(:,iwu1) r(iwu2)*tvec(:,iwu2)];
y = [-thdot2(iwk)* r(iwk)* nvec(:,iwk)  + ...
     -thdot2(iwu1)*r(iwu1)*nvec(:,iwu1) + ...
     -thdot2(iwu2)*r(iwu2)*nvec(:,iwu2)];
x = -A\y;
%
% set values in thddot
%
thddot = zeros(3,1);
thddot(iwk)  = 0.0;
thddot(iwu1) = x(1);
thddot(iwu2) = x(2);
%
end; % end of calculation of angular accelerations
%
if (nargout > 2);
%
% find LINEAR ACCELERATION of mass centers
%
[ag,ng] = linkage_ag(thdot2,thddot,r,rg,lthg,nvec,tvec);
%
end; % end calculation of linear acceleration of mass centers
%
%
if (nargout > 3);
%
% find FORCES AND MOMENT
%
% The code below works for a sequence of links with forces defined in the
% negative sense at the beginning of a link (connection to previous link)
% and in the position sense at the end of a link (connection to next link)
%
% We have two equations from force balance and one equation from 
% moment balance for each link
%
neq_per_link = 2+1; % number of equations per link
neq = nlink*neq_per_link; % number of total equations
%
% allow only one unknown moment; its index is imd
imd = (nlink+1)*2+1;
%
% zero necessary arrays
A = zeros(neq,neq);
b = zeros(neq,1);
f = zeros(neq,1);
%
% initialize equation counter; as entries are add to the system matrix A,
% this index will keep track of the equation number (row) to which numbers
% are being added; 
eq_num = 0;
% 
for i_link = 1:nlink; % loop over links
 %
 % equation numbers; 
 % ieq -- force equation in the i-direction for the current link
 % jeq -- force equation in the j-direction for the current link
 % meq -- moment equation (in the k-direction) for the current link
 %
 ieq = eq_num+1; jeq = eq_num+2; meq = eq_num+3; 
 %
 % increment the equation counter
 eq_num = eq_num + neq_per_link;
 %
 % force indices for ends of links
 if_base = (i_link-1)*2;
 % forces at start of link in i and j directions
 ifsi = 1 + if_base; ifsj = 2 + if_base;
 if_base = if_base + 2;
 % forces at end of link in i and j directions
 ifei = 1 + if_base; ifej = 2 + if_base;
 %
 % force equations;
 %   forces at S act in negative direction and forces at E act in 
 %   positive direction
 A(ieq,[ifsi, ifei]) = [-1.0 1.0]; 
 A(jeq,[ifsj, ifej]) = [-1.0 1.0];
 b([ieq,jeq]) =  m(i_link)*(ag(:,i_link)/gc - g_by_gc(:));
 %
 % moment equation, about beginning of link, point S
 %
 ntemp = cross_2d_ij(nvec(:,i_link));
 % ntemp is a 2-vector and you needn't worry about trying to give it a 
 % physical meaning, it is simply the vector that when dotted with the
 % force at E (and multiplied by the length of the link) gives the moment
 % about S
 %
 A(meq,[ifei,ifej]) = r(i_link)*ntemp';
 if (ium == i_link);
   %this is the link on which unknown moment is applied
   A(meq, imd) = 1.0;
 end;
 b(meq) = ai(i_link)*thddot(i_link)/gc + ...
	+m(i_link)*rg(i_link)*cross_2d(ng(:,i_link),ag(:,i_link)) + ...
	-m(i_link)*rg(i_link)*cross_2d(ng(:,i_link),g_by_gc);
 %
end; % end of loop over links
%
% finally, ready to solve
%
f = A\b;
%
end; % end of force and moment calculations
