clear;
format long
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c = 299792458 ; % m/s speed of light
lmd = 1.064*10^-6; % m, wavelength Nd:YAG laser
%lmd = 514*10^-9; % m, wavelength Nd:YAG laser % Gerhard thesis 
%%%%GEO MC\
L = 4.0750;
g1 = 1;
R2 = 6.72;
g2 = 1-L/R2;

%%% AEI 10m
L = 10.6;
g1 = 1;
R2 = 37.8;
g2 = 1-L/R2;

wo = (L*lmd/pi)^0.5.*(g1.*g2.*(1-g1.*g2)./((g1+g2-2.*g1.*g2).^2)).^0.25%waist size (radius)

zo = pi.*wo.^2./lmd;
size = wo.*(1+(L./zo).^2).^0.5% end spot size(radius)

%% AEI 10m
%eps = 1.0e-9;
dx = [-10.0511 0 10.0511 -10.0511 0 -19.7703 37.8 -19.7703 -19.7703 0]';
dar = [1.00544 -1.36968 1.00544 0 1.00544  0.701889 0 -0.701889 0 0.701889]';
daa = [2 0 0 1 1 1.43 0 0 0.714 0.714 ]';
da = dar - daa;

%%% check it with GEO MC
%dx = [-4.0001 0 4.0001  -4.0001 0 1.942 -6.720 1.942  1.942 0]';
%dar = [1.028 -2.542 1.028 0 1.028  0.701 0 -0.701 0 0.701]';
%daa = [2 0 0 1 1 1.43 0 0 0.714 0.714 ]';
%da = dar - daa;
%
%%% matches Gerhard and hartmuts result

%% check

%pdd = 3.6;% 4 lenses 
pdd = 1.75;
x = -1.849;
da = -2.524;
dx = x - (pdd)*da 

K1 = dx./wo;% In this version of c code, the output shows the wais position and angle wrp to 
%the aligned case.
K2 = da.*pi.*wo./lmd;
K = (K1.^2 + K2.^2).^0.5;

Out = atan(K2./K1).*180./pi
%%
%Hartmut theses 
%G1 = 37;
%G2 = 127;

% Gerhard thesis
%G1 = 0;
%G2 = 90;

% just something
%G1 = 45;
%G2 = G1 + 90;
G1 = 0;
G2 = G1 + 90;

near = sin((G1+Out).*pi/180)
far =  sin((G2+Out).*pi/180)



SOut = sin(Out.*pi./180);
effb = lmd*dx(7,1)/(pi*wo^2);

    
Yawbn = da(2,1)*near(2,1)/SOut(2,1)
Yawdifn =  da(4,1)*near(4,1)/SOut(4,1)
Yawcomn = da(5,1)*near(5,1)/SOut(5,1)
Pitbn = effb*sin(G1*pi/180)
Pitcomn =  da(9,1)*near(9,1)/SOut(9,1)
Pitdifn = da(10,1)*near(10,1)/SOut(10,1)


Snear = [Yawbn Yawdifn Yawcomn ; Pitbn Pitcomn Pitdifn ]

Yawb = da(2,1).*far(2,1)./SOut(2,1);
Yawdif = da(4,1).*far(4,1)./SOut(4,1);
Yawcom = da(5,1).*far(5,1)./SOut(5,1);
Pitb = effb.*sin(G2*pi/180);
Pitcom =  da(9,1).*far(9,1)./SOut(9,1);
Pitdif = da(10,1).*far(10,1)./SOut(10,1);
    
Sfar = [Yawb Yawdif Yawcom; Pitb Pitcom Pitdif ]


%%%% Check it with FINESSE results
D1 = [-1.07e-5 -1.49e-5 1.09e-5 7.80e-6 0 -7.35e-6]'
D2 = [-6.69e-6 1.61e-7 6.53e-6 -1.23e-5 2.39e-5 1.25e-5]'
Fout = atan(D1./D2).*180./pi




