format long;

% Vertex location
vertex.phi = (19+36/60+47.9017/3600)*pi/180; % latitude
vertex.lambda = (77+1/60+51.0997/3600)*pi/180; % longitude
vertex.h = 440;

% X-end location
x.phi = (19+38/60+43.1430/3600)*pi/180; % latitude
x.lambda = (77+0/60+47.4656/3600)*pi/180; % longitude
x.h = 440;

% Y-end location
y.phi = (19+35/60+47.5998/3600)*pi/180; % latitude
y.lambda = (76+59/60+49.4969/3600)*pi/180; % longitude
y.h = 440;

% Use WGS 84 earth model to calculate azimuth
% Matlab has predefined WGS 84 model; The access to the model as well
% azimuth function to calculate the azimuths requires MAPPING TOOLBOX
%earthModel = referenceEllipsoid('wgs84');
%x_arm_azimuth = azimuth(vertex.phi*180/pi, vertex.lambda*180/pi, x.phi*180/pi, x.lambda*180/pi, earthModel)*pi/180;
%y_arm_azimuth = azimuth(vertex.phi*180/pi, vertex.lambda*180/pi, y.phi*180/pi, y.lambda*180/pi, earthModel)*pi/180;

% The azimuth values were obtained from the website https://geodesyapps.ga.gov.au/vincenty-inverse
% by provies GPS coordinates (could also be done using matlab's azimuth but requires special
% MAPPING TOOLBOX
x_arm_azimuth = (332+23/60+3.64/3600)*pi/180;
y_arm_azimuth = (242+23/60+0.71/3600)*pi/180;

% By design we don't expecct to have any slope, so we are setting them to zero;
% This will be changed when we have as-built measurements
x_arm_altitude = 0;
y_arm_altitude = 0;

% By design the location of the mid-point is at the middle
x_arm_mid = 2000;
y_arm_mid = 2000;

% WGS 84 earth model
a = 6378137;
b = 6356752.314;

R = a^2/sqrt(a^2*cos(vertex.phi)^2+b^2*sin(vertex.phi)^2);
vertex.E_x = (R+vertex.h)*cos(vertex.phi)*cos(vertex.lambda);
vertex.E_y = (R+vertex.h)*cos(vertex.phi)*sin(vertex.lambda);
vertex.E_z = (b^2*R/a^2+vertex.h)*sin(vertex.phi);

% calculation of units vectors for arms follows T010110
psi_x = mod(450*pi/180 - x_arm_azimuth,2*pi);
psi_y = mod(450*pi/180 - y_arm_azimuth,2*pi);
omega_x = x_arm_altitude;
omega_y = y_arm_altitude;

x_arm_direction_x = -cos(psi_x)*cos(omega_x)*sin(vertex.lambda)-sin(psi_x)*cos(omega_x)*cos(vertex.lambda)*sin(vertex.phi)+sin(omega_x)*cos(vertex.phi)*cos(vertex.lambda);
x_arm_direction_y = cos(psi_x)*cos(omega_x)*cos(vertex.lambda)-sin(psi_x)*cos(omega_x)*sin(vertex.lambda)*sin(vertex.phi)+sin(omega_x)*cos(vertex.phi)*sin(vertex.lambda);
x_arm_direction_z = sin(psi_x)*cos(omega_x)*cos(vertex.phi)+sin(omega_x)*sin(vertex.phi);

y_arm_direction_x = -cos(psi_y)*cos(omega_y)*sin(vertex.lambda)-sin(psi_y)*cos(omega_y)*cos(vertex.lambda)*sin(vertex.phi)+sin(omega_y)*cos(vertex.phi)*cos(vertex.lambda);
y_arm_direction_y = cos(psi_y)*cos(omega_y)*cos(vertex.lambda)-sin(psi_y)*cos(omega_y)*sin(vertex.lambda)*sin(vertex.phi)+sin(omega_y)*cos(vertex.phi)*sin(vertex.lambda);
y_arm_direction_z = sin(psi_y)*cos(omega_y)*cos(vertex.phi)+sin(omega_y)*sin(vertex.phi);

fprintf('LAL_LIO_4K_DETECTOR_LONGITUDE_RAD \t %.11f\n', vertex.lambda);
fprintf('LAL_LIO_4K_DETECTOR_LATITUDE_RAD \t %.11f\n', vertex.phi);
fprintf('LAL_LIO_4K_DETECTOR_ELEVATION_SI \t %f \n', vertex.h);
fprintf('LAL_LIO_4K_DETECTOR_ARM_X_AZIMUTH_RAD \t %.11f\n', x_arm_azimuth);
fprintf('LAL_LIO_4K_DETECTOR_ARM_Y_AZIMUTH_RAD \t %.11f\n', y_arm_azimuth);
fprintf('LAL_LIO_4K_DETECTOR_ARM_X_ALTITUDE_RAD \t %.11f\n', x_arm_altitude);
fprintf('LAL_LIO_4K_DETECTOR_ARM_Y_ALTITUDE_RAD \t %.11f\n', y_arm_altitude);
fprintf('LAL_LIO_4K_DETECTOR_ARM_X_MIDPOINT_SI \t %.11f\n', x_arm_mid);
fprintf('LAL_LIO_4K_DETECTOR_ARM_Y_MIDPOINT_SI \t %.11f\n', x_arm_mid);
fprintf('LAL_LIO_4K_VERTEX_LOCATION_X_SI \t %.11e\n', vertex.E_x);
fprintf('LAL_LIO_4K_VERTEX_LOCATION_Y_SI \t %.11e\n', vertex.E_y);
fprintf('LAL_LIO_4K_VERTEX_LOCATION_Z_SI \t %.11e\n', vertex.E_z);
fprintf('LAL_LIO_4K_ARM_X_DIRECTION_X \t %.11f\n', x_arm_direction_x);
fprintf('LAL_LIO_4K_ARM_X_DIRECTION_Y \t %.11f\n', x_arm_direction_y);
fprintf('LAL_LIO_4K_ARM_X_DIRECTION_Z \t %.11f\n', x_arm_direction_z);
fprintf('LAL_LIO_4K_ARM_Y_DIRECTION_X \t %.11f\n', y_arm_direction_x);
fprintf('LAL_LIO_4K_ARM_Y_DIRECTION_Y \t %.11f\n', y_arm_direction_y);
fprintf('LAL_LIO_4K_ARM_Y_DIRECTION_Z \t %.11f\n', y_arm_direction_z);
