function LCGT_ASC()
% ASC for LCGT
% uses  Optickle and tools made by Y. Aso. Add paths!
% needs (1) parambLCGT.m            for parameter setting (paramiLCGT.m also available)
%       (2) modelbLCGT.m            for constructing Optickle model (use modelbLCGT.m also for iLCGT)
%       (3) plotGouyWFS.m           for plotting Gouy phase dependance of WFS signal
%       (4) plotSensingMatrixASC.m  for generating angle sensing matrix
%       (5) DemodGouy.m             for finding best demod and Gouy phase
%       (6) plotbode.m              for bode plot
% Author: Yuta Michimura

%% Clear everything
%close all
clear all
cd('C:\Users\yuta\Lab\LCGT\IFOmodel\bLCGT_ASC')

% % Set parameters and save directory
    % Set parameters
%p=paramiLCGT('pitch');
p=parambLCGT('DRSE','negative','pitch');

saveDir=['./results/',p.modelName,'-',date,'/'];
mkdir(saveDir);

% % Construct Optickle model
% Construct Optickle model
[opt,n,l,pr] = modelbLCGT(p);

% Define some constants
c=opt.c;
h=opt.h;
hbar=h/(2*pi);
lambda=opt.lambda;
omega=2*pi*c/lambda;
omegamod1=2*pi*p.fmod1;
omegamod2=2*pi*p.fmod2;
L=p.Larm;
t1=sqrt(p.ITMXThr);
r1=sqrt(1-p.ITMXThr);
t2=sqrt(p.ETMXThr);
r2=sqrt(1-p.ETMXThr);
Finesse=pi*sqrt(r1*r2)/(1-r1*r2)
tau=2*L*Finesse/(pi*c);

% Get useful things
vDist = getLinkLengths(opt);
vBasis = getAllFieldBases(opt);
vPhiGouy = getGouyPhase(vDist, vBasis(:, 2)) * 180 / pi;

fprintf('Arm cavity one-way Gouy phase: %g deg \n', vPhiGouy(l.ITMXtoETMX));
fprintf('PRC one-way Gouy phase: %g deg \n', vPhiGouy(l.PRMtoPR2)+vPhiGouy(l.PR2toPR3)+vPhiGouy(l.PR3toBS));
fprintf('SRC one-way Gouy phase: %g deg \n', vPhiGouy(l.SRMtoSR2)+vPhiGouy(l.SR2toSR3)+vPhiGouy(l.SR3toBS));

% vBasis(l.ITMYtoBS)
% vBasis(l.BStoITMY)
% vBasis(l.ITMYtoETMY)
% vBasis(l.BStoITMX)
% vBasis(l.ITMXtoETMX)
% vBasis(l.ITMXtoBS)
% vBasis(l.BStoSR3)
% vBasis(l.BStoPR3)


save([saveDir,'parameters'],'p');  %save parameter set
save([saveDir,'opt'],'opt');       %save optickle model

% % Check power at each DC probe, field evaluation point
[fDC, sigDC] = tickle(opt, [], []);

P_POPA=sigDC(pr.POP_ADC);
P_REFLA=sigDC(pr.REFL_ADC);
P_ASA=sigDC(pr.AS_ADC);
P_TRXA=sigDC(pr.TRX_ADC);
P_TRYA=sigDC(pr.TRY_ADC);

P_LasertoMod1=fDC(l.LasertoMod1,round(length(p.vMod)/2),1).^2;
P_PR3toBS=fDC(l.PR3toBS,round(length(p.vMod)/2),1).^2;
P_ITMXtoETMX=fDC(l.ITMXtoETMX,round(length(p.vMod)/2),1).^2;
P_ETMXtoITMX=fDC(l.ETMXtoITMX,round(length(p.vMod)/2),1).^2;
Pintracav=abs(P_ITMXtoETMX+P_ETMXtoITMX)/2;

fprintf('Power from PR3 to BS: \t %g W \n', P_PR3toBS);
fprintf('intra-cavity power: \t %g kW \n', Pintracav/1e3);
fprintf('Power at POP_ADC: \t %g mW \n', P_POPA*1e3);
fprintf('Power at REFL_ADC: \t %g mW \n', P_REFLA*1e3);
fprintf('Power at AS_ADC: \t %g mW \n', P_ASA*1e3);
fprintf('Power at TRX_ADC: \t %g mW \n', P_TRXA*1e3);
fprintf('Power at TRY_ADC: \t %g mW \n', P_TRYA*1e3);

