% plot mirror TFs, seismic noises
%% bLCGT
btypeA_seism_pitch=importdata('./blcgt_typeA/btypeA_seism_pitch_110523.txt');
btypeA_seism_yaw=importdata('./blcgt_typeA/btypeA_seism_yaw_110523.txt');

figure();
plotspectrum(btypeA_seism_pitch(:,1),[btypeA_seism_pitch(:,2),btypeA_seism_yaw(:,2)]);
ylabel('Angular noise [rad/rtHz]');
legend('Type-A pitch','Type-A yaw');
title('Seismic Noise (bLCGT)');
ylim([1e-30,1e-3]);
set(gca,'YTick',logspace(-30,-3,28));
saveas(gcf,['./results/suspension/bLCGTSeismic.png'])

H=importdata('./blcgt_typeA/btypeA_tf_pitch_110523.txt');
btypeA_tf_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('./blcgt_typeA/btypeA_tf_yaw_110523.txt');
btypeA_tf_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
figure();
plotbode(H(:,1),[btypeA_tf_pitch,btypeA_tf_yaw]);
legend('Type-A pitch','Type-A yaw');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title('Torque on TM to angle of TM (bLCGT)');
saveas(gcf,['./results/suspension/bLCGTTMtorquetoTMangle.png'])

H=importdata('./blcgt_typeA/btypeA_tf_wRM_pitch_110523.txt');
btypeA_tf_wRM_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('./blcgt_typeA/btypeA_tf_wRM_yaw_110523.txt');
btypeA_tf_wRM_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
figure();
plotbode(H(:,1),[btypeA_tf_wRM_pitch,btypeA_tf_wRM_yaw]);
legend('Type-A pitch','Type-A yaw');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title('Torque from RM to angle of TM (bLCGT)');
saveas(gcf,['./results/suspension/bLCGTRMtorquetoTMangle.png'])

H=importdata('./blcgt_typeA/btypeA_tf_IM_pitch_110523.txt');
btypeA_tf_IM_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('./blcgt_typeA/btypeA_tf_IM_yaw_110523.txt');
btypeA_tf_IM_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
figure();
plotbode(H(:,1),[btypeA_tf_IM_pitch,btypeA_tf_IM_yaw]);
legend('Type-A pitch','Type-A yaw');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title('Torque from IRM to angle of TM (bLCGT)');
saveas(gcf,['./results/suspension/bLCGTIRMtorquetoTMangle.png'])


%% bLCGT TM to TM
p=parambLCGT();
p.ITMXChr=1/1600;
p.ITMYChr=1/1600;
p.ETMXChr=1/1900;
p.ETMYChr=1/1900;

c=299792458;
gITM=1-p.Larm*p.ITMXChr;
gETM=1-p.Larm*p.ETMXChr;
r=2/((gITM-gETM)+sqrt((gITM-gETM)^2+4));
Pintracav=379e3;  %intra-cavity power
k0=2*Pintracav*p.Larm/(c*(gETM*gITM-1));
kSOFT=k0*(gETM+1/r)
kHARD=k0*(gITM-1/r)

fres=2.3;
I=0.15;
fit=filterdesign(H(:,1),[],[fres 1e4],1/I);
k=(2*pi*fres)^2*I;
fSOFT=sqrt((k+kSOFT)/I)/(2*pi)
fHARD=sqrt((k+kHARD)/I)/(2*pi)

