function [wn_mod,zt_mod,phi_mod,M_hat,C_hat,K_hat,B,all_data] = ritzsmod(wn,zt,phi,dm,ns_m,dk,ns_k);
% 
% Structural modification using a experimentally determined mode vectors as
% a Ritz vector basis set.
%
% Computes the modal parameters of the system modified by adding the masses
% and stiffnesses in the vectors dm and km to nodes ns_m and ns_k;
%
% [wn_mod,zt_mod,phi_mod,M_mod,C_mod,K_mod] = ritzsmod(wn,zt,phi,dm,ns_m,dk,ns_k);
%
% "ritzmod" requires the following necessary inputs:
%   wn - vector of natural frequencies for the un-modified structure.
%   zt - (optional) vector of modal damping ratios.
%   phi - matrix of mass normalized mode vectors where each column
%       corresponds to a natural frequency in wn.
%   dm - vector of masses to be added to node numbers listed in ns_m.
%   dk - vector of stiffnesses to be added to node numbers listed in ns_k.
%       Each spring is between the node listed and ground.
%
%
% Matt Allen, June 2006
%   Last Modified:  August 23, 2006

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

if length(dm) ~= length(ns_m);
    error('Lengths of dm and ns_m must be equal - list a node number for each mass addition');
end
if isempty(zt)
    zt = zeros(size(wn));
end
if length(wn) ~= length(zt) | length(wn) ~= size(phi,2);
    error('Sizes of modal parameters are not consistent.');
end
% add more error checking?
ns_m = ns_m(:);
ns_k = ns_k(:);

N = length(wn);
Nmod = length(dm) + length(dk);
Nt = N + Nmod;

% Form unconstrained Mass and Stiffness matrices
% qt = [modal_coordinates; mass mod nodes; stiff mod nodes]
% y (physical_coordinates) = phi*modal_coordinates
Mt = zeros(Nt,Nt); Ct = zeros(Nt,Nt); Kt = zeros(Nt,Nt);
Mt(1:N,1:N) = eye(N);
Ct(1:N,1:N) = diag(2*zt.*wn);
Kt(1:N,1:N) = diag(wn.^2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constraint equations and their M and K elements  (Chapter 9 of Ginsberg)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a = zeros(Nmod,Nt);
for k = 1:length(dm)
    Mt(N+k,N+k) = dm(k);
    a(k,N+k) = -1;
    a(k,1:N) = phi(ns_m(k),:);
end
for k = 1:length(dk)
    Kt(N+k+length(dm),N+k+length(dm)) = dk(k);
    a(k+length(dm),N+k+length(dm)) = -1;
    a(k+length(dm),1:N) = phi(ns_k(k),:);
end

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

% This constitutes chosing which generalized coordinates are constrained
% and unconstrained.

P = [zeros(N,Nmod), eye(N,N);
    eye(Nmod,Nmod), zeros(Nmod,N)];
    % Keep modal DOF (unconstrained) and constrain others.
    % It would be good to add error checking to see if this gives a rank
    % deficient a_c;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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;
a_c=a_hat(:,1:Nmod);
a_u=a_hat(:,(Nmod+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-Nmod)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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.
% Note - one could alternatively form M_hat and K_hat directly using
% equation (6.1.37) on p. 344 of Ginsberg.  I don't think that any
% computational advantage that one or the other has is likely to be
% significant for the numbers of modes one is likely to have when using
% this.
    
% 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.
phi_mod = phi*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);
Phi_hat = Phi_hat(:,sind);

% save ritzsmod_debugdata.mat

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