close all
clear all

%
% Load Data
data = load('LBLSimulatedSurvey.csv');
%%% data = load('test.csv');
%
% Get logged x, y, and range at multiple control points
X_true = data(:,1);
Y_true = data(:,2);
Z_true = zeros(length(X_true),1);  % Control points on surface, where Z = 0

%
% Make best guess of target location
x0_1stguess = mean(X_true);
y0_1stguess = mean(Y_true);
z0_1stguess = -700;
c0_1stguess = 1500;  % sound speed (m/s)

% Compute time residuals; travel times between control points and guessed target point
Tij0 = sqrt((X_true-x0_1stguess).^2 + (Y_true-y0_1stguess).^2 + (Z_true-z0_1stguess).^2) / c0_1stguess; 

% Measured ranges between control points and target point
R_measured = data(:,3); 

% Measured travel times between control points and target point
Tij = R_measured / c0_1stguess;

b1 = (X_true-x0_1stguess)./(Tij0*(c0_1stguess^2));
b2 = (Y_true-y0_1stguess)./(Tij0*(c0_1stguess^2));
b3 = (Z_true-z0_1stguess)./(Tij0*(c0_1stguess^2));
b4 = Tij0 / c0_1stguess;

B = [b1 b2 b3 b4];
F = Tij0-Tij;

% Compute best estimates
XX = B\F;    % What is backslash operator?

disp(['x_BestEst = ' num2str(x0_1stguess+XX(1))]);
disp(['y_BestEst = ' num2str(y0_1stguess+XX(2))]);
disp(['z_BestEst = ' num2str(z0_1stguess+XX(3))]);
disp(['c_BestEst = ' num2str(c0_1stguess+XX(4))]);

northing = y0_1stguess+XX(2);
easting = x0_1stguess+XX(1);
[lat lon] = utm2deg(single(easting), single(northing), '10 S');
disp(['Latitude = '  num2str(lat)]);
disp(['Longitude = '  num2str(lon)]);
%
%
figure;
plot(Y_true,X_true,'x',y0_1stguess+XX(2),x0_1stguess+XX(1),'o');
print('-djpeg','TethysLeastSquares');
%
