function [wn_mod,zt_mod,phi_mod,subsys_ind,coord_ind,M_hat,C_hat,K_hat,all_data] = ...
    ritzscomb(subsys,carray,sgn_vec);
% 
% Structural modification using a experimentally determined mode vectors as
% a Ritz vector basis set.  Combines substructures (add or subtract)
% defined by modal parameters using the user supplied constraints and
% returns modal parameters for master structure.
%
% [wn_mod,zt_mod,phi_mod,subsys_ind,coord_ind,M_hat,C_hat,K_hat,all_data] = ... 
%       ritzscomb(subsys,carray,sgn_vec);
%
% INPUTS:
%   subsys - N_sub dimensional array with the following fields for each
%   substructure:  (i.e) subsys(1).wn = wn vector for subsystem #1.
%       wn - vector of natural frequencies for the un-modified structure.
%       zt - (optional) vector of modal damping ratios - set to zero
%           if not supplied.
%       phi - matrix of mass normalized mode vectors where each column
%           corresponds to a natural frequency in wn.
%       mmass - (optional) vector of modal masses - set to one if not
%           supplied.  (Set these to zero for residual flexibilities.)
%       names - (optional) vector of node names for subsystem.  These are
%           concatenated into a vector of names for phi_mod if supplied.
%
%   carray - 3D array containing the constraints between subsystems.  Each
%       constraint is a 3-column matrix with the following columns:
%       carray(:,:,C_num) = [a1, pt1, subsys1;
%                           a2, pt2, subsys2;
%                           ...];
%       where a1 are the coefficients in an equation of the form:
%           a1*y1+a2*y2+...=0,
%       subsys1 is the index of the subsystem being joined (y1 is on
%           subsys1),
%       and pt1 is the index of the node in subsys1 that is being joined.
%       For Example:  To assign node 3 on subsystem 2 to have the same
%       displacement as node 4 on subsystem 1, the following matrix would
%       be used:  carray = [1, 3, 2; -1, 4, 1]
%           NOTE:  This assumes the coordinate systems are identical for
%           each component - use care if this is not the case!!
%       ##### SEE mkconstr for alternative #####
%
%   sgn_vec - [optional] vector of length N_sub of +1 or -1 values if each
%       substructure is being added or subtracted respectively to the
%       master system.  All substructures are added if this is not
%       supplied.
%
% OUTPUTS:
%   Modal params of combined system:  wn_mod,zt_mod,phi_mod
%   The rows in phi_mod correspond to a master set of coordinates
%   including each subsystem stacked sequentially.  Thus, the coordinates
%   that have been joined will be repeated in this vector.
%   subsys_ind - vector telling which subsystem each node belongs to.
%   coord_ind - vector telling which coordiate each node was in its
%       original subsystem.
%   M,C,K matrices for combined system in modal coordinates.
%   phi_all matrix to transfer from modal to physical coordinates
%
% Matt Allen, June 2006
%   Last Modified Aug. 23, 2006

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Note: All numbered equations are in reference to Ginsberg, "Mechanical
% and Structural Vibrations: Theory and Applications", Wiley, 2001
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

N_sub = length(subsys);
% Add damping values of zero if not supplied
if ~isfield(subsys,'zt')
    for k = 1:N_sub;
        subsys(k).zt = zeros(size(subsys(k).wn));
    end
end
% Add unit modal masses if not supplied by user
if ~isfield(subsys,'mmass')
    for k = 1:N_sub;
        subsys(k).mmass = ones(size(subsys(k).wn));
    end
end
% Error checking on sizes of wn, zt, phi and mmass
for k = 1:N_sub;
    if length(subsys(k).wn) ~= length(subsys(k).zt) | ...
            length(subsys(k).wn) ~= size(subsys(k).phi,2) | ...
            length(subsys(k).wn) ~= length(subsys(k).mmass);
        error(['Hey Good Lookin, Sizes of modal parameters are not consistent \n',...
            'for Subsys # ',num2str(k),', Call me when its fixed.'],1);
    end
    Nses(k) = length(subsys(k).wn); % Number of DOF for each subsystem
    Nsout(k) = size(subsys(k).phi,1); % Number of outputs in each subsystem
end
if nargin < 3;
    sgn_vec = ones(N_sub,1);