% % Set important vectors, matrices
% Mirrors to drive
driveNames=p.driveNames;
iDrv=getDriveNumbers(opt,driveNames);

BSMprobeNames=p.BSMprobeNames;
iBSMPrb=cellfun(@(x)getProbeNum(opt,x),BSMprobeNames);

Nprobe=opt.Nprobe-length(BSMprobeNames);   % ignoring BSM probes
iPrb=1:Nprobe;
probeNames = getProbeName(opt,iPrb)';
iPrb = cellfun(@(x)getProbeNum(opt,x),probeNames);

% DOFs to control
cDrvNames=p.cDrvNames;

mDrv=p.mDrv;
mDOFtoMIRROR=mDrv';
vMirrorNames=driveNames;
vDOFNames=cDrvNames;
save([saveDir,'vMirrorNames'],'vMirrorNames');  % "MIRRORS" (see LIGO-T0900511 Figure 10)
save([saveDir,'vDOFNames'],'vDOFNames');  % DOFs (see LIGO-T0900511 Figure 10)
save([saveDir,'mDOFtoMIRROR'],'mDOFtoMIRROR');  % "DOF map to MIRROR" (see LIGO-T0900511 Figure 10)

opt2dot(opt, 'LCGT_ASC.pdf');

     
%% tickle01 for Gouy - WFS signal plot
gouystep=39;
wfs=zeros(length(probeNames),length(cDrvNames), gouystep);
phi=linspace(-pi,pi,gouystep);
for k=1:gouystep
    opt = setGouyPhase(opt, 'GouyPOPA', phi(k));
    opt = setGouyPhase(opt, 'GouyREFLA', phi(k));
    opt = setGouyPhase(opt, 'GouyASA', phi(k));
    opt = setGouyPhase(opt, 'GouyTRXA', phi(k));
    opt = setGouyPhase(opt, 'GouyTRYA', phi(k));
    [sigAC, mMech] = tickle01(opt, [], p.ftickle01);
    wfs(:,:,k)=sigAC(iPrb,iDrv)*mDrv';
end

%% Gouy - WFS signal plot
% plotGoyWFS saves plots
plotGouyWFS(phi,wfs,probeNames,iPrb,{'POP_A','REFL_A','AS_A','TRX_A','TRY_A'},cDrvNames,saveDir);

%% Find demod phases and Gouy phases that maximize signals / minimize signal contamination
% Tune again and run from the beginning if demod phases look bad
opt = setGouyPhase(opt, 'GouyPOPA', 0);
opt = setGouyPhase(opt, 'GouyPOPB', pi/2);
opt = setGouyPhase(opt, 'GouyREFLA', 0);
opt = setGouyPhase(opt, 'GouyREFLB', pi/2);
opt = setGouyPhase(opt, 'GouyASA', 0);
opt = setGouyPhase(opt, 'GouyASB', pi/2);
opt = setGouyPhase(opt, 'GouyTRXA', 0);
opt = setGouyPhase(opt, 'GouyTRXB', pi/2);
opt = setGouyPhase(opt, 'GouyTRYA', 0);
opt = setGouyPhase(opt, 'GouyTRYB', pi/2);

[sigAC, mMech]=tickle01(opt,[],p.ftickle01);

cPrbNames = probeNames;
mPrb = eye(length(probeNames));

figure();
mSens=plotSensingMatrixASC(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames);
title('Demod phases and Gouy phases un-optimized')

mSens=abs(mSens).*sign(real(mSens));

figure();
subplot(3,1,1);
title('POP DC')
POP_DCprobeIndexes = cellfun(@(x)getProbeNum(opt,x),{'POP_ADC','POP_ADC','POP_BDC','POP_BDC'});
[sAImax,d_max,G_max]=DemodGouy(mSens,POP_DCprobeIndexes,cDrvNames,'DC');
subplot(3,1,2);
title('POP f1 demodulation')
POP_1probeIndexes = cellfun(@(x)getProbeNum(opt,x),{'POP_A1I','POP_A1Q','POP_B1I','POP_B1Q'});
[sAImax,d_max,G_max]=DemodGouy(mSens,POP_1probeIndexes,cDrvNames);
subplot(3,1,3);
title('POP f2 demodulation')
POP_1probeIndexes = cellfun(@(x)getProbeNum(opt,x),{'POP_A2I','POP_A2Q','POP_B2I','POP_B2Q'});
DemodGouy(mSens,POP_1probeIndexes,cDrvNames);
saveas(gcf,[saveDir,'WFSDemodGouy_POP.png'])

