% Test script for ritzsmod m-file.
%
% Simulates the change in the first few modal frequencies and mode shapes
% of a free-free beam when a mass and inertia is added to one end using 
% ritzscomb.m and compares the result with that obtained using the full
% 82DOF-element FE model.
%
% 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).

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);

% Check Results:  Compute fn for Euler-Beam with same properties
% fn for 3 foot beam
    N = length(s1ind) + length(s2ind);
    % 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).';
    % Natural frequencies from FE Model
    wns3_fe = data.C.wn(1:N);
    
    BeamA_mass_lb = rho*Area*(L)*(32.2*12)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
End_mass_lb = dm(1)*32.2*12
% Remove mass from smaller beam modal model
    [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],[],[]);
    
% Sensitivity method to compute shift in natural frequency
% delta_omega = -0.5*delta_m*phi(p)^2*omega_orig
    sens_dw_disp = -0.5*(-dm(1))*(data.Aml.wn(s1ind_ml).').*(data.Aml.phi(11,s1ind_ml).').^2;
    sens_dw_rot = -0.5*(-dm(2))*(data.Aml.wn(s1ind_ml).').*(data.Aml.phir(11,s1ind_ml).').^2;
    % Add the shifts from the mass and inertia to compute the total
    % frequency shift:
    wn_sens = data.Aml.wn(s1ind_ml).' + sens_dw_disp + sens_dw_rot;
    
% Compare Natural Frequencies
disp('fns (in frequency range) of subsystems before and after removing masses');
disp('A (mass loaded), A (FF Full FE), A (FF Est Ritz), % Err Ritz, A (FF Sens), % Err Sens');
view_vecs(data.Aml.wn(s1ind_ml)/2/pi, data.A.wn(s1ind_ml)/2/pi, wn1_ml2/2/pi,...
    pct_err(data.A.wn(s1ind_ml).',wn1_ml2),wn_sens/2/pi, ...
    pct_err(data.A.wn(s1ind_ml).',wn_sens));

% Indices in mode vectors for plots
x0_mod_inds = [1:size(data.Aml.phi,1)];
x0_mod_inds_rot = [size(data.Aml.phi,1)+1:size(phi1_ml2,1)];

% All mode shapes on one figure:
plt_modes = [3:5]; % Only shows results of a few modes at a time, otherwise the plot is too crowded.
figure(4)
subplot(2,1,1)
hls = plot(data.A.x0, phi1_ml2(x0_mod_inds,plt_modes)*diag(sign(phi1_ml2(1,plt_modes)))); hold on;
plot(data.A.x0, data.A.phi(:,plt_modes)*diag(sign(data.A.phi(1,plt_modes))),'o:');
plot(data.A.x0, data.Aml.phi(:,plt_modes)*diag(sign(data.Aml.phi(1,plt_modes))),'--');
set(get(gca,'Children'),'LineWidth',2); hold off;
ylabel('\bfDisplacement'); grid on;
legend(hls, num2str(plt_modes.'));
title('\bfMode Shapes - 1 ft Beam - (-) FE (--) ML (o:) ML Mass Removed');
subplot(2,1,2)
hls = plot(data.A.x0, phi1_ml2(x0_mod_inds_rot,plt_modes)*diag(sign(phi1_ml2(1,plt_modes)))); hold on;
plot(data.A.x0, data.A.phir(:,plt_modes)*diag(sign(data.A.phi(1,plt_modes))),'o:');
plot(data.A.x0, data.Aml.phir(:,plt_modes)*diag(sign(data.Aml.phi(1,plt_modes))),'--');
set(get(gca,'Children'),'LineWidth',2); hold off;
xlabel('\bfPosition (X)'); ylabel('\bfRotation'); grid on;
sdaxsetb([0,14])

