close all
clear all



%data = load('tethys_pings_bound.log');
%x = data(:,1);
%y = data(:,2);
%r = data(:,3);
%z = zeros(length(x),1);  %All measurements are made from surface.
%
%
%
%%Make best guess
%x0_1stguess = mean(x);
%y0_1stguess = mean(y);
%z0_1stguess = -750;
%c0_1stguess = 1500;

% Initial guess of location of Tethys and sound speed
x0_1stguess = 10; % for simulated data
y0_1stguess = 5; % for simulated data
z0_1stguess = -700; % for simulated data
c0_1stguess = 1500; % for simulated data

% True values of location of Tethys and sound speed
x0 = 0; % for simulated data
y0 = 0; % for simulated data
z0 = -700; % for simulated data
c0 = 1500; % for simulated data
%

%
%Load Data
%Or fake data
%
X_true = [100 200 300 400 500 600 700 800]'; % for simulated data
%X_true = data(:,1);
Y_true = [100 300 500 700 900 1100 1300 1500]'; % for simulated data
%Y_true = data(:,2);
Z_true = z0 * ones(length(X_true),1); % for simulated data
%Z_true = zeros(length(X_true),1);

R_true = sqrt((X_true-x0).^2+(Y_true-y0).^2+(Z_true-z0).^2); % for simulated data
Tij_hat = R_true / c0; % for simulated data
%
Tij0 = sqrt((X_true-x0_1stguess).^2+(Y_true-y0_1stguess).^2+(Z_true-z0_1stguess).^2) / ...
    c0_1stguess; 
%

range_error = 0; % for simulated data
R_measured = R_true + (rand(length(X_true),1)-0.5)*range_error; % for simulated data
%R_measured = data(:,3); % measured ranges from boat

Tij = R_measured / c0;
%
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;
%
XX = B\F;
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))]);
%
%
figure;
plot(Y_true,X_true,'x',y0+XX(2),x0+XX(1),'o');
print('-djpeg','TethysLeastSquares');
%
