global sos_lx sos_ly sos_cx sos_cy tt_lx tt_ly tt_cx tt_cy

%% Survey of the PRC length

%% measured distances
d_MB2_MY  = 2000.0;
d_MB3_MX  = 2000.0;
d_MB1_M31 = 400.0;
d_M32_M21 = 3000.0;
d_M22_MP  = 2000.0;

%% parameters
thick_bs = 25.4;        % BS thickness
n = 1.45;               % fused silica refractive index
thick_itm = 25.4;       % ITM thickness
thick_pr3 = 9.5;        % PR3 thickness
alpha_pr3 = 41/180*pi;  % beam angle of incidence on PR3
thick_pr2 = 9.5;        % PR2 thickness
alpha_pr2 = 1.5/180*pi; % beam angle of incidence on PR2
thick_prm = 25.4;       % PRM thickness

%% Suspension cage dimensions
% dimensions and distances of SOS [mm]
sos_lx = 104.8; % width
sos_ly = 108.0; % height
sos_cx = sos_lx/2;  % mirror center position
sos_cy = 58.8;      % mirror center position (distance from front side)
% dimensions and distances of TT [mm]
tt_lx = 124.0;  % width
tt_ly = 89.9;   % height
tt_cx = tt_lx/2;    % mirror center position
tt_cy = 27.9;       % mirror center position (distance from front side)

%% Beam splitter (see 3, 1)
alphaB = asin(sin(pi/4)/n);
% mirror center is positioned at 0,0
CB = [0,0]';
% coordinates of beam impact points
AB = thick_bs/2 * sqrt(2)/2 * [-1,-1]';
DB = AB + thick_bs/cos(alphaB) * [cos(pi/4 - alphaB), sin(pi/4 - alphaB)]';
% coordinate of measured point 1
MB1 = [-sos_cy/sqrt(2) - sos_cx/sqrt(2), -sos_cy/sqrt(2) + sos_cx/sqrt(2)]';
MB2 = [+(sos_ly-sos_cy)/sqrt(2) + sos_cx/sqrt(2), +(sos_ly-sos_cy)/sqrt(2) - sos_cx/sqrt(2)]';
MB3 = [-sos_cy/sqrt(2) + sos_cx/sqrt(2), -sos_cy/sqrt(2) - sos_cx/sqrt(2)]';

%% ITMY (see 3)
% we measure from MB2 to front surface of support. Mirror center is aligned
% with beam exit point on BS. Mirror front is on the far side
CY = [MB2(1) + d_MB2_MY + sos_ly - sos_cy , DB(2)]';
AY = CY - [thick_itm/2, 0]';
DY = CY + [thick_itm/2, 0]';
MY = CY - [sos_ly-sos_cy, 0]';

%% ITMX (see 3)
% we measure from MB3 to front surface of support. Mirror center is aligned
% with beam exit point on BS. Mirror front is on the far side
CX = [AB(1), MB3(2) - d_MB3_MX - sos_ly + sos_cy]';
AX = CX - [0, thick_itm/2]';
MX = CX + [0, sos_ly - sos_cy]';

%% PR3 (see 3, 2)
alpha_pr3p = asin(sin(alpha_pr3)/n);
% we measure the distance from MB1 to M31. Since the suspensions are
% different, the two points are not aligned in y. The beam impact point on
% PR3 is aligned with the BS one

% positions of the points w.r.t to center, in a coordinate system which is
% oriented as the mirror, y increasing into the substrate
CP_P1 = [-thick_pr3*sin(alpha_pr3p), thick_pr3/2]';
CP_P2 = [ thick_pr3*sin(alpha_pr3p), thick_pr3/2]';
CP_P0 = [0, -thick_pr3/2]';
CP_M31 = [-tt_cx, tt_cy]';      % here we assume we flipped only the mirror, not the cage
CP_M32 = [tt_cx, tt_cy]';
% must rotate the points to use a coordinate system oriented as the global
% one, but still centered on the mirror
angle3 = -(pi/2 + alpha_pr3);
Mrot = [cos(angle3), -sin(angle3); sin(angle3), cos(angle3)];
CP_P1  = Mrot * CP_P1;
CP_P2  = Mrot * CP_P2;
CP_P0  = Mrot * CP_P0; 
CP_M31 = Mrot * CP_M31;
CP_M32 = Mrot * CP_M32;
% now we can compute the position of the center and of the other
% measurement points
C3y = AB(2) - CP_P1(2); 
M31y = C3y + CP_M31(2);
M31x = AB(1) - sqrt(d_MB1_M31^2 - (MB1(2) - M31y)^2);
M31 = [M31x, M31y]';
C3 = M31 - CP_M31;
M32 = C3 + CP_M32;
B3 = C3 + CP_P2;
A3 = C3 + CP_P1;
D3 = C3 + CP_P0;

%% PR2 (see 3)
alpha_pr2p = asin(sin(alpha_pr2)/n);

% we measure the distance between M32 and M21. 
% the beam is going down with an angle 2*alpha_pr3 from horizontal. (see 4)
beam = [cos(2*alpha_pr3), -sin(2*alpha_pr3)]';