figure();
subplot(3,1,1);
title('REFL DC')
REFL_DCprobeIndexes = cellfun(@(x)getProbeNum(opt,x),{'REFL_ADC','REFL_ADC','REFL_BDC','REFL_BDC'});
DemodGouy(mSens,REFL_DCprobeIndexes,cDrvNames,'DC');
subplot(3,1,2);
title('REFL f1 demodulation')
REFL_1probeIndexes = cellfun(@(x)getProbeNum(opt,x),{'REFL_A1I','REFL_A1Q','REFL_B1I','REFL_B1Q'});
DemodGouy(mSens,REFL_1probeIndexes,cDrvNames);
subplot(3,1,3);
title('REFL f2 demodulation')
REFL_1probeIndexes = cellfun(@(x)getProbeNum(opt,x),{'REFL_A2I','REFL_A2Q','REFL_B2I','REFL_B2Q'});
DemodGouy(mSens,REFL_1probeIndexes,cDrvNames);
saveas(gcf,[saveDir,'WFSDemodGouy_REFL.png'])

figure();
subplot(3,1,1);
title('AS DC')
AS_DCprobeIndexes = cellfun(@(x)getProbeNum(opt,x),{'AS_ADC','AS_ADC','AS_BDC','AS_BDC'});
DemodGouy(mSens,AS_DCprobeIndexes,cDrvNames,'DC');
subplot(3,1,2);
title('AS f1 demodulation')
AS_1probeIndexes = cellfun(@(x)getProbeNum(opt,x),{'AS_A1I','AS_A1Q','AS_B1I','AS_B1Q'});
DemodGouy(mSens,AS_1probeIndexes,cDrvNames);
saveas(gcf,[saveDir,'WFSDemodGouy_AS.png'])

figure();
subplot(3,1,1);
title('TRX DC')
TRX_DCprobeIndexes = cellfun(@(x)getProbeNum(opt,x),{'TRX_ADC','TRX_ADC','TRX_BDC','TRX_BDC'});
DemodGouy(mSens,TRX_DCprobeIndexes,cDrvNames,'DC');
subplot(3,1,2);
title('TRY DC')
TRY_DCprobeIndexes = cellfun(@(x)getProbeNum(opt,x),{'TRY_ADC','TRY_ADC','TRY_BDC','TRY_BDC'});
DemodGouy(mSens,TRY_DCprobeIndexes,cDrvNames,'DC');
saveas(gcf,[saveDir,'WFSDemodGouy_TR.png'])

%% Whole WFS sensing matrix
% Tune again if Gouy phases look bad
opt = setGouyPhase(opt, 'GouyPOPA', p.GouyPOPA);
opt = setGouyPhase(opt, 'GouyPOPB', p.GouyPOPB);
opt = setGouyPhase(opt, 'GouyREFLA', p.GouyREFLA);
opt = setGouyPhase(opt, 'GouyREFLB', p.GouyREFLB);
opt = setGouyPhase(opt, 'GouyASA', p.GouyASA);
opt = setGouyPhase(opt, 'GouyASB', p.GouyASB);
opt = setGouyPhase(opt, 'GouyTRXA', p.GouyTRA);
opt = setGouyPhase(opt, 'GouyTRXB', p.GouyTRB);
opt = setGouyPhase(opt, 'GouyTRYA', p.GouyTRA);
opt = setGouyPhase(opt, 'GouyTRYB', p.GouyTRB);

save([saveDir,'parameters'],'p');  %save parameter set
save([saveDir,'opt'],'opt');       %save optickle model

[sigAC, mMech]=tickle01(opt,[],p.ftickle01);

cPrbNames = probeNames;
mPrb = eye(length(probeNames));

figure();
plotSensingMatrixASC(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames);
fnote=strcat('(Gouy phases at POP A: ',num2str(p.GouyPOPA/pi*180,'%.1f'),', POP B: ',num2str(p.GouyPOPB/pi*180,'%.1f'),' REFL A: ',num2str(p.GouyREFLA/pi*180,'%.1f'),', REFL B: ',num2str(p.GouyREFLB/pi*180,'%.1f'),', AS A: ',num2str(p.GouyASA/pi*180,'%.1f'),', AS B: ',num2str(p.GouyASB/pi*180,'%.1f'), ', TR A: ',num2str(p.GouyTRA/pi*180,'%.1f'), '  deg)');
xlabel(fnote,'FontSize',10)
saveas(gcf,[saveDir,'WFSSensingMatrix_all.png'])

