clear all


global sos_lx sos_ly sos_cx sos_cy tt_lx ...
       tt_ly tt_cx tt_cy sos_sx sos_sy sos_dy

%% Survey of the PRC+SRC lengths %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% measured distances
d_MB2_MY  = 2114 + 27 + 9;
d_MB3p_MX = 2190 + 27;
d_MB1_M31 = 277;
d_M32_M22 = 1978 + 27;
d_M22_MPp = 1783 + 27;

d_MB4_S31 = 90;
d_S32_S21 = 2200;
d_S22_MS1 = 300;

% cross check measurements
d_M32_MP1  = 118;
d_MP2_MB3  = 249;
d_M23_MX1p = 251;

d_S23_MY1 = 255;

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

alpha_sr3 = 44/180*pi;  % beam angle of incidence on SR3
thick_sr3 = 9.5;        % SR3 thickness
alpha_sr2 = 8/180*pi;   % beam angle of incidnce on SR2
thick_sr2 = 9.5;        % SR2 thickness
thick_srm = 25.0;       % 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)

sos_sx = 15;        % thickness of lateral support
sos_sy = 64;        % width of the lateral support
sos_dy = 22;        % distance of lateral support corner to base corner (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)

%% Survey of the PRC length %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 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)]';
EB = 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)]';
MB3p = MB3 + [sos_dy/sqrt(2) + sos_sx/sqrt(2), sos_dy/sqrt(2) - sos_sx/sqrt(2)]';
MB4 = [+(sos_ly-sos_cy)/sqrt(2) - sos_cx/sqrt(2), +(sos_ly-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]';
MY1 = CY - [-sos_cy, sos_cx]';

%% 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), MB3p(2) - d_MB3p_MX - sos_ly + sos_cy]';
AX = CX + [0, thick_itm/2]';
DX = CX - [0, thick_itm/2]';
MX = CX + [0, sos_ly - sos_cy]';
MX1 = CX + [-sos_cx, -sos_cy]';
MX1p = MX1 + [-sos_sx, sos_dy]';

%% 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 = MB1(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]';
CP_M23 = [-tt_cx, tt_cy - tt_ly]'; 
% 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;
C2_M23 = Mrot * CP_M23;

% 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_M22 + 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_M22^2, d_M32_M22);
% 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;
M23 = C2 + C2_M23;


%% 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]';    
CP_MP1 = [-sos_cx, -sos_cy + sos_ly]';
CP_MP2 = [sos_cx, -sos_cy + sos_ly]';
CP_MPp = [-sos_cx, -sos_cy]';
CP_MP3 = [sos_cx, -sos_cy]';
% 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;
CP_MP1 = Mrot * CP_MP1;
CP_MP2 = Mrot * CP_MP2;
CP_MPp = Mrot * CP_MPp;
CP_MP3 = Mrot * CP_MP3;
% 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_MPp + 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_MPp^2, d_M22_MPp);
% compute points
AP = B2 + y*beam2;
CP = AP - CP_AP;
DP = CP + CP_DP;
MP = CP + CP_MP;
MP1 = CP + CP_MP1;
MP2 = CP + CP_MP2;
MPp = CP + CP_MPp;
MP3 = CP + CP_MP3;

%% 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(AX, DX, 'm')
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(MB3p)
draw_point(MX)
draw_point(MY)
draw_point(M31)
draw_point(M32)
draw_point(M21)
draw_point(M22)
draw_point(MPp)
draw_point(MP3)

draw_point(MP1)
draw_point(MP2)
draw_point(M23)
draw_point(MX1p)

draw_measurement(M31, MB1, 'g', 'd\_M31\_MB1')
draw_measurement(MB2, MY, 'g', 'd\_MB2\_MY')
draw_measurement(MB3p, MX, 'g', 'd\_MB3p\_MX')
draw_measurement(M32, M22, 'g', 'd\_M32\_M22')
draw_measurement(M22, MPp, 'g', 'd\_M22\_MPp')

draw_measurement(M32, MP1, 'm', 'd\_M32\_MP1')
draw_measurement(MP2, MB3, 'm', 'd\_MP2\_MB3')
draw_measurement(M23, MX1p, 'm', 'd\_M23\_MX1p')

