%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Type-Bp SAS for KAGRA
% Coded by Y. Fujii on 2016/11/16
% ----------------------------------------------------------------------

%% PRELIMINARY
clear all;  % Clear workspace
close all;  % Close plot windows
addpath('../../../utility');  % Add path to utilities
% addpath('/usr/local/matlab/altmany-export_fig-76bd7fa');
g = 9.81;

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

%% TUNING DAMPER
% This part compensates the failure in converting the structural damping to
% viscous damping.
% REDUCE DAMPING ON RIM
sys1.a(34,34)=sys1.a(34,34)/30; % RIM
sys1.a(40,40)=sys1.a(40,40)/30; % RRM
%sys1.a(46,46)=sys1.a(46,46)/30; % RTM
% INCREASE DAMPING ON YF0
%sys1.a(15,15)=sys1.a(15,15)*3000; % YF1
%sys1.a(15,15)=sys1.a(15,15)*1000; % YF1
% INCREASE DAMPING ON LF1
%sys1.a(13,13)=sys1.a(13,13)*3000; % LF1
% INCREASE DAMPING ON TF1
%sys1.a(14,14)=sys1.a(14,14)*3000; % TF1
% INCREASE DAMPING ON YRM, YTM
sys1.a(42,42)=sys1.a(42,42)*30;   % YRM
sys1.a(48,48)=sys1.a(48,48)*300;  % YTM
% INCREASE DAMPING ON VTM
sys1.a(45,45)=sys1.a(45,45)*100;  % VTM

%% IMPORT SERVO FILTERS
addpath('servofilter');         % Add path to servo
typeBp161115_no_control;        % NO CONTROL MODE
rmpath ('servofilter');         % Remove path to servo

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

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

