%Check for accidental resonances of HOM in the arms (maybe due to
%sidebands). At the moment there is only data for the y arm.

clear all
close all
clc


%Stuff one might change often
modeorder = 0:5;          % Look for TEMmn modes where m,n run over modeorder
Nfsr = -100:100;          % Number of FSRs to look at
range = [-0.3e6 0.3e6];   % Frequncies of interest [Hz]
L = 38.6462;              %Arm length  Koji's measurement [m] - Elog #801 
R1 = inf;                 % ITM ROC
R2x = 56.1620;            % ETM ROC in the `m' direction [m] - Elog #802
R2y = 57.3395;            % ETM ROC in the `n' direction [m] - Elog #802
f166 = 165.977195e6;      % fmod2 [Hz]
f33 = 33.195439e6;        % fmod1 [Hz]


%Speed of light
c = 299792458;
%Free spectral range
FSR = c/(2*L);
%Wavelength
lambda = 1064e-9;
%Frequncies of interest
npts = 4000;
f = linspace(range(1), range(2), npts);

%Mirror parameters
T1 = 0.005;
T2 = 1e-5;
Ref1 = 1-T1;
Ref2 = 1-T2;
r1 = sqrt(Ref1);
r2 = sqrt(Ref2);
g1 = 1-L/R1;
g2x = 1-L/R2x;
g2y = 1-L/R2y;

%Arm resonance
Pstored = T1./(1-2*r1*r2*cos(2*(2*pi*f/c)*L)+Ref1*Ref2);


%Resonance frequencies

%Higher order mode frequency shift
delfx = acos(sqrt(g1*g2x))/pi;
delfy = acos(sqrt(g1*g2y))/pi;

fres = [];
f00 = [];
modeindex = [];
labels2 = [];
for q = Nfsr
  f00 = [f00 q*FSR]; % TEM00 resonances
  for m = modeorder
    for n = modeorder
      modeindex = [modeindex m+n]; % m+n of modes
      labels2 = [labels2; m n]; % labels for plot
      fres = [fres (q+m*delfx+n*delfy)*(c/(2*L))]; % resonance frequncies
    end
  end
end

labels2 = cellstr(int2str(labels2)); %labels for plot


%To get the sidebands can just add +/-166,33 - this is an approximation 

%Matrix for plotting lines
linemat = repmat(fres,2,1);
TEM00mat = repmat(f00,2,1);

%To get the sidebands can just add +/-166,33 - this is an approximation
f00166pmat = TEM00mat+f166;
f00166mmat = TEM00mat-f166;
fres166pmat = linemat+f166;
fres166mmat = linemat-f166;

f0033pmat = TEM00mat+f33;
f0033mmat = TEM00mat-f33;
fres33pmat = linemat+f33;
fres33mmat = linemat-f33;

%Scale lines nicely
height = 1./ repmat(modeindex,2,1).^2;
[aa, bb] = find(height==Inf);
height(aa, bb) = 0;


%Normalise peak power gain to one
Pstored = Pstored/max(Pstored);


%----------------Make Plot------------------------------------------
figure(1)
semilogy(f*1e-6,Pstored,'k')
title('Sideband HOM resonances in the arms')
xlim(range*1e-6)

line(TEM00mat*1e-6, repmat([1e-6 1]',1,size(TEM00mat,2)),'Color','Black')
line(linemat*1e-6, repmat([1e-6 1]',1,size(linemat,2)).*height,'Color','Black')
 
line(f00166pmat*1e-6, repmat(get(gca,'Ylim')',1,size(TEM00mat,2)),'Color','Blue','LineWidth',4)
line(fres166pmat*1e-6, repmat([1e-6 1]',1,size(linemat,2)).*height,'Color','Blue')

line(f00166mmat*1e-6, repmat(get(gca,'Ylim')',1,size(TEM00mat,2)),'Color','Green','LineWidth',4)
line(fres166mmat*1e-6,  repmat([1e-6 1]',1,size(linemat,2)).*height,'Color','Green')

line(f0033mmat*1e-6, repmat(get(gca,'Ylim')',1,size(TEM00mat,2)),'Color','Red','LineWidth',4)
line(fres33mmat*1e-6,  repmat([1e-6 1]',1,size(linemat,2)).*height,'Color','Red')

line(f0033pmat*1e-6, repmat(get(gca,'Ylim')',1,size(TEM00mat,2)),'Color','Magenta','LineWidth',4)
line(fres33pmat*1e-6,   repmat([1e-6 1]',1,size(linemat,2)).* ...
     height,'Color','Magenta')
set(gcf,'Color','White','units','normalized','outerposition',[0 0 1 1]);

%Make text vectors for labels
scaling = height(1,:);

x = fres*1e-6;
[ind] = find(abs(x)>range(2)*1e-6);
x(ind) = 2*range(2);
y = ones(length(x),1)'.*scaling;
text(x,y,labels2,'Rotation',90)

x = (fres+f166)*1e-6;
[ind] = find(abs(x)>range(2)*1e-6);
x(ind) = 2*range(2);
y = ones(length(x),1)'.*scaling;
labels = (modeindex)';
text(x,y,labels2,'Color','Blue','Rotation',90)

x = (fres-f166)*1e-6;
[ind] = find(abs(x)>range(2)*1e-6);
x(ind) = 2*range(2);
y = ones(length(x),1)'.*scaling;
labels = (modeindex)';
text(x,y,labels2,'Color','Green','Rotation',90)

x = (fres+f33)*1e-6;
[ind] = find(abs(x)>range(2)*1e-6);
x(ind) = 2*range(2);
y = ones(length(x),1)'.*scaling;
labels = (modeindex)';
text(x,y,labels2,'Color','Magenta','Rotation',90)

x = (fres-f33)*1e-6;
[ind] = find(abs(x)>range(2)*1e-6);
x(ind) = 2*range(2);
y = ones(length(x),1)'.*scaling;
labels = (modeindex)';
text(x,y,labels2,'Color','Red','Rotation',90)


text(0.9*range(1)*1e-6,5,'Carrier')
text(.45*range(1)*1e-6,5,'+33','color','Magenta')
text(0,5,'-33','color','Red')
text(.45*range(2)*1e-6,5,'+166','color','Blue')
text(.9*range(2)*1e-6,5,'-166','color','Green')
grid
xlabel('Frequency/ MHz')
ylabel('Normalised cavity Power Gain')
ylim([1e-6 10*max(Pstored)])