%QPD Calibration
close all
%% Enter voltage readings.
X_M1_down = [4.13
4.12
4.11
4.10
4.09
4.08
4.07
4.06
4.05
4.04
4.03
4.02
4.01
4.0
3.99
3.98
3.97
3.96
3.95
3.94
3.93
3.92
3.91
3.90
3.89
3.88];


X_V_QPD_down = [448e-3
-1.02
-1.17
-2.92
-4.63
-5.91
-6.97
-8.17
-8.13
-8.52
-8.76
-8.89
-9.01
-9.08
-9.10
-9.11
-9.11
-9.12
-9.12
-9.13
-9.13
-9.12
-9.13
-9.11
-9.11
-9.10];

Y_V_QPD_down = [-1.98
-2.34
-2.14
-2.43
-3.30
-3.18
-3.40
-4.24
-4.28
-4.47
-4.77
-5.27
-5.45
-5.44
-5.85
-5.91
-5.93
-6.16
-6.18
-6.34
-6.49
-6.49
-6.61
-6.55
-6.70
-6.46];

X_M1_up = [4.13
4.14
4.15
4.16
4.17
4.18
4.19
4.20
4.21
4.22
4.23
4.24
4.25
4.26
4.27
4.28
4.29
4.30
4.31
4.32
4.33
4.34
4.35
4.36
4.37
4.38
4.39
4.40
4.41
4.42
4.43
4.44
4.45
4.46
4.47
4.48
4.49
4.50
4.51
4.52
4.53
4.54
4.55
4.56
4.57
4.58];

X_V_QPD_up = [239e-3
252e-3
2.34
4.46
4.26
5.62
7.80
7.81
8.45
8.82
9.19
9.28
9.37
9.36
9.37
9.44
9.47
9.50
9.49
9.51
9.55
9.58
9.62
9.66
9.65
9.63
9.69
9.69
9.70
9.70
9.71
9.71
9.72
9.74
9.73
9.74
9.75
9.74
9.76
9.74
9.73
9.74
9.73
9.72
9.71
9.70];

Y_V_QPD_up = [-1.77
-1.84
-1.60
-1.17
-1.11
-835e-3
-600e-3
-321e-3
-222e-3
70.6e-3
771e-3
1.12
1.88
2.36
2.38
2.71
3.10
3.41
3.35
3.72
4.27
4.55
4.86
5.44
5.31
5.60
5.75
6.00
6.15
6.16
6.33
6.50
6.72
7.09
6.87
7.40
7.46
7.46
7.75
7.67
7.82
7.96
8.08
8.33
8.43
8.50];

%% Add Together Vectors and flip ones where we put the value on the stage down.
X_M1 = cat(1, flip(X_M1_down), X_M1_up);
X_V_QPD = cat(1, flip(X_V_QPD_down), X_V_QPD_up);
Y_V_QPD = cat(1, flip(Y_V_QPD_down), Y_V_QPD_up);

X_M1_m = X_M1 .* 0.0254 .* 0.1;%m because readings are actually fractions of an inch.
X_M1_mm = X_M1_m .* 1000;
%% Plot data
figure(1)
plot(X_M1_mm-min(X_M1_mm), Y_V_QPD, 'g*')
xlabel('M1 lateral position [mm]')
ylabel('Y Voltage on QPD')
title('Pitch calibration of QPD')

figure(2)
plot(X_M1_mm-min(X_M1_mm), X_V_QPD, 'b*')
xlabel('M1 lateral position [mm]')
ylabel('X Voltage on QPD')
title('Yaw calibration of QPD')

%% Offset x data by lowest value

delta_X = X_M1_mm-min(X_M1_mm);
%% Select data for fitting

%between X_M1=4.05 and 4.21
lower_lim = 4.05 .* 0.0254 .* 1000.*0.1;%factor 0.1 because scale is in fractions of an inch
up_lim = 4.21 .* 0.0254 .* 1000.*0.1;
indexx = find((delta_X >= (lower_lim-min(X_M1_mm))) & (delta_X <= (up_lim-min(X_M1_mm))));
x_pos_linear = delta_X(indexx);
x_V_linear = X_V_QPD(indexx);

%% Select data for fitting
lower_y_lim = 0.2794;%mm
upper_y_lim = 1.2446;%mm
indexy = find((delta_X >= lower_y_lim) & (delta_X <= upper_y_lim));
x_pos_y_linear = delta_X(indexy);
y_V_linear = Y_V_QPD(indexy);


%% Fitting X meas
P = polyfit(x_pos_linear,x_V_linear,1);
yfit = P(2) + P(1).*x_pos_linear;
formula_str = sprintf("y = %.1f + %.1f * x", P(2), P(1));

%% Fitting Y meas
Q = polyfit(x_pos_y_linear, y_V_linear,1);
y2fit = Q(2) + Q(1).*x_pos_y_linear;
formula_str2 = sprintf("y = %.1f + %.1f * x", Q(2), Q(1));
 %% Plot fit
figure(3)
plot(delta_X, X_V_QPD, 'g*', x_pos_linear, yfit, 'r-', 'LineWidth', 3)
legend('X calib data', formula_str)
xlabel('X Pos on QPD [mm]')
ylabel('X Voltage on QPD [V]')
% legend('X pos in [inches]')
title('Horizontal beam motion to X motion on QPD')
fontsize(gcf,scale=2)

figure(4)
plot(delta_X, Y_V_QPD, 'g*', x_pos_y_linear, y2fit, 'r-', 'LineWidth', 3)
legend('Y calib data', formula_str2)
xlabel('X Pos on QPD [mm]')
ylabel('Y Voltage on QPD [V]')
% legend('X pos in [inches]')
title('Horizontal beam motion to Y motion on QPD')
fontsize(gcf,scale=2)


figure(6)
plot(x_pos_linear, yfit, 'b-', x_pos_y_linear, y2fit, 'm-', 'LineWidth', 3)
legend('X data fit', 'Y data fit')
xlabel('X Pos on QPD [mm]')
ylabel('Y Voltage on QPD [V]')
% legend('X pos in [inches]')
title('Horizontal beam motion to motion on QPD')
fontsize(gcf,scale=2)

%% Calculate Calibration for horizontal.
%delta_V_X = max(X_V_QPD) - min(X_V_QPD); %change in voltage in X while changing input beam position.
%delta_V_Y = max(Y_V_QPD) - min(Y_V_QPD); %change in voltage in Y while changing input beam position.

H_V = sqrt((Q(1).*2)+(P(1).*2)); %resultant voltage change.
angle = (Q(1))./(P(1)); 
H_calib_angle = atan(angle); %angle of vector relative to QPD axis.
H_calib_angle_deg = H_calib_angle .*180./pi; %convert to degrees.
%%
%H_calib = H_V./(max(delta_X)); %calibration for horizontal motion in mirror plane.

%% Calibration for dither amp.
X_pk_pk = 2.20;%V alog #85853
Y_pk_pk = 0.8;

H_dither_calib = sqrt((2.2.^2) + (0.8.^2));%V

d_pk_pk_h_dither = H_dither_calib./H_V;%mm of motion.