%% NOISE
% 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/2)/96e-3;% [rad/rtHz]
n_OSEM_PIM = noise_OSEM/sqrt(3/2)/96e-3;% [rad/rtHz]
n_OSEM_YIM = noise_OSEM/sqrt(2)/87e-3;  % [rad/rtHz]

 data_LVDT = importdata('../../../noise/LVDTnoiseADC_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/2);        % [m/rtHz]
%n_LVDT_TF0 = noise_LVDT/sqrt(3/2);        % [m/rtHz]
%n_LVDT_YF0 = noise_LVDT/sqrt(3)/600e-3;   % [rad/rtHz]
%n_LVDT_VF0 = noise_LVDT*2;                % [m/rtHz]
n_LVDT_VF1 = noise_LVDT*2;                % [m/rtHz]
n_LVDT_VF2 = noise_LVDT*2;                % [m/rtHz]

 data_F7 = importdata('../../../noise/F7LVDTprotonoise.dat'); % m/rtHz
noise_F7 = interp1(data_F7(:,1),data_F7(:,2),freq')';     %  m/rtHz
n_LVDT_LBF = noise_F7/sqrt(2);
n_LVDT_TBF = noise_F7/sqrt(2);
n_LVDT_YBF = noise_F7/sqrt(2)/440e-3;
n_LVDT_RBF = noise_F7/sqrt(3/2)/440e-3;
n_LVDT_PBF = noise_F7/sqrt(3/2)/440e-3;
n_LVDT_VBF = noise_F7/sqrt(3);

n_Oplev = zeros(size(freq))+1.e-7;           %[rad/rtHz]  (from Michimura's slide JGW-T1202403-v1)

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

%% NOISE MODEL
% mypsdplotopt({noise_OSEM,noise_seism},freq,...
%     'title','Noise Model',...
%     'legend',{'OSEM','seismic'},...
%     'color',{'r-','k-'},...
%     'ylim',[1e-13,1e-5],'ylabel','Magnitude [m/rtHz] ')


%% MODEL
addpath('servofilter');         % Add path to servo
typeBp161115_servo_lock8;        % Damping MODE
rmpath ('servofilter');         % Remove path to servo

%% IMPORT SIMULINK MODEL
mdlfile='typeBp161115simctrl_LengthOL'; % typeBp ver.160620
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);


%% CALC INITIAL
[mag0Vg,~]=bodesus(sysc0,'accVGND','IFO_LTM',freq);
[mag0S,~]=bodesus(sysc0,'accLGND','IFO_LTM',freq);
mag0Vg=mag0Vg.*freq.*freq*pp*pp.*noise_seism;
mag0S = mag0S.*freq.*freq*pp*pp.*noise_seism;

mag0T=sumpsd({mag0Vg,mag0S});   % Mirror Displacement w/o Ctrl
rms0T=makerms(freq,mag0T);            % Mirror Displacement RMS w/o Ctrl 
rms0Tv=makerms(freq,mag0T.*freq*pp);    % Mirror Velocity RMS w/o Ctrl

%% CALC SEISM for LENGTH
[magVg,~]=bodesus(sysc,'accVGND','IFO_LTM',freq);
[magsS,~]=bodesus(sysc,'accLGND','IFO_LTM',freq);
magVg=magVg.*freq.*freq*pp*pp.*noise_seism;
magsS=magsS.*freq.*freq*pp*pp.*noise_seism; 

magSeisT=sumpsd({magsS,magVg});    % Seismic noise of mirror
rmsSeisT=makerms(freq,magSeisT);             % Seismic noise RMS of mirror
rmsSeisTv=makerms(freq,magSeisT.*freq*pp); % Seismic noise velocity RMS of mirror

 %% CALC GAS for LENGTH
[magg1,~]=bodesus(sysc,'n_LVDT_VF1','IFO_LTM',freq);
[magg2,~]=bodesus(sysc,'n_LVDT_VF2','IFO_LTM',freq);
magg1=magg1.*n_LVDT_VF1;
magg2=magg2.*n_LVDT_VF2;

magGasTot=sumpsd({magg1,magg2});    %GAS LVDT noise
 
[magfL,~]=bodesus(sysc,'n_LVDT_LBF','IFO_LTM',freq);
[magfT,~]=bodesus(sysc,'n_LVDT_TBF','IFO_LTM',freq);
[magfY,~]=bodesus(sysc,'n_LVDT_YBF','IFO_LTM',freq);
[magfV,~]=bodesus(sysc,'n_LVDT_VBF','IFO_LTM',freq);
[magfP,~]=bodesus(sysc,'n_LVDT_PBF','IFO_LTM',freq);
[magfR,~]=bodesus(sysc,'n_LVDT_RBF','IFO_LTM',freq);
magfL=magfL.*n_LVDT_LBF;
magfT=magfT.*n_LVDT_TBF;
magfY=magfY.*n_LVDT_YBF;
magfV=magfV.*n_LVDT_VBF;
magfP=magfP.*n_LVDT_PBF;
magfR=magfR.*n_LVDT_RBF;

magLvdtTot=sumpsd({magfL,magfT,magfY,magfV,magfP,magfR});    %F2 LVDT noise


%% CALC ABOUT OSEM for LENGTH
[magoL,~]=bodesus(sysc,'n_OSEM_LIM','IFO_LTM',freq);
[magoP,~]=bodesus(sysc,'n_OSEM_PIM','IFO_LTM',freq);
[magoY,~]=bodesus(sysc,'n_OSEM_YIM','IFO_LTM',freq);
[magoT,~]=bodesus(sysc,'n_OSEM_TIM','IFO_LTM',freq);
[magoR,~]=bodesus(sysc,'n_OSEM_RIM','IFO_LTM',freq);
[magoV,~]=bodesus(sysc,'n_OSEM_VIM','IFO_LTM',freq);
magoL=magoL.*n_OSEM_LIM;
magoT=magoT.*n_OSEM_TIM;
magoV=magoV.*n_OSEM_VIM;
magoR=magoR.*n_OSEM_RIM;
magoP=magoP.*n_OSEM_PIM;
magoY=magoY.*n_OSEM_YIM;
magOsemTot=sumpsd({magoL,magoP,magoY,magoT,magoR,magoV}); %OSEM noise

%[magtL,~]=bodesus(sysc,'n_OSEM_LTM','IFO_LTM',freq);
%[magtP,~]=bodesus(sysc,'n_OSEM_PTM','IFO_LTM',freq);
%[magtY,~]=bodesus(sysc,'n_OSEM_YTM','IFO_LTM',freq);
%magtL=magtL.*n_OSEM_LTM;
%magtP=magtP.*n_OSEM_PTM;
%magtY=magtY.*n_OSEM_YTM;
%magOsemTot=sumpsd({magoL,magoP,magoY,magoT,magoR,magoV,magtL,magtP,magtY}); %OSEM noise

%% CALC ABOUT OPLEV for LENGTH
[magolL,~]=bodesus(sysc,'n_OpLev_LTM','IFO_LTM',freq);
[magolP,~]=bodesus(sysc,'n_OpLev_PTM','IFO_LTM',freq);
[magolY,~]=bodesus(sysc,'n_OpLev_YTM','IFO_LTM',freq);
magolL=magolL.*n_Oplev;
magolP=magolP.*n_Oplev;     %Assume Oplev noise level is same as OSEM
magolY=magolY.*n_Oplev;

magOlTot=sumpsd({magolL, magolP, magolY});

%% ALL Noise
magnTot=sumpsd({magGasTot,magLvdtTot, magOsemTot, magOlTot});    %ALL Noise
%magnTot=sumpsd({magGasTot, magOsemTot});    %ALL Noise
rmsnTot=makerms(freq,magnTot);
rmsnTotv=makerms(freq,magnTot.*freq*pp);

%% ALL
magAllT=sumpsd({magSeisT,magnTot});
% magaT=magoTot;
rmsAllT=makerms(freq,magAllT);
rmsAllTv=makerms(freq,magAllT.*freq*pp);

%% Requirement
%reqPR=freq;
%for i=1:length(freq)
%   if freq(i)<8
%    reqPR(i)=0;
%   else
%    reqPR(i)=5e-15*(freq(i)/10)^(-2)+5e-16;
%   end
%end

% dataBRSE=importdata('../../../SuspensionRequirement/BRSE/DisplacementNoiseRequirementCopy5Hz.dat');
% dataBRSE=[dataBRSE(:,1),dataBRSE(:,2),dataBRSE(:,3),dataBRSE(:,4),dataBRSE(:,5),dataBRSE(:,6),...
%         dataBRSE(:,7),dataBRSE(:,8),dataBRSE(:,9)];
% reqPR_BRSE = interp1(dataBRSE(:,1),dataBRSE(:,8),freq')';% m/rtHz

dataDRSE=importdata('../../../SuspensionRequirement/DRSE/DisplacementNoiseRequirementCopy10Hz.dat');
dataDRSE=[dataDRSE(:,1),dataDRSE(:,2),dataDRSE(:,3),dataDRSE(:,4),dataDRSE(:,5),dataDRSE(:,6),...
        dataDRSE(:,7),dataDRSE(:,8),dataDRSE(:,9)];
reqPR_DRSE = interp1(dataDRSE(:,1),dataDRSE(:,8),freq')';% m/rtHz


%% PLOT
% mypsdplotopt({magoTot,maggT,magsLG,magaT,mag0T,reqBS},freq,...
%     'title','Control noise coupling during observation phase',...
%     'legend',{'OSEM noise','GAS control noise','top control noise'...
%     'With control total','No control',...
%     'BS requirement'},...
%     'color',{'m-','c-','g-','r-','b-','k--'},...
%     'ylim',[1e-18,1e-5],'ylabel','Magnitude [m/rtHz] or [m/sec]')
mypsdplotopt1({magOsemTot,magGasTot,magLvdtTot,magOlTot,magAllT,mag0T,rmsAllT,rms0T,reqPR_DRSE},freq,...
    'legend',{'IM OSEM noise','GAS LVDT noise','BF LVDT noise','TM Oplev noise',...
    'With control total','No control','With control total (RMS)','No control (RMS)','requirement'},...
    'color',{'m-','g-','c-','y-','r-','b-','r--','b--','k-'},...
    'ylim',[1e-20,1e-4],'ylabel','Magnitude [m/$$\sqrt{Hz}$$] or RMS [m]')
export_fig('figures/SensorNoiseCoupling/typebp161115_Disp_damp_.pdf')


%saving point on 2016.11.16