figure();
plotbode(H(:,1),[btypeA_tf_pitch,btypeA_tf_pitch./(1+kSOFT*btypeA_tf_pitch),btypeA_tf_pitch./(1+kHARD*btypeA_tf_pitch),fit]);
legend('Type-A pitch no radiation','Type-A pitch SOFT','Type-A pitch HARD','fitted');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from TM to angle of TM (bLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/bLCGTopto-mechanicalTF_TM_pitch_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

fres=0.8;
I=0.15;
fit=filterdesign(H(:,1),[],[fres 1e4],1/I);
k=(2*pi*fres)^2*I;
fSOFT=sqrt((k+kSOFT)/I)/(2*pi)
fHARD=sqrt((k+kHARD)/I)/(2*pi)

figure();
plotbode(H(:,1),[btypeA_tf_yaw,btypeA_tf_yaw./(1+kSOFT*btypeA_tf_yaw),btypeA_tf_yaw./(1+kHARD*btypeA_tf_yaw),fit]);
legend('Type-A yaw no radiation','Type-A yaw SOFT','Type-A yaw HARD','fit');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from TM to angle of TM (bLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/bLCGTopto-mechanicalTF_TM_yaw_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

%% bLCGT RM to TM

figure();
plotbode(H(:,1),[btypeA_tf_wRM_pitch,btypeA_tf_wRM_pitch./(1+kSOFT*btypeA_tf_pitch),btypeA_tf_wRM_pitch./(1+kHARD*btypeA_tf_pitch)]);
legend('Type-A pitch no radiation','Type-A pitch SOFT','Type-A pitch HARD');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from RM to angle of TM (bLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/bLCGTopto-mechanicalTF_RM_pitch_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

figure();
plotbode(H(:,1),[btypeA_tf_wRM_yaw,btypeA_tf_wRM_yaw./(1+kSOFT*btypeA_tf_yaw),btypeA_tf_wRM_yaw./(1+kHARD*btypeA_tf_yaw)]);
legend('Type-A yaw no radiation','Type-A yaw SOFT','Type-A yaw HARD');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from RM to angle of TM (bLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/bLCGTopto-mechanicalTF_RM_yaw_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

figure();
plotbode(H(:,1),[btypeA_tf_IM_pitch,btypeA_tf_IM_pitch./(1+kSOFT*btypeA_tf_pitch),btypeA_tf_IM_pitch./(1+kHARD*btypeA_tf_pitch)]);
legend('Type-A pitch no radiation','Type-A pitch SOFT','Type-A pitch HARD');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from IRM to angle of TM (bLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/bLCGTopto-mechanicalTF_IRM_pitch_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

figure();
plotbode(H(:,1),[btypeA_tf_IM_yaw,btypeA_tf_IM_yaw./(1+kSOFT*btypeA_tf_yaw),btypeA_tf_IM_yaw./(1+kHARD*btypeA_tf_yaw)]);
legend('Type-A yaw no radiation','Type-A yaw SOFT','Type-A yaw HARD');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from IRM to angle of TM (bLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/bLCGTopto-mechanicalTF_IRM_yaw_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

%% plot seismic noise
typeA_seism_pitch=importdata('../iLCGT_ASC/seism_angle_110513/TypeA/typeA_seism_pitch_110513.txt');
typeA_seism_yaw=importdata('../iLCGT_ASC/seism_angle_110513/TypeA/typeA_seism_yaw_110513.txt');
BS_seism_pitch=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_BS/BS_seism_pitch_110513.txt');
BS_seism_yaw=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_BS/BS_seism_yaw_110513.txt');
RM_seism_pitch=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_RM/RM_seism_pitch_110513.txt');
RM_seism_yaw=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_RM/RM_seism_yaw_110513.txt');

figure();
plotspectrum(typeA_seism_pitch(:,1),[typeA_seism_pitch(:,2),typeA_seism_yaw(:,2),BS_seism_pitch(:,2),BS_seism_yaw(:,2),RM_seism_pitch(:,2),RM_seism_yaw(:,2)]);
ylabel('Angular noise [rad/rtHz]');
legend('Type-A pitch','Type-A yaw','Type-B BS pitch','Type-B BS yaw','Type-B RC pitch','Type-B RC yaw');
title('Seismic Noise (iLCGT)');
ylim([1e-30,1e-3]);
set(gca,'YTick',logspace(-30,-3,28));
saveas(gcf,['./results/suspension/iLCGTSeismic.png'])

%% plot TM torque to TM angle (Hrad: radiation pressure torque to TM angle)
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeA/typeA_tf_pitch_110513.txt');
typeA_tf_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeA/typeA_tf_yaw_110513.txt');
typeA_tf_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_BS/BS_tf_pitch_110513.txt');
BS_tf_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_BS/BS_tf_yaw_110513.txt');
BS_tf_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_RM/RM_tf_pitch_110513.txt');
RM_tf_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_RM/RM_tf_yaw_110513.txt');
RM_tf_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
figure();
plotbode(H(:,1),[typeA_tf_pitch,typeA_tf_yaw,BS_tf_pitch,BS_tf_yaw,RM_tf_pitch,RM_tf_yaw]);
legend('Type-A pitch','Type-A yaw','Type-B BS pitch','Type-B BS yaw','Type-B RC pitch','Type-B RC yaw');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title('Torque on TM to angle of TM (iLCGT)');
saveas(gcf,['./results/suspension/iLCGTTMtorquetoTMangle.png'])

%% plot RM torque to TM angle (Hact: actuator TF)
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeA/typeA_tf_wRM_pitch_110513.txt');
typeA_tf_wRM_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeA/typeA_tf_wRM_yaw_110513.txt');
typeA_tf_wRM_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_BS/BS_tf_wRM_pitch_110513.txt');
BS_tf_wRM_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_BS/BS_tf_wRM_yaw_110513.txt');
BS_tf_wRM_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_RM/RM_tf_wRM_pitch_110513.txt');
RM_tf_wRM_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110513/TypeB_RM/RM_tf_wRM_yaw_110513.txt');
RM_tf_wRM_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
figure();
plotbode(H(:,1),[typeA_tf_wRM_pitch,typeA_tf_wRM_yaw,BS_tf_wRM_pitch,BS_tf_wRM_yaw,RM_tf_wRM_pitch,RM_tf_wRM_yaw]);
legend('Type-A pitch','Type-A yaw','Type-B BS pitch','Type-B BS yaw','Type-B RC pitch','Type-B RC yaw');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title('Torque from RM to angle of TM (iLCGT)');
saveas(gcf,['./results/suspension/iLCGTRMtorquetoTMangle.png'])

%% plot IRM torque to TM angle (Hact: actuator TF)
H=importdata('../iLCGT_ASC/seism_angle_110508/TypeA/typeA_tf_IM_pitch_110508.txt');
typeA_tf_IM_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110508/TypeA/typeA_tf_IM_yaw_110508.txt');
typeA_tf_IM_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110508/TypeB_BS/BS_tf_IM_pitch_110508.txt');
BS_tf_IM_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110508/TypeB_BS/BS_tf_IM_yaw_110508.txt');
BS_tf_IM_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110508/TypeB_RM/RM_tf_IM_pitch_110508.txt');
RM_tf_IM_pitch=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
H=importdata('../iLCGT_ASC/seism_angle_110508/TypeB_RM/RM_tf_IM_yaw_110508.txt');
RM_tf_IM_yaw=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));
figure();
plotbode(H(:,1),[typeA_tf_IM_pitch,typeA_tf_IM_yaw,BS_tf_IM_pitch,BS_tf_IM_yaw,RM_tf_IM_pitch,RM_tf_IM_yaw]);
legend('Type-A pitch','Type-A yaw','Type-B BS pitch','Type-B BS yaw','Type-B RC pitch','Type-B RC yaw');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title('Torque from IRM to angle of TM (iLCGT)');
saveas(gcf,['./results/suspension/iLCGTIRMtorquetoTMangle.png'])

%% plot RM/IRM torque to TM angle under radiation pressure (H=Hact/(1+k*Hrad))
p=parambLCGT();
% p.ITMXChr=1/1600;
% p.ITMYChr=1/1600;
% p.ETMXChr=1/1900;
% p.ETMYChr=1/1900;

c=299792458;
gITM=1-p.Larm*p.ITMXChr;
gETM=1-p.Larm*p.ETMXChr;
r=2/((gITM-gETM)+sqrt((gITM-gETM)^2+4));
Pintracav=400e3;  %intra-cavity power
k0=2*Pintracav*p.Larm/(c*(gETM*gITM-1));
kSOFT=k0*(gETM+1/r)
kHARD=k0*(gITM-1/r)

fres=1.25;
I=0.05;
fit=filterdesign(H(:,1),[],[fres 1e4],1/I);
k=(2*pi*fres)^2*I;
fSOFT=sqrt((k+kSOFT)/I)/(2*pi)
fHARD=sqrt((k+kHARD)/I)/(2*pi)

figure();
plotbode(H(:,1),[typeA_tf_pitch,typeA_tf_pitch./(1+kSOFT*typeA_tf_pitch),typeA_tf_pitch./(1+kHARD*typeA_tf_pitch),fit]);
legend('Type-A pitch no radiation','Type-A pitch SOFT','Type-A pitch HARD','fitted');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from TM to angle of TM (iLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/iLCGTopto-mechanicalTF_TM_pitch_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

fres=0.63;
I=0.05;
fit=filterdesign(H(:,1),[],[fres 1e4],1/I);
k=(2*pi*fres)^2*I;
fSOFT=sqrt((k+kSOFT)/I)/(2*pi)
fHARD=sqrt((k+kHARD)/I)/(2*pi)

figure();
plotbode(H(:,1),[typeA_tf_yaw,typeA_tf_yaw./(1+kSOFT*typeA_tf_yaw),typeA_tf_yaw./(1+kHARD*typeA_tf_yaw),fit]);
legend('Type-A yaw no radiation','Type-A yaw SOFT','Type-A yaw HARD','fitted');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from TM to angle of TM (iLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/iLCGTopto-mechanicalTF_TM_yaw_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

%%
figure();
plotbode(H(:,1),[typeA_tf_wRM_pitch,typeA_tf_wRM_pitch./(1+kSOFT*typeA_tf_pitch),typeA_tf_wRM_pitch./(1+kHARD*typeA_tf_pitch)]);
legend('Type-A pitch no radiation','Type-A pitch SOFT','Type-A pitch HARD');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from RM to angle of TM (iLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/iLCGTopto-mechanicalTF_RM_pitch_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

figure();
plotbode(H(:,1),[typeA_tf_wRM_yaw,typeA_tf_wRM_yaw./(1+kSOFT*typeA_tf_yaw),typeA_tf_wRM_yaw./(1+kHARD*typeA_tf_yaw)]);
legend('Type-A yaw no radiation','Type-A yaw SOFT','Type-A yaw HARD');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from RM to angle of TM (iLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/iLCGTopto-mechanicalTF_RM_yaw_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

figure();
plotbode(H(:,1),[typeA_tf_IM_pitch,typeA_tf_IM_pitch./(1+kSOFT*typeA_tf_pitch),typeA_tf_IM_pitch./(1+kHARD*typeA_tf_pitch)]);
legend('Type-A pitch no radiation','Type-A pitch SOFT','Type-A pitch HARD');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from IRM to angle of TM (iLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/iLCGTopto-mechanicalTF_IRM_pitch_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

figure();
plotbode(H(:,1),[typeA_tf_IM_yaw,typeA_tf_IM_yaw./(1+kSOFT*typeA_tf_yaw),typeA_tf_IM_yaw./(1+kHARD*typeA_tf_yaw)]);
legend('Type-A yaw no radiation','Type-A yaw SOFT','Type-A yaw HARD');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e5]);
set(gca,'YTick',logspace(-5,5,11));
title(['Torque from IRM to angle of TM (iLCGTsus Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);
saveas(gcf,['./results/suspension/iLCGTopto-mechanicalTF_IRM_yaw_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'.png'])

%% kH plot
figure();
plotbode(H(:,1),[kSOFT*typeA_tf_pitch,kHARD*typeA_tf_pitch]);
legend('Type-A pitch SOFT','Type-A pitch HARD');
subplot(2,1,1);
ylabel('Abs');
title(['kH, SOFT Mode (Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f'),')']);

%%
freq=logspace(-2,2,1000);
InTM=0.05;
fres=1.5;
kres=InTM*(2*pi*fres)^2
H=filterdesign(freq,[],[fres 2e3],1/InTM);

p=paramiLCGT();
p.ITMXChr=1/1600;
p.ITMYChr=1/1600;
p.ETMXChr=1/1900;
p.ETMYChr=1/1900;

c=299792458;
gITM=1-p.Larm*p.ITMXChr;
gETM=1-p.Larm*p.ETMXChr;
r=2/((gITM-gETM)+sqrt((gITM-gETM)^2+4));
Pintracav=400e3;  %intra-cavity power
k0=2*Pintracav*p.Larm/(c*(gETM*gITM-1));
kSOFT=k0*(gETM+1/r);
kHARD=k0*(gITM-1/r);

fSOFT=1/(2*pi)*sqrt((kres+kSOFT)/InTM)  % SOFT frequency
fHARD=1/(2*pi)*sqrt((kres+kHARD)/InTM)  % HARD frequency

figure();
plotbode(freq,[H,H./(1+kSOFT.*H),H./(1+kHARD.*H)]);
legend('no radiation','SOFT','HARD');
subplot(2,1,1);
ylabel('Abs [rad/(Nm)]');
ylim([1e-5,1e2]);
title(['Pincav=', num2str(Pintracav/1e3),'kW, gITM=',num2str(gITM,'%.2f'),', gETM=',num2str(gETM,'%.2f')]);
%saveas(gcf,['./optmechfres/opto-mechanicalTF_pitch_g=',num2str(gITM,'%.2f'),'and',num2str(gETM,'%.2f'),'_fres=',num2str(fres,'%.1f'),'.png'])


%% fitting by simple pendulum
HTMPit=typeA_tf_pitch;
HTMYaw=typeA_tf_yaw;
freq=H(:,1);

% pitch
InTM = 0.04; %Moment of innertia
wTMPit = 2*pi*2; %Pit resonant freq.
QTMPit = 30; %1e5;

kres=InTM*(wTMPit)^2;
p.tfTMPit = zpk([], -wTMPit*[1/(2*QTMPit) + sqrt((1/(2*QTMPit))^2 - 1), ...
                      1/(2*QTMPit) - sqrt((1/(2*QTMPit))^2 - 1)], 1/InTM);

[Hgain,Hphase,omega]=bode(p.tfTMPit,2*pi*freq);
HoptPit=squeeze(Hgain.*exp(i.*Hphase/180*pi));
fPit=1/(2*pi)*sqrt(kres/InTM)  % pitch pendulum frequency
fSOFT=1/(2*pi)*sqrt((kres+kSOFT)/InTM)  % SOFT frequency
fHARD=1/(2*pi)*sqrt((kres+kHARD)/InTM)  % HARD frequency

% yaw
InTM = 0.04; %Moment of innertia
wTMPit = 2*pi*0.5; %Pit resonant freq.
QTMPit = 13; %1e5;

kres=InTM*(wTMPit)^2;
p.tfTMPit = zpk([], -wTMPit*[1/(2*QTMPit) + sqrt((1/(2*QTMPit))^2 - 1), ...
                      1/(2*QTMPit) - sqrt((1/(2*QTMPit))^2 - 1)], 1/InTM);

[Hgain,Hphase,omega]=bode(p.tfTMPit,2*pi*freq);
HoptYaw=squeeze(Hgain.*exp(i.*Hphase/180*pi));
fYaw=1/(2*pi)*sqrt(kres/InTM)  % yaw pendulum frequency
fSOFT=1/(2*pi)*sqrt((kres+kSOFT)/InTM)  % SOFT frequency
fHARD=1/(2*pi)*sqrt((kres+kHARD)/InTM)  % HARD frequency

figure();
plotbode(freq,HTMPit,HoptPit,HTMYaw,HoptYaw);
legend('pitch','fitting pitch','yaw','fitting yaw')

%% fitting by zpk Type-A
HTMPit=typeA_tf_pitch;
freq=H(:,1);

% pitch
ZQ=[0.473 2e3];
PQ=[0.353 2e3
    2.582 2e3];
p.tfTMPit = filterdesign('zpk',ZQ,PQ,1/0.05);

[Hgain,Hphase,omega]=bode(p.tfTMPit,2*pi*freq);
HoptPit=squeeze(Hgain.*exp(i.*Hphase/180*pi));

figure();
plotbode(freq,HTMPit,HoptPit);
legend('pitch','zpk fitting pitch')

%% fitting by zpk Type-B BS
HTMPit=BS_tf_pitch;
freq=H(:,1);

% pitch
ZQ=[1.714 2e3
    4.786 2e3];
PQ=[0.337 2e3
    3.467 2e3
    5.321 2e3];
p.tfTMPit = filterdesign('zpk',ZQ,PQ,1/0.4);

[Hgain,Hphase,omega]=bode(p.tfTMPit,2*pi*freq);
HoptPit=squeeze(Hgain.*exp(i.*Hphase/180*pi));

figure();
plotbode(freq,HTMPit,HoptPit);
legend('pitch','zpk fitting pitch')

%% fitting by zpk Type-B RM
HTMPit=RM_tf_pitch;
freq=H(:,1);

% pitch
ZQ=[2.547 2e3
    5.598 2e3];
PQ=[0.336 2e3
    4.699 2e3
    6.397 2e3];
p.tfTMPit = filterdesign('zpk',ZQ,PQ,1/0.05);

[Hgain,Hphase,omega]=bode(p.tfTMPit,2*pi*freq);
HoptPit=squeeze(Hgain.*exp(i.*Hphase/180*pi));

figure();
plotbode(freq,HTMPit,HoptPit);
legend('pitch','zpk fitting pitch')