%% Use the results from the Model_with_2parallel_BRDs.nb
% based on the model from Norna T1600259
% MK November 2019

clc; clear all; close all;
 
% BS and damper parameters
% primary oscillator with damping
 
% BS Bounce mode
BS.bounce.f = 16.69; %fixed bounce mode
BS.bounce.m = 27.78; % in [kg] for the k computation
BS.bounce.Q = 4400;

% BS Roll mode
BS.roll.f = 24.34; %fixed roll mode
BS.roll.m = 12.9; % in [kg] for the k computation
BS.roll.Q = 4300;

% Dampers BRD1 and BRD3
brd1.bounce.m = 4.936e-3;% brd bounce mass in [kg] for the k computation
brd1.bounce.Q = 100.4;
brd1.roll.m = 3.063e-3;% brd roll mass in [kg] for the k computation
brd1.roll.Q = 118;

brd2.bounce.m = 4.865e-3;% brd bounce mass in [kg] for the k computation
brd2.bounce.Q = 154;
brd2.roll.m = 3.207e-3;% brd roll mass in [kg] for the k computation
brd2.roll.Q = 137;

%% 1 = BS, 2 = damper BRD1, 3 = damper BRD3

mode = 'roll';

% BS
f1 = BS.(mode).f; %fixed bounce mode
m1 = BS.(mode).m; % in [kg] for the k computation

% = 1e12; % effectively infinite
Q1 = BS.(mode).Q;
w1 = 2*pi*f1; %fixed
b1 = m1*w1/Q1;
gamma1 = b1/m1;
  
% damper1
m2 = brd1.(mode).m;% brd damper mass in [kg] for th k computation
mu = m2/m1; %actual 
 
f2 = f1; % perfect tuning
f2 = 24.26;
%f2 = 16.693;
w2= 2*pi*f2;
k2 = m2*w2^2; 
 
Q2 = brd1.(mode).Q;
b2 = m2*w2/Q2; 
gamma2 = b2/m2; 

% damper 2
m3 = brd2.(mode).m;% brd damper mass in [kg] for th k computation
mu = m3/m1; %actual 
 
f3 = f1; % perfect tuning
f3 = 24.32+0.1;
%f3 = 16.66;
w3= 2*pi*f3;
k3 = m3*w3^2; 
 
Q3 = brd2.(mode).Q;
b3 = m3*w3/Q3; 
gamma3 = b3/m3; 

%% Compute the transfer function

freq = linspace(10,50,1e5);
w=2*pi*freq;
 
%double oscillator, both modes with damping
%num1/den2 gives x1/x0
num1 = [w1^2,...
       (gamma2*w1^2 + gamma3*w1^2),(gamma2*gamma3*w1^2 + w1^2*w2^2 + w1^2*w3^2),...
       (gamma3*w1^2*w2^2 + gamma2*w1^2*w3^2), ...
       w1^2*w2^2*w3^2];
den2 = [1,...
    (gamma2 + gamma3 + (gamma2*m2)/m1 + (gamma3*m3)/m1),...
    (gamma2*gamma3 + (gamma2*gamma3*m2)/m1 + (gamma2*gamma3*m3)/m1 + w1^2 + w2^2 + (m2*w2^2)/m1 + w3^2 + (m3*w3^2)/m1), ...
    (gamma2*w1^2 + gamma3*w1^2 + gamma3*w2^2 + (gamma3*m2*w2^2)/m1 + (gamma3*m3*w2^2)/m1 + gamma2*w3^2 + (gamma2*m2*w3^2)/m1 +(gamma2*m3*w3^2)/m1), ...
    (gamma2*gamma3*w1^2 + w1^2*w2^2 + w1^2*w3^2 + w2^2*w3^2 + (m2*w2^2*w3^2)/m1 + (m3*w2^2*w3^2)/m1), ...
    (gamma3*w1^2*w2^2 + gamma2*w1^2*w3^2), ...
     w1^2*w2^2*w3^2];
    
        
spring2tf = tf(num1,den2);
Y3= freqresp(spring2tf,freq,'Hz');

[wn,z]= damp(spring2tf);
F1= wn(1,1)/(2*pi);Q1new= 1/(2*z(1,1));
F2 = wn(3,1)/(2*pi);Q2new= 1/(2*z(3,1));
F3 = wn(5,1)/(2*pi);Q3new= 1/(2*z(5,1));
F = [F1 F2 F3]
Q = [Q1new Q2new Q3new]

%% 2 masses model - BRD1
num1 = [w1^2, w1^2*gamma2,(w1^2)*(w2^2)];
den2 = [1,(gamma2*(1+m2/m1)+ gamma1),((w1^2)+(w2^2)*(1+m2/m1) + gamma1*gamma2), gamma2*(w1^2) + gamma1*(w2^2),(w1^2)*(w2^2)];

spring2tf = tf(num1,den2);
Y2_1= freqresp(spring2tf,freq,'Hz');

[wn,z]= damp(spring2tf);
F1= wn(1,1)/(2*pi);Q1new= 1/(2*z(1,1));
F2 = wn(3,1)/(2*pi);Q2new= 1/(2*z(3,1));
F = [F1 F2]
Q = [Q1new Q2new]

%% 2 masses model - BRD3
num1 = [w1^2, w1^2*gamma3,(w1^2)*(w3^2)];
den2 = [1,(gamma3*(1+m3/m1)+ gamma1),((w1^2)+(w3^2)*(1+m3/m1) + gamma1*gamma3), gamma3*(w1^2) + gamma1*(w3^2),(w1^2)*(w3^2)];

spring2tf = tf(num1,den2);
Y2_3= freqresp(spring2tf,freq,'Hz');

[wn,z]= damp(spring2tf);
F1= wn(1,1)/(2*pi);Q1new= 1/(2*z(1,1));
F2 = wn(3,1)/(2*pi);Q2new= 1/(2*z(3,1));
F = [F1 F2]
Q = [Q1new Q2new]

% %%
% % Bounce
% fid = fopen('srs0033.txt');
% data = textscan(fid,'%f %f','HeaderLines',35);
% data = cell2mat(data);
%%
figure(1)
plot(freq,mag2db(abs(squeeze(Y3))), ...
     freq,mag2db(abs(squeeze(Y2_1))), ...
     freq,mag2db(abs(squeeze(Y2_3))));
xlabel('Frequency [Hz]'); ylabel('Magnitude []');
title(['BRDs on BS ' mode])
grid on; hold on;
xlim([16 17.5])
xlim([23.5 25])
ylim([20 50])
legend('Model 2 BRDs', 'Model 1 BRD (BRD1)','Model 1 BRD (BRD3)','Measurement (#33)');