%% Survey of the SRC length %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% SR3 (see 5)
alpha_sr3p = asin(sin(alpha_sr3)/n);
% we measure the distance from MB4 to S31. Since the suspensions are
% different, the two points are not aligned in y. The beam impact point on
% SR3 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_sr3*sin(alpha_sr3p), thick_sr3/2]';
CP_P2 = [ thick_sr3*sin(alpha_sr3p), thick_sr3/2]';
CP_P0 = [0, -thick_sr3/2]';
CP_S31 = [tt_cx, tt_cy]';      % here we assume we flipped only the mirror, not the cage
CP_S32 = [-tt_cx, tt_cy]';
% must rotate the points to use a coordinate system oriented as the global
% one, but still centered on the mirror
angles3 = -(pi/2 + alpha_sr3);
Mrot = [cos(angles3), -sin(angles3); sin(angles3), cos(angles3)];
CP_P1  = Mrot * CP_P1;
CP_P2  = Mrot * CP_P2;
CP_P0  = Mrot * CP_P0; 
CP_S31 = Mrot * CP_S31;
CP_S32 = Mrot * CP_S32;
% now we can compute the position of the center and of the other
% measurement points
CS3x = EB(1) - CP_P2(1); 
S31x = CS3x + CP_S31(1);
S31y = MB4(2) + sqrt(d_MB4_S31^2 - (MB4(1) - S31x)^2);
S31 = [S31x, S31y]';
CS3 = S31 - CP_S31;
S32 = CS3 + CP_S32;
BS3 = CS3 + CP_P1;
AS3 = CS3 + CP_P2;
DS3 = CS3 + CP_P0;

%% SR2 (see 5)
alpha_sr2p = asin(sin(alpha_sr2)/n);

% we measure the distance between S32 and S21. 
% the beam is going down with an angle 90 - 2*alpha_sr3 from horizontal. (see 4)
beam3 = [cos(pi/2 - 2*alpha_sr3), -sin(pi/2 - 2*alpha_sr3)]';

% 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_sr2*sin(alpha_sr2p), thick_sr2/2]';
CP_P2 = [ thick_sr2*sin(alpha_sr2p), thick_sr2/2]';
CP_P0 = [0, -thick_sr2/2]';
CP_S21 = [-tt_cx, tt_cy]';      % here we assume we flipped only the mirror, not the cage
CP_S22 = [tt_cx, tt_cy]';
CP_S23 = [tt_cx, tt_cy - tt_ly]'; 
% must rotate the points to use a coordinate system oriented as the global
% one, but still centered on the mirror
angles2 =   2*alpha_sr3 - alpha_sr2;
Mrot = [cos(angles2), -sin(angles2); sin(angles2), cos(angles2)];
CS2_AS2  = Mrot * CP_P1;
CS2_BS2  = Mrot * CP_P2;
CS2_DS2  = Mrot * CP_P0; 
CS2_S21 = Mrot * CP_S21;
CS2_S22 = Mrot * CP_S22;
CS2_S23 = Mrot * CP_S23;

% xs is the unknwon distance between the two impact points of the beam. L is
% the distance between measurement points
L = @(xs) sum((BS3 - CS2_AS2 + CS2_S21 + xs*beam3 - S32).^2);
% find the value of x that makes the distance equal to the measured one
xs = fzero(@(x) L(x) - d_S32_S21^2, d_S32_S21);
% compute points
AS2 = BS3 + xs*beam3;
CS2 = AS2 - CS2_AS2;
BS2 = CS2 + CS2_BS2;
DS2 = CS2 + CS2_DS2;
S21 = CS2 + CS2_S21;
S22 = CS2 + CS2_S22;
S23 = CS2 + CS2_S23;

%% SRM (see 5)

% we measure the distance between S22 and MS. 
% the beam is going up with an angle 90 - 2*alpha_sr3 + 2*alpha_sr2
beam4 = [-cos(pi/2 - 2*alpha_sr3 + 2*alpha_sr2), sin(pi/2 - 2*alpha_sr3 + 2*alpha_sr2)]';

% 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_srm/2]';
CP_P0 = [0, thick_srm/2]';
CP_MP = [0, sos_cy]';     
CP_MS1 = [-sos_cx, sos_cy ]';
CP_MS2 = [sos_cx, sos_cy ]';
CP_MSp = [-sos_cx, -sos_cy + sos_ly]';
CP_MS3 = [sos_cx, -sos_cy + sos_ly]';
% must rotate the points to use a coordinate system oriented as the global
% one, but still centered on the mirror
angles =  2*alpha_sr3 - 2*alpha_sr2 + pi;
Mrot = [cos(angles), -sin(angles); sin(angles), cos(angles)];
CS_AS  = Mrot * CP_P0;
CS_MS = Mrot * CP_MP;
CS_MS1 = Mrot * CP_MS1;
CS_MS2 = Mrot * CP_MS2;
CS_MSp = Mrot * CP_MSp;
CS_MS3 = Mrot * CP_MS3;
% ys is the unknwon distance between the two impact points of the beam. L is
% the distance between measurement points
L2 = @(ys) sum((BS2 - CS_AS + CS_MS1 + ys*beam4 - S22).^2);
% find the value of ys that makes the distance equal to the measured one
ys = fzero(@(y) L2(y) - d_S22_MS1^2, d_S22_MS1);
% compute points
AS = BS2 + ys*beam4;
CS = AS - CS_AS;
MS = CS + CS_MS;
MS1 = CS + CS_MS1;
MS2 = CS + CS_MS2;
MSp = CS + CS_MSp;
MS3 = CS + CS_MS3;

