%% 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-12;           %[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_obs8;        % 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 INITIAL for Pitch
 [mag0pS,~]=bodesus(sysc0,'accLGND','YTM',freq);
 mag0pS=mag0pS.*freq.*freq*pp*pp.*noise_seism;  % Pitch Angular Fluctuation w/o Ctrl
 rms0pT=makerms(freq,mag0pS);       % Pitch RMS w/o Ctrl
 rms0pTv=makerms(freq,mag0pS.*freq*pp);
 
%% CALC SEISM for Pitch
 [magpS,~]=bodesus(sysc,'accLGND','YTM',freq);
 magpS=magpS.*freq.*freq*pp*pp.*noise_seism;
 
%% CALC LVDT for Pitch
 [magpfL,~]=bodesus(sysc,'n_LVDT_LBF','YTM',freq);
 [magpfT,~]=bodesus(sysc,'n_LVDT_TBF','YTM',freq);
 [magpfY,~]=bodesus(sysc,'n_LVDT_YBF','YTM',freq);
 [magpfV,~]=bodesus(sysc,'n_LVDT_VBF','YTM',freq);
 [magpfP,~]=bodesus(sysc,'n_LVDT_PBF','YTM',freq);
 [magpfR,~]=bodesus(sysc,'n_LVDT_RBF','YTM',freq);
  magpfL=magpfL.*n_LVDT_LBF;
  magpfT=magpfT.*n_LVDT_TBF;
  magpfY=magpfY.*n_LVDT_YBF;
  magpfV=magpfV.*n_LVDT_VBF;
  magpfP=magpfP.*n_LVDT_PBF;
  magpfR=magpfR.*n_LVDT_RBF;

magpLvdtTot=sumpsd({magpfL, magpfT, magpfY, magpfV, magpfR, magpfP});


 %% CALC OSEM for Pitch
 [magpoL,~]=bodesus(sysc,'n_OSEM_LIM','YTM',freq);
 [magpoP,~]=bodesus(sysc,'n_OSEM_PIM','YTM',freq);
 [magpoY,~]=bodesus(sysc,'n_OSEM_YIM','YTM',freq);
 [magpoT,~]=bodesus(sysc,'n_OSEM_TIM','YTM',freq);
 [magpoR,~]=bodesus(sysc,'n_OSEM_RIM','YTM',freq);
 [magpoV,~]=bodesus(sysc,'n_OSEM_VIM','YTM',freq);
  magpoL=magpoL.*n_OSEM_LIM;
  magpoP=magpoP.*n_OSEM_PIM;
  magpoY=magpoY.*n_OSEM_YIM;
  magpoT=magpoT.*n_OSEM_TIM;
  magpoR=magpoR.*n_OSEM_RIM;
  magpoV=magpoV.*n_OSEM_VIM;
 
 %[magptL,~]=bodesus(sysc,'n_OSEM_LTM','YTM',freq);
 %[magptP,~]=bodesus(sysc,'n_OSEM_PTM','YTM',freq);
 %[magptY,~]=bodesus(sysc,'n_OSEM_YTM','YTM',freq);
 % magptL=magptL.*n_OSEM_LTM;
 % magptP=magptP.*n_OSEM_PTM;
 % magptY=magptY.*n_OSEM_YTM;
 % magpOsemTot=sumpsd({magpoL,magpoP,magpoY,magpoT,magpoR,magpoV,magptL,magptP,magptY});

 magpOsemTot=sumpsd({magpoL,magpoP,magpoY,magpoT,magpoR,magpoV});
  
  %% CALC WFS for Pitch
 [magpolL,~]=bodesus(sysc,'n_OpLev_LTM','YTM',freq);
 [magpolP,~]=bodesus(sysc,'n_OpLev_PTM','YTM',freq);
 [magpolY,~]=bodesus(sysc,'n_OpLev_YTM','YTM',freq);
  magpolL=magpolL.*n_Oplev;
  magpolP=magpolP.*n_Oplev;  %Assume that Oplev noise level is the same as OSEM
  magpolY=magpolY.*n_Oplev;

  magpOlTot=sumpsd({magpolL,magpolP,magpolY});
  
 %% ALL
 magpAllT=sumpsd({magpS,magpLvdtTot,magpOsemTot,magpOlTot});
 rmspAllT =makerms(freq, magpAllT);
 rmspAllTv=makerms(freq, magpAllT.*freq*pp);

%% PLOT PITCH
mypsdplotopt({mag0pS,magpLvdtTot,magpOsemTot,magpOlTot,magpAllT,rms0pT,rmspAllT},freq,...
    'title','Angular Fluctuation of the Mirror',...
    'legend',{'No control','BF LVDT noise',...
    'IM OSEM noise','WFS noise','total with control','no control RMS',...
    'with control RMS'},...
    'color',{'b-','c-','m-','y-','r-','b--','r--'},...
    'ylim',[1e-20,1e-4],'ylabel','Magnitude [rad/rtHz] or [rad]')
export_fig('figures/SensorNoiseCoupling/typebp161115_yaw_obs.pdf')

obs9 = [transpose(freq),transpose(mag0pS),transpose(magpLvdtTot),...
        transpose(magpOsemTot),transpose(magpOlTot),transpose(magpAllT),...
        transpose(rms0pT),transpose(rmspAllT)];

%saving point on 2016.11.16