%This program gives the spot position on the two PSDs as a function of the wire position.
%All length dimensions are in mm.  Angles are in degrees.

%Inputs
l1 = 30;
l2 = 3;
alpha = 45;
s1 = [-1.5:.01:1.5];
s2 = s1;

%Compute x and y
alpha_rad = alpha * pi / 180.;
Delta = ((s1 + s2) / l2) * (sin(alpha_rad)^2 - cos(alpha_rad)^2) + ...
            2 * (-1 + (s1 .* s2) / l2^2) * cos(alpha_rad) * sin(alpha_rad);
x = ((l1 / l2) ./ Delta) .* (s2 - s1) * sin(alpha_rad);
y = ((l1 / l2) ./ Delta) .* ((s2 + s1) * cos(alpha_rad) - ...
        2 * (s1 .* s2 / l2) * sin(alpha_rad));
    
%Plot the results
figure
plot(s1, x)
xlabel('S1 (mm)')
ylabel('Xwire (mm)')
title('Wire X Position vs Left Sensor Image Position, S2 = 0')
grid on

figure
plot(s1, y)
xlabel('S1 (mm)')
ylabel('Ywire (mm)')
title('Wire Y Position vs Left Sensor Image Position, S2 = 0')
grid on

%Add noise to s1 and s2, recompute x and y
alpha_rad = alpha * pi / 180.;
Delta = ((s1 + s2) / l2) * (sin(alpha_rad)^2 - cos(alpha_rad)^2) + ...
            2 * (-1 + (s1 .* s2) / l2^2) * cos(alpha_rad) * sin(alpha_rad);
x = ((l1 / l2) ./ Delta) .* (s2 - s1) * sin(alpha_rad);
y = ((l1 / l2) ./ Delta) .* ((s2 + s1) * cos(alpha_rad) - ...
        2 * (s1 .* s2 / l2) * sin(alpha_rad));
    
%Plot the results
figure
plot(s1, x)
xlabel('S1 (mm)')
ylabel('Xwire (mm)')
title('Wire X Position vs Left Sensor Image Position, S2 = 0')
grid on

figure
plot(s1, y)
xlabel('S1 (mm)')
ylabel('Ywire (mm)')
title('Wire Y Position vs Left Sensor Image Position, S2 = 0')
grid on