%% Port-selected WFS sensing matrix
selectedprobeNames = p.selectedprobeNames;
cPrbNames = selectedprobeNames;
mPrb = eye(length(selectedprobeNames));

fnote=strcat('(Gouy phases at POP A: ',num2str(p.GouyPOPA/pi*180,'%.1f'),', POP B: ',num2str(p.GouyPOPB/pi*180,'%.1f'),' REFL A: ',num2str(p.GouyREFLA/pi*180,'%.1f'),', REFL B: ',num2str(p.GouyREFLB/pi*180,'%.1f'),', AS A: ',num2str(p.GouyASA/pi*180,'%.1f'),', AS B: ',num2str(p.GouyASB/pi*180,'%.1f'), ', TR A: ',num2str(p.GouyTRA/pi*180,'%.1f'), '  deg)');

figure();
mSens=plotSensingMatrixASC(opt, sigAC, selectedprobeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames);
xlabel(fnote,'FontSize',10)
saveas(gcf,[saveDir,'WFSSensingMatrix_selected.png'])

figure();
mSens=plotNormalizedSensingMatrixASC(opt, sigAC, selectedprobeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames);
xlabel(fnote,'FontSize',10)
saveas(gcf,[saveDir,'WFSSensingMatrix_normalized.png'])

vProbeNames=selectedprobeNames;
save([saveDir,'vProbeNames'],'vProbeNames');  % "SENSORS" (see LIGO-T0900511 Figure 10)

mSens=abs(mSens).*sign(real(mSens));
save([saveDir,'mSens'],'mSens');  % sensing matrix. inverse of this will be "Input Matrix" (see LIGO-T0900511 Figure 10)

[fDC, sigDC] = tickle(opt, [], []);
vProbePower = sigDC(cellfun(@(x)getProbeNum(opt,x),p.selectedprobeDC));
save([saveDir,'vProbePower'],'vProbePower');  % power at each probes (for calculating shot noise)

% degree of diagonalization of the sensing matrix
hP=6.6261e-34;   % Planck's constant
c=299792458;     % speed of light
lambda=1.064e-6; % wavelength of laser
nu=c/lambda;     % frequency of laser

for kk=1:length(cDrvNames)
    if strfind(cDrvNames{kk},'SR2')>0
        mSens(:,kk)=zeros(length(selectedprobeNames),1);
    end
end

mInput=pinv(mSens);
mShot=diag(sqrt(2*hP*nu*vProbePower));
mAllShot=mInput*mShot;
vShot=zeros(length(selectedprobeNames),1);
for kk=1:length(cDrvNames)
    vShot(kk)=norm(mAllShot(kk,:));     % shot noise for each error signal [rad/rtHz]
end
fprintf('Shot noise for each reconstructed error signal [rad/rtHz]\n');
for kk=1:length(cDrvNames)
    fprintf(cDrvNames{kk});
    fprintf(': \t %.2e\n', vShot(kk));
end

mInput=mDrv'*pinv(mSens);
mShot=diag(sqrt(2*hP*nu*vProbePower));
mAllShot=mInput*mShot;
vShot=zeros(length(selectedprobeNames),1);
for kk=1:length(cDrvNames)
    vShot(kk)=norm(mAllShot(kk,:));     % shot noise for each error signal [rad/rtHz]
end
fprintf('Shot noise for each mirror [rad/rtHz]\n');
for kk=1:length(driveNames)
    fprintf(driveNames{kk});
    fprintf(': \t %.2e\n', vShot(kk));
end
fprintf('Shot noise for each mirror pitch and yaw [rad/rtHz]\n');
for kk=1:length(driveNames)
    fprintf(driveNames{kk});
    fprintf(': \t %.2e\n', vShot(kk)*sqrt(2));
end



%% Optical response of IFO in frequency space
% Set mechanical transfer functions
opt = setMechTF(opt, 'ITMX', p.tfTMPit,2);
opt = setMechTF(opt, 'ITMY', p.tfTMPit,2);
opt = setMechTF(opt, 'ETMX', p.tfTMPit,2);
opt = setMechTF(opt, 'ETMY', p.tfTMPit,2);
opt = setMechTF(opt, 'BS', p.tfBSPit,2);
opt = setMechTF(opt, 'PRM', p.tfPRMPit,2);
opt = setMechTF(opt, 'PR2', p.tfPR2Pit,2);
opt = setMechTF(opt, 'PR3', p.tfPR3Pit,2);
opt = setMechTF(opt, 'SR2', p.tfSR2Pit,2);
opt = setMechTF(opt, 'SR3', p.tfSR3Pit,2);
opt = setMechTF(opt, 'SRM', p.tfSRMPit,2);

