% Test script for ritzscomb m-file.
%
% Simulates joining a 12" and 24" beam using the Ritz method and all modes
% below 6400 Hz and compares the result with that obtained using full
% Finite Element models of the individual beams.  The result of using 
% free-free modes, and mass loaded modes for the subsystems are compared.
%
% Required subroutines:  view_vecs.m, pct_err.m, sdaxsetb.m
% Also requires the FE data set
%
% Matt Allen, Aug 2006
%

clear all; close all;

load FE_Data_B40epf.mat
% Contains data structure with modal parameters of A, B and C systems and
% with mass-loaded modal parameters for A and B (Aml, Bml).

% % Plotting
% figure(1)
% subplot(2,1,1)
% plot(data.A.x0,data.A.phi(:,1:10)); grid on;
% xlabel('Position (in)'); ylabel('Mode Function');
% title('Mode Shapes: Beam A (free-free) in isolation');
% subplot(2,1,2)
% plot(data.A.x0,data.A.phir(:,1:10)); grid on;
% xlabel('Position (in)'); ylabel('Rotation');

fmax = 6400;
disp(['Using Only Modes below ',num2str(fmax),' Hz!!']);
s1ind = find(data.A.wn < fmax*2*pi); s2ind = find(data.B.wn < fmax*2*pi);

subsys(1).wn = data.A.wn(s1ind); subsys(1).phi = [data.A.phi(:,s1ind); data.A.phir(:,s1ind)];
subsys(2).wn = data.B.wn(s2ind); subsys(2).phi = [data.B.phi(:,s2ind); data.B.phir(:,s2ind)];
carray(:,:,1) = [1, 11, 1; -1, 1, 2];% Equal displacement
carray(:,:,2) = [1, 22, 1; -1, 22, 2]; % Equal rotation
sgn_vec = [1; 1];

[wn_mod,zt_mod,phi_mod,subsys_ind,coord_ind,M_hat,C_hat,K_hat,phi_all] = ...
       ritzscomb(subsys,carray,sgn_vec);

% Check Results:
% fn for 3 foot beam
    N = length(wn_mod);
    % Find Alpha Values
        ce = inline('cos(x)*cosh(x)-1');
        an0 = [4.73,(2*[4:(N)]-3)/2*pi];
        for k = 1:length(an0);
            an(k+2) = fzero(ce,an0(k));
        end
    % Beam Parameters:
    L = 1*12; % length (in)
        L2 = 2*12; % Second Beam length (in)
    h = 0.75;      % height (in)
    b = 1;    % width (in)
    % Steel Properties from Morton - Machine Design
        E = 30.0e6; %lb/in^2
        rho = 0.28/(32.2*12); %lb-s^2/in^4
        Eorho = E/rho; % E/Rho (in^2/s^2)
    Iarea = b*h^3/12; % in^4
    Area = b*h; % in^2
    wns3_an = (sqrt(Eorho*Iarea/Area)*(1/(L+L2))^2*an.^2).';
    
    wns3_an = data.C.wn(1:length(wn_mod));
    
disp('fns in frequency band of subsystems (free-free modes)');
view_vecs(data.A.wn(s1ind)/2/pi, data.B.wn(s2ind)/2/pi);
disp('fn FE, fn RitzSComb, % Error');
fn_comp = real(view_vecs(wns3_an/2/pi, wn_mod/2/pi));
[fn_comp, pct_err(fn_comp(:,1),fn_comp(:,2))]

nm_plot = max(find(wn_mod < 6400*2*pi));
x0_mod_inds = [1:11,24:43];
x0_mod_inds_rot = setdiff([1:size(phi_mod,1)],x0_mod_inds);
    x0_mod_inds_rot = x0_mod_inds_rot([1:11,14:end]); % skip repeated points