end
if length(sgn_vec) ~= N_sub; error('Length of sgn_vec ~= Number subsystems'); end
N_const = size(carray,3);
if size(carray,2) ~= 3; error('Hey Bozo, can''t you read?! Constraint array must have 3 columns'); end
% Check that all subsystems referenced in carray exist:
if any(vec(carray(:,3,:)) > N_sub) | any(vec(carray(:,3,:)) < 0)
    error('Hey you!  ''carray'' references subsystems that you haven''t supplied in subsys!');
end
% Check that all points in each constraint exist in the subsystem.
for cnum = 1:N_const
    for rnum = 1:size(carray(:,:,cnum),1);
        if carray(rnum,2,cnum) > size(subsys(carray(rnum,3,cnum)).phi,1) | ...
                carray(rnum,2,cnum) < 0;
            error(['You Silly Goose!  ''carray'' references a point that doesn''t exist at: ',...
                num2str([rnum,2,cnum])]);
        end
    end
end
if isfield(subsys,'names');
    phi_all_names = [];
    for k = 1:N_sub;
        if length(subsys(k).names) ~= Nsout(k);
            error('Length of Node Names vector not consistent with size of mode matrix');
        end
        for kout = 1:Nsout(k);
            phi_all_names{sum(Nsout(1:k-1))+kout} = subsys(k).names{kout};
        end
    end
    phi_all_names = phi_all_names.';
end
% add more error checking?

Nt = sum(Nses); % Length of combined system before applying constraints.

% Form unconstrained Mass and Stiffness matrices
% qt = [modal_coordinates_sys1; modal_coord_sys2; etc...]
% y (physical_coordinates) = phi*modal_coordinates
Mt = zeros(Nt,Nt); Ct = zeros(Nt,Nt); Kt = zeros(Nt,Nt);
for k = 1:N_sub
    ind_vec = (sum(Nses(1:k-1))+1:sum(Nses(1:k)));
    Mt(ind_vec,ind_vec) = sgn_vec(k)*diag(subsys(k).mmass);
    Ct(ind_vec,ind_vec) = sgn_vec(k)*diag(2*subsys(k).zt.*subsys(k).wn);
    Kt(ind_vec,ind_vec) = sgn_vec(k)*diag((subsys(k).wn).^2);
    subsys(k).ind_vec = ind_vec; % Store indices to generate constraints later.
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constraint equations (Chapter 9 of Ginsberg, p. 533)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a = zeros(N_const,Nt);
for cnum = 1:N_const
    for rnum = 1:size(carray(:,:,cnum),1);
        % Assign component to a matrix for each constraint coefficient
        % a(indicies) = acoeff*(row of phi_subsys for coordinate)
        a(cnum,subsys(carray(rnum,3,cnum)).ind_vec) = a(cnum,subsys(carray(rnum,3,cnum)).ind_vec) + ...
            carray(rnum,1,cnum)*subsys(carray(rnum,3,cnum)).phi(carray(rnum,2,cnum),:);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define sorting matrix "P". Chapter 9 textbook, Eq.(9.1.23)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% This constitutes chosing which generalized coordinates are constrained
% and unconstrained. {q} = P*[qc; qu] - constrained on top.

% Might this be numerically sensitive, or is it just an issue of making
% sure a_c is numerically invertible?

% ac_mode = 'last'
% warning('RitzSComb is using the last mode, not SVD algorithm');
ac_mode = 'svd';
if strcmp(ac_mode,'svd');
    [Ua,Sa,Va] = svd(a);
    P = Va; % this gives a_hat = a*P = U*S => largest sv's in leading cols.
    % Hopefully this method will not be sensitive to noise.  One could also
    % try using the modes that one trusts the most.
    
    % save ritz_debugdata.mat