save([saveDir,'parameters'],'p');  %save parameter set
save([saveDir,'opt'],'opt');       %save optickle model

% tickle01
nfreq=100;
freq=logspace(-2,2,nfreq);
[sigAC, mMech]=tickle01(opt,[],freq);

iselPrb=cellfun(@(x)getProbeNum(opt,x),selectedprobeNames);
mOpt=sqrt(2/pi)*sigAC(iselPrb,iDrv,:);  % sqrt(2/pi) needed for Optickle
save([saveDir,'freq'],'freq');  % frequency space
save([saveDir,'mOpt'],'mOpt');  % "IFO Optical Response" (see LIGO-T0900511 Figure 10)

mMech = mMech(iDrv,iDrv,:);
save([saveDir,'mMech'],'mMech');  % modification of the actuator TF under radiation pressure

%% Calibrate beam spot motion
iFieldonMirror=cellfun(@(x)getFieldProbed(opt,x),BSMprobeNames);
z=real(vBasis(iFieldonMirror,2));
z0=imag(vBasis(iFieldonMirror,2));
w=sqrt(lambda*z0/pi).*sqrt(1+(z./z0).^2);  % beam size on each mirror
P=sigDC(cellfun(@(x)getProbeNum(opt,x),BSMprobeNames));  % power on each mirror

for kk=1:length(BSMprobeNames)
    strs=strsplit(BSMprobeNames{kk},'_');
    fprintf('%s: \t %g mm \t %g \t %g \n', strs{1},w(kk)*1e3,z(kk),z0(kk));
end

mBSMAB=sigAC(iBSMPrb,iDrv,:);
for kk=1:length(BSMprobeNames)
    mBSMAB(kk,:,:)=mBSMAB(kk,:,:)*w(kk)/(2*P(kk));  % calibrate signal from BSM probes [W] to BSM [m] 
end

iBSMfr=[];
for kk=1:length(BSMprobeNames)
    if strfind(BSMprobeNames{kk},'_BSMA')>0
        mBSMAB(kk+1,:,:)=(mBSMAB(kk+1,:,:)+mBSMAB(kk,:,:))/2;  % add frA and frB motion
    else
        iBSMfr=[iBSMfr,kk];
    end
end
mBSM=mBSMAB(iBSMfr,:,:);

abs(mBSM(:,:,100))
save([saveDir,'mBSM'],'mBSM');  % TF from each mirror angular motion to beam spot motion on each mirror [m/rad]

%% Modified mechanical TFs for TM (torque on TM to TM angle)
H_data=importdata(p.TM_Hact_TFdata);
H=zeros(nfreq,3);
H(:,1:2)=change_sampling([H_data(:,1),H_data(:,2)],freq);
H(:,1:2:3)=change_sampling([H_data(:,1),H_data(:,3)],freq);
Hact=squeeze(H(:,2).*exp(i.*H(:,3)/180*pi));

mMIRRORtoDOF=inv(mDrv');
MechSOFT=zeros(nfreq,1);
MechHARD=zeros(nfreq,1);

for k=1:nfreq
    mMechDOF=mMIRRORtoDOF*mMech(:,:,k)*inv(mMIRRORtoDOF);
    MechSOFT(k)=mMechDOF(1,1);  %CSOFT
    MechHARD(k)=mMechDOF(2,2);  %CHARD
end

figure();
plotbode(freq',Hact,MechSOFT.*Hact,MechHARD.*Hact);
legend('no radiation pressure','SOFT','HARD','Location','NorthEast');
subplot(2,1,1);
ylim([1e-6,1e2]);
set(gca,'YTick',logspace(-6,2,9));
if strfind(p.modelName,'pitch')>0
    angleName='pitch';
elseif strfind(p.modelName,'yaw')>0
    angleName='yaw'
end
title(['Torque from RM to angle of TM (',angleName,', Pintracav=', num2str(Pintracav/1e3,'%.1f'),' kW, gITM=',num2str(1-p.Larm*p.ITMXChr,'%.2f'),', gETM=',num2str(1-p.Larm*p.ETMXChr,'%.2f'),')']);
saveas(gcf,[saveDir,'SOFTHARDopto-mechanicalTF.png'])