% figure(2)
% subplot(2,1,1)
% hls = plot(data.C.x0, phi_mod(x0_mod_inds,[1:nm_plot]));
% xlabel('Position (X/L)'); ylabel('Displacement'); grid on;
% legend(hls,num2str([1:nm_plot].'));
% title('Mode Shapes: Combined Beams Using Free-Free Modes');
% subplot(2,1,2)
% plot(data.C.x0, phi_mod(x0_mod_inds_rot,[1:nm_plot]));
% xlabel('Position (X/L)'); ylabel('Rotation'); grid on; leg_rop 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% In this section, the Ritz models for the two beams are used to predict
% the modes of the beam that would be measured if a mass and inertia were
% added to the end of the beam.
s1ind_ml = find(data.Aml.wn < fmax*2*pi); s2ind_ml = find(data.Bml.wn < fmax*2*pi);

% Load data with mass loaded modes of beam and remove mass
dm = [1, (4.6^2+1^2)/12+1*0.5^2]*(4.6*rho); % beam plus paralell axis to edge
% Remove mass from smaller beam modal model - could do this in one step.
    [wn1_ml2,zt1_ml2,phi1_ml2] = ritzsmod(data.Aml.wn(s1ind_ml),[],...
        [data.Aml.phi(:,s1ind_ml); data.Aml.phir(:,s1ind_ml)],...
        -dm,[11,22],[],[]);

% Remove mass from longer beam modal model - could do this in one step.
    [wn2_ml2,zt2_ml2,phi2_ml2] = ritzsmod(data.Bml.wn(s2ind_ml),[],...
        [data.Bml.phi(:,s2ind_ml); data.Bml.phir(:,s2ind_ml)],...
        -dm,[1,22],[],[]);

% Combine subsystems
subsys(1).wn = wn1_ml2; subsys(1).zt = zt1_ml2; subsys(1).phi = phi1_ml2;
subsys(2).wn = wn2_ml2; subsys(2).zt = zt2_ml2; subsys(2).phi = phi2_ml2;
[wn_mod2,zt_mod2,phi_mod2] = ritzscomb(subsys,carray,sgn_vec);

% Check Results:
% fn for 3 foot beam
disp('fns (in frequency range) of subsystems before and after removing masses');
disp('A (mass loaded), A (mass removed), B (mass loaded), B (mass removed)');
view_vecs(data.Aml.wn(s1ind_ml)/2/pi,wn1_ml2/2/pi, data.Bml.wn(s2ind_ml)/2/pi, wn2_ml2/2/pi);
disp('fn FEA, fn RitzSComb, fn RitzSComb ML, % Error, % Error ML');
fn_comp = real(view_vecs(wns3_an/2/pi, wn_mod/2/pi, wn_mod2/2/pi));
[fn_comp, pct_err(fn_comp(:,1),fn_comp(:,2)), pct_err(fn_comp(:,1),fn_comp(:,3))]
disp('Note: Combined system natural frequencies and mode shapes are predicted');
disp('more accurately using mass-loaded modes.');

% Export to Excel if desired
    % xlswrite('SSData.xls',[fn_comp, pct_err(fn_comp(:,1),fn_comp(:,2)),
    % pct_err(fn_comp(:,1),fn_comp(:,3))]);

% figure(3)
% subplot(2,1,1)
% hls = plot(data.C.x0, phi_mod2(x0_mod_inds,[1:nm_plot]));
% xlabel('Position (X/L)'); ylabel('Displacement'); grid on;
% legend(hls,num2str([1:nm_plot].'));
% title('Mode Shapes - Combined Beams Using Mass-Loaded Modes');
% subplot(2,1,2)
% plot(data.C.x0, phi_mod2(x0_mod_inds_rot,[1:nm_plot]));
% xlabel('Position (X/L)'); ylabel('Rotation'); grid on; leg_rop 


% All mode shapes on one figure:
plt_modes = [3:5];
figure(4)
subplot(2,1,1)
hls = plot(data.C.x0, data.C.phi(:,plt_modes)*diag(sign(data.C.phi(1,plt_modes)))); hold on;
plot(data.C.x0, phi_mod2(x0_mod_inds,plt_modes)*diag(sign(phi_mod2(1,plt_modes))),'o:');
plot(data.C.x0, phi_mod(x0_mod_inds,plt_modes)*diag(sign(phi_mod(1,plt_modes))),'-.');
set(get(gca,'Children'),'LineWidth',2); hold off;
ylabel('Displacement'); grid on;
legend(hls, num2str(plt_modes.'));
title('Mode Shapes - 3 ft Beam - (-) True FE (-.) FF (o:) ML');
subplot(2,1,2)
plot(data.C.x0, data.C.phir(:,plt_modes)*diag(sign(data.C.phi(1,plt_modes)))); hold on;
plot(data.C.x0, phi_mod2( x0_mod_inds_rot,plt_modes)*diag(sign(phi_mod2(1,plt_modes))),'o:');
plot(data.C.x0, phi_mod(x0_mod_inds_rot,plt_modes)*diag(sign(phi_mod(1,plt_modes))),'-.');
set(get(gca,'Children'),'LineWidth',2); hold off;
xlabel('Position (X)'); ylabel('Rotation'); grid on;

% Observation - the free-free modes do not predict the mode shapes as well
% because they all have zero rotation at the connection point!