else
    % Initial pass, constrain last modal DOF of each set.  This could be
    % upgraded by trying various things and choosing the one that gives the
    % best conditioning in a_c.
    %   This may not be the best solution for experimental data - the last mode
    %   might be the least accurate?
    %   Could we solve for Psort_ind to maximize the rank of a_c?
    %   Or, could we start with the lowest modes and work up until the rank of
    %   a_c is acceptable?  Other ideas? - Ask Vit Babuska or Keith Miller -
    %   how is this done in CMS?

    Psort_ind = []; nc_psub = ceil(N_sub/N_const);
    for ssnum = 1:N_sub
        % Assign indicies of last mode in each subsystem to beginning of
        % Psort_ind so it will be constrained.
        Psort_ind = [subsys(ssnum).ind_vec((end-[nc_psub-1:-1:0])), Psort_ind, ...
            subsys(ssnum).ind_vec(1:end-nc_psub)];
        % Note: this moves some modes to the beginning that aren't constrained,
        % but that shouldn't affect anything.
    end
    Psort_ind(1:nc_psub*N_sub) = Psort_ind(nc_psub*N_sub:-1:1);
    P = eye(Nt);
    P = P(:,Psort_ind);
        % One could use this procedure to resort below and eliminate the P
        % matrix to make this somewhat more computationally efficient.
     disp('You may want to consider other choices for unconstrained gen coords.');
     disp('See the comments in this code.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Find the sorted constraint matrix "a_hat" and the matrix "B", Eq.(9.1.25)
% Eq.(9.1.26) and Eq.(9.1.28)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

a_hat=a*P; % or a(Psort_ind);
a_c=a_hat(:,1:N_const);
a_u=a_hat(:,(N_const+1):end);

rc_a_c = rcond(a_c);
if rc_a_c < 1e-10; warning(['Condition number of a_c is ',num2str(rc_a_c)]); end
B=P*[-a_c\a_u; eye(Nt-N_const)]; % or B = [-ac/au; eye(Nt-N_const)]; B = B(Psort_ind);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Form the "hat" matrices according to Eq.(9.1.32) & (9.1.33)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% note:  eom:  M_hat*{q_u_dd} + C_hat*{q_u_d} + K_hat*{q_u} = B.'*{Q}
M_hat=B.'*Mt*B;
C_hat=B.'*Ct*B;
K_hat=B.'*Kt*B;

disp(['Condition numbers of M_hat, K_hat: ',num2str([cond(M_hat), cond(K_hat)],3)]);
    % Note - could also solve for the frequency response directly without
    % eliminating variables - see eq. 9.1.35, p. 545.

% Note - Should also verify that M_hat and K_hat are still positive
% definite - or let the user figure that out from the new modal parameters.

% Find new modal parameters
[phi_hat,wn_mod] = eig(K_hat,M_hat);
wn_mod = sqrt(diag(wn_mod));
    % note - this phi_hat is in terms of the old modal coordinates defined
    % by y = phi*old_modal_coords, where y are the measurement points.

% normalize phi - (Matlab may do this automatically - this added to be safe.)
Phi_hat = phi_hat*diag(sqrt(diag(phi_hat.'*M_hat*phi_hat).^-1));

% Find Damping ratios for new system assuming the viscous modal damping
% matrix is unmodified.
ptCp = Phi_hat.'*C_hat*Phi_hat;
    if max(max(abs(ptCp))) > 0; % don't do this check if original zts are zero.
    disp('Largest off diagonal in Phi_hat.''*C_hat*Phi_hat divided by max on diagonal:');
    disp(max(max(ptCp-diag(diag(ptCp))))/max(max(abs(ptCp))));
    end
zt_mod = diag(ptCp)./(2*wn_mod + (wn_mod==0));
    % zt_mod(find(isnan(zt_mod))) = 0;

% find Phi_hat in terms of physical coordinates.
for k = 1:N_sub
    subsys(k).oind_vec = (sum(Nsout(1:k-1))+1:sum(Nsout(1:k)));
    phi_all(subsys(k).oind_vec,subsys(k).ind_vec) = subsys(k).phi;
    subsys_ind(subsys(k).oind_vec) = k*ones(size(subsys(k).oind_vec));
    coord_ind(subsys(k).oind_vec) = [1:Nsout(k)].';
end
phi_allB = phi_all*B; % Eliinate constrained DOF
phi_mod = phi_allB*Phi_hat; % in terms of measurement points y.

% Sort everything - why didn't Matlab do it with eig?
[wn_mod,sind] = sort(wn_mod);
zt_mod = zt_mod(sind);
phi_mod = phi_mod(:,sind);

if nargout > 8;
    all_data = [];
    S = whos;
    for k = 1:length(S)
        eval(['all_data.',S(k).name,' = ',S(k).name,';']);
    end
end