%% draw SRC part

draw_beam(AB, EB, 'm')
draw_tt(CS3, 180/pi*angles3)
draw_beam(EB, AS3, 'r')
draw_beam(AS3, DS3, 'm')
draw_beam(DS3, BS3, 'm')
draw_measurement(MB4, S31, 'g', 'd\_MB3\_S31')
draw_point(S31)
draw_point(S32)
draw_point(MB4)

draw_tt(CS2, 180/pi*angles2)
draw_point(S21)
draw_point(S22)
draw_point(S23)
draw_point(MY1)

draw_measurement(S32, S21, 'g', 'd\_S32\_S21');
draw_measurement(S23, MY1, 'm', 'd\_S23\_MY1');
draw_beam(BS3, AS2, 'r');
draw_beam(AS2, DS2, 'm');
draw_beam(DS2, BS2, 'm');

draw_sos(CS, 180/pi*angles)
draw_point(MS1)
draw_beam(BS2, AS, 'r')
draw_measurement(S22, MS1, 'g', 'd\_S22\_MS1');

xlim([-600, 2500])
ylim([-2500, 500])


%% Compute lengths (see 3)

LY = distance(AP, B2) + n*distance(B2, D2) + n*distance(D2, A2) + ...
     distance(A2, B3) + n*distance(B3, D3) + n*distance(D3, A3) + ...
     distance(A3, AB) + n*distance(AB, DB) + distance(DB, AY) + ...
     n*distance(AY, DY);
LX = distance(AP, B2) + n*distance(B2, D2) + n*distance(D2, A2) + ...
     distance(A2, B3) + n*distance(B3, D3) + n*distance(D3, A3) + ...
     distance(A3, AB) + distance(AB, AX) + n*distance(AX, DX);

LPRC = (LX+LY)/2;
SCHN = LX-LY;

LSX = distance(AS, BS2) + n*distance(BS2, DS2) + n*distance(DS2, AS2) + ...
      distance(AS2, BS3) + n*distance(BS3, DS3) + n*distance(DS3, AS3) + ...
      distance(AS3, EB) + n*distance(EB, AB) + distance(AB, AX) + ...
      n*distance(AX, DX);
LSY = distance(AS, BS2) + n*distance(BS2, DS2) + n*distance(DS2, AS2) + ...
      distance(AS2, BS3) + n*distance(BS3, DS3) + n*distance(DS3, AS3) + ...
      distance(AS3, EB) + n*distance(EB, AB) + n*distance(AB, DB) + ...
      distance(DB, AY) + n*distance(AY,  DY);
  
LSRC = (LSX+LSY)/2;

fprintf(2, '\n\nReconstructed lengths:\n\n');
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);
fprintf(2, 'LSX   = %g mm\n', LSX);
fprintf(2, 'LSY   = %g mm\n', LSY);
fprintf(2, 'LSRC  = %g mm\n', LSRC);

% consistency check distances% consistency check distances
fprintf(2, '\n\nConsistency checks:\n\n');
fprintf(2, 'M32_MP1  = %g mm \t(measured = %g mm, error = %.2g mm)\n', ...
    distance(M32, MP1), d_M32_MP1, distance(M32, MP1) - d_M32_MP1);
fprintf(2, 'MP2_MB3  = %g mm \t(measured = %g mm, error = %.2g mm)\n', ...
    distance(MP2, MB3), d_MP2_MB3, distance(MP2, MB3) - d_MP2_MB3);
fprintf(2, 'M23_MX1p = %g mm \t(measured = %g mm, error = %.2g mm)\n', ...
    distance(M23, MX1p), d_M23_MX1p, distance(M23, MX1p) - d_M23_MX1p);
fprintf(2, 'M23_MY1  = %g mm \t(measured = %g mm, error = %.2g mm)\n', ...
    distance(S23, MY1), d_S23_MY1, distance(S23, MY1) - d_S23_MY1);