%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Type-B1 prototype for KAGRA
% Coded by T. Sekiguchi on 2015/06/16
% ----------------------------------------------------------------------

%% PRELIMINARY
clear all;  % Clear workspace
close all;  % Close plot windows
addpath('../../utility');  % Add path to utilities
g = 9.81;

%% IMPORT SUSPENSION MODEL
matfile='typeB1susmdl';
load([matfile,'.mat']);

%% TUNING DAMPER
% This part compensates the failure in converting the structural damping to
% viscous damping.
% REDUCE DAMPING ON RIM
sys1.a(43,43)=sys1.a(43,43)/30; % RIM
sys1.a(49,49)=sys1.a(49,49)/30; % RRM
sys1.a(55,55)=sys1.a(55,55)/30; % RTM
% INCREASE DAMPING ON YF0
sys1.a(15,15)=sys1.a(15,15)*3000; % YF0
% INCREASE DAMPING ON YRM, YTM
sys1.a(51,51)=sys1.a(51,51)*30;   % YRM
sys1.a(57,57)=sys1.a(57,57)*300;  % YTM

%% IMPORT SERVO FILTERS
addpath('servofilter');         % Add path to servo
typeB1proto_servo_lock_150616;  % LOCK ACQUISITION MODE
rmpath ('servofilter');         % Remove path to servo

%% IMPORT SIMULINK MODEL
mdlfile='typeB1simctrl_150616'; % typeB1 ver.150616
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc   =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% FREQUENCY
freq =logspace(-2,1.99,1001);
% freq2=logspace(-2,3,1001);

%% IMPORT NOISE

 data_OSEM = importdata('../../noise/OSEMnoiseworstproto_disp.dat'); % um/rtHz
noise_OSEM = interp1(data_OSEM(:,1),data_OSEM(:,2)*1e-6,freq')'; %m/rtHz

n_OSEM_LTM = noise_OSEM/sqrt(4);        % [m/rtHz]
n_OSEM_PTM = noise_OSEM/sqrt(2)/117e-3; % [rad/rtHz]
n_OSEM_YTM = noise_OSEM/sqrt(2)/117e-3; % [rad/rtHz]

n_OSEM_LIM = noise_OSEM/sqrt(1);        % [m/rtHz]
n_OSEM_TIM = noise_OSEM/sqrt(2);        % [m/rtHz]
n_OSEM_VIM = noise_OSEM/sqrt(3);        % [m/rtHz]
n_OSEM_RIM = noise_OSEM/sqrt(3)/96e-3;  % [rad/rtHz]
n_OSEM_PIM = noise_OSEM/sqrt(3)/96e-3;  % [rad/rtHz]
n_OSEM_YIM = noise_OSEM/sqrt(2)/87e-3;  % [rad/rtHz]

 data_LVDT = importdata('../../noise/LVDTnoiseworstproto_disp.dat'); % um/rtHz
noise_LVDT = interp1(data_LVDT(:,1),data_LVDT(:,2)*1e-6,freq')';     %  m/rtHz

n_LVDT_LF0 = noise_LVDT/sqrt(3);        % [m/rtHz]
n_LVDT_TF0 = noise_LVDT/sqrt(2);        % [m/rtHz]
n_LVDT_YF0 = noise_LVDT/sqrt(3)/600e-3; % [rad/rtHz]

 data_GEO = importdata('../../noise/GEOnoiseproto_vel.dat'); % um/sec/rtHz
noise_GEO = interp1(data_GEO(:,1),data_GEO(:,2)*1e-6,freq')';%  m/sec/rtHz

n_GEO_LF0 = noise_GEO/sqrt(3);        % [m/rtHz]
n_GEO_TF0 = noise_GEO/sqrt(2);        % [m/rtHz]
n_GEO_YF0 = noise_GEO/sqrt(3)/594e-3; % [rad/rtHz]

 data_seism = importdata('../../noise/KamiokaSeismicHighNoise.dat'); % m/rtHz
noise_seism = interp1(data_seism(:,1),data_seism(:,2),freq')';% m//rtHz

%% NOISE COUPLING

[mag_OSEM_LTM,~]=bodesus(sysc,'n_OSEM_LTM','IFO_LTM',freq);
[mag_OSEM_LIM,~]=bodesus(sysc,'n_OSEM_LIM','IFO_LTM',freq);
[mag_LVDT_LF0,~]=bodesus(sysc,'n_LVDT_LF0','IFO_LTM',freq);
[mag_GEO_LF0 ,~]=bodesus(sysc,'n_GEO_LF0' ,'IFO_LTM',freq);
[mag_seism   ,~]=bodesus(sysc,'accLGND'   ,'IFO_LTM',freq);

%% NOISE COUPLING IFO
nIFO_OSEM_LTM = mag_OSEM_LTM.*n_OSEM_LTM;
nIFO_OSEM_LIM = mag_OSEM_LIM.*n_OSEM_LIM;
nIFO_LVDT_LF0 = mag_LVDT_LF0.*n_LVDT_LF0;
nIFO_GEO_LF0  = mag_GEO_LF0 .*n_GEO_LF0;
nIFO_seism    = mag_seism   .*noise_seism.*freq.*freq*(2*pi)^2;

%% PLOT
mypsdplotopt({n_GEO_LF0},freq,...
    'title','geophone noise','unit','[m/sec/rtHz]')
mypsdplotopt({noise_seism},freq,...
    'title','Kamioka seismic noise','unit','[m/rtHz]')

%% PLOT2
mypsdplotopt({nIFO_OSEM_LTM,nIFO_OSEM_LIM,nIFO_LVDT_LF0,...
    nIFO_GEO_LF0,nIFO_seism,noise_seism},freq,...
    'title','Sensor noise coupling',...
    'legend',{'noise OSEM LTM', 'noise OSEM LIM',...
    'noise LVDT LF0', 'noise GEO LF0', 'seismic noise','ground'},...
    'ylim',[1e-19,1e-4])

%% SUM
nIFO_noise = sumpsd({nIFO_OSEM_LTM,nIFO_OSEM_LIM,nIFO_LVDT_LF0,...
    nIFO_GEO_LF0});
nIFO_rms   = makerms(freq,nIFO_noise);
nIFO_rms_v = makerms(freq,nIFO_noise.*freq*2*pi);

%% PLOT3
mypsdplotopt({nIFO_noise,nIFO_rms,nIFO_rms_v},freq,...
    'title','Sensor noise coupling',...
    'legend',{'PSD', 'RMS','RMS velocity'},...
    'ylabel','Magnitude [m/rtHz] [m] [m/sec]',...
    'ylim',[1e-12,1e-4])