% positions of the points w.r.t to center, in a coordinate system which is
% oriented as the mirror, y increasing into the substrate
CP_P1 = [-thick_pr2*sin(alpha_pr2p), thick_pr2/2]';
CP_P2 = [ thick_pr2*sin(alpha_pr2p), thick_pr2/2]';
CP_P0 = [0, -thick_pr2/2]';
CP_M21 = [-tt_cx, tt_cy]';      % here we assume we flipped only the mirror, not the cage
CP_M22 = [tt_cx, tt_cy]';
% must rotate the points to use a coordinate system oriented as the global
% one, but still centered on the mirror
angle2 = pi/2 - (2*alpha_pr3 + alpha_pr2);
Mrot = [cos(angle2), -sin(angle2); sin(angle2), cos(angle2)];
C2_A2  = Mrot * CP_P1;
C2_B2  = Mrot * CP_P2;
C2_D2  = Mrot * CP_P0; 
C2_M21 = Mrot * CP_M21;
C2_M22 = Mrot * CP_M22;

% x is the unknwon distance between the two impact points of the beam. L is
% the distance between measurement points
L = @(x) sum((B3 - C2_A2 + C2_M21 + x*beam - M32).^2);
% find the value of x that makes the distance equal to the measured one
x = fzero(@(x) L(x) - d_M32_M21^2, d_M32_M21);
% compute points
A2 = B3 + x*beam;
C2 = A2 - C2_A2;
B2 = C2 + C2_B2;
D2 = C2 + C2_D2;
M21 = C2 + C2_M21;
M22 = C2 + C2_M22;

%% PRM (see 3)

% we measure the distance between M22 and MP. 
% the beam is going up with an angle angle2-angle_pr2 from vertical. (see 4)
beam2 = [-sin(angle2 - alpha_pr2), cos(angle2 - alpha_pr2)]';

% positions of the points w.r.t to center, in a coordinate system which is
% oriented as the mirror, y increasing into the substrate
CP_P1 = [0, -thick_prm/2]';
CP_P0 = [0, thick_prm/2]';
CP_MP = [0, -sos_cy]';      % here we assume we flipped only the mirror, not the cage
% must rotate the points to use a coordinate system oriented as the global
% one, but still centered on the mirror
anglep = angle2 - alpha_pr2;
Mrot = [cos(anglep), -sin(anglep); sin(anglep), cos(anglep)];
CP_AP  = Mrot * CP_P1;
CP_DP  = Mrot * CP_P0;
CP_MP = Mrot * CP_MP;

% x is the unknwon distance between the two impact points of the beam. L is
% the distance between measurement points
L2 = @(y) sum((B2 - CP_AP + CP_MP + y*beam2 - M22).^2);
% find the value of x that makes the distance equal to the measured one
y = fzero(@(y) L2(y) - d_M22_MP^2, d_M22_MP);
% compute points
AP = B2 + y*beam2;
CP = AP - CP_AP;
DP = CP + CP_DP;
MP = CP + CP_MP;


%% Draw everything to check consistency
figure(1)
clf
draw_sos(CB, 90+45)
axis equal
draw_sos(CX, 180)
draw_sos(CY, -90)
draw_tt(C3, 180/pi*angle3)
draw_tt(C2, 180/pi*angle2)
draw_sos(CP, 180 + 180/pi*anglep)

draw_beam(DB, AY, 'r')
draw_beam(AY, DY, 'm')
draw_beam(DB, AB, 'm')
draw_beam(AB, AX, 'r')
draw_beam(AB, A3, 'r')
draw_beam(A3, D3, 'm')
draw_beam(D3, B3, 'm')
draw_beam(B3, A2, 'r')
draw_beam(A2, D2, 'm')
draw_beam(D2, B2, 'm')
draw_beam(B2, AP, 'r')

draw_point(MB1)
draw_point(MB2)
draw_point(MB3)
draw_point(MX)
draw_point(MY)
draw_point(M31)
draw_point(M32)
draw_point(M21)
draw_point(M22)
draw_point(MP)

draw_measurement(M31, MB1, 'g', 'd\_M31\_MB1')
draw_measurement(MB2, MY, 'g', 'd\_MB2\_MY')
draw_measurement(MB3, MX, 'g', 'd\_MB3\_MX')
draw_measurement(M32, M21, 'g', 'd\_M31\_M21')
draw_measurement(M22, MP, 'g', 'd\_M22\_MP')

%% Compute lengths (see 3)
LY = distance(AP, B2) + n*2*thick_pr2 + distance(A2, B3) + ...
     n*2*thick_pr3/cos(alpha_pr3p) + distance(A3, AB) + ...
     n*thick_bs/cos(alphaB) + distance(DB, AY) + n * thick_itm;
LX = distance(AP, B2) + n*2*thick_pr2 + distance(A2, B3) + ...
     n*2*thick_pr3/cos(alpha_pr3p) + distance(A3, AB) + ...
     distance(AB, AX) + + n * thick_itm;
 
LPRC = (LX+LY)/2;
SCHN = LX-LY;

fprintf(2, 'LX    = %g mm\n', LX);
fprintf(2, 'LY    = %g mm\n', LY);
fprintf(2, 'LPRC  = %g mm\n', LPRC);
fprintf(2, 'LX-LY = %g mm\n', SCHN);