close all
clear all

X0 = [ 0 100 100 0];
Y0 = [0 0 100 100];

C0 = 1400;

N = length(X0);

Tij = zeros(N,N);
for i = 1:N
    for j = 1:N
     Tij(i,j) = 2*sqrt((X0(i)-X0(j))^2+(Y0(i)-Y0(j))^2)/C0;       
    end
end

%Introduce random error 
TijMean =  mean(mean(Tij));
Error = .1*TijMean*(rand(N)-.5);
Tij = Tij+Error;

[c nu] = SpeedOfSound(X0,Y0,Tij);


C = c;

%[X Y nu] = ComputePositions( Tij, c, X0, Y0);
epsilon = .1;  %Iterate until the norm of the errors is less than this value.

N = length(X0);
if(N ~= length(Y0))
    error('Length of X0 not equal to length of Y0');
end

if((N ~= size(Tij,1)) || (N ~= size(Tij,2)))
     error('Size of Tij not consistent with length of X0 and Y0');
end   



t0_ij = zeros(N,N);
b1_ij = zeros(N,N);
b2_ij = zeros(N,N);
b3_ij = zeros(N,N);
b4_ij = zeros(N,N);

for i = 1:N
    for j = 1:N
      if(i ~= j)
          t0_ij(i,j) = 2*sqrt((X0(i)-X0(j))^2+(Y0(i)-Y0(j))^2)/C;  %Two way travel times based on initial guesses.
          dtij_dxi(i,j) = (X0(j)-X0(i))/(t0_ij(i,j)*(C^2));
          dtij_dxj(i,j) = -dtij_dxi(i,j);
          dtij_dyi(i,j) = (Y0(j)-Y0(i))/(t0_ij(i,j)*(C^2));
          dtij_dyj(i,j) = -dtij_dyi(i,j);
      end
    end
end


f = zeros((N-1)*N,1);
B = zeros((N-1)*N,2*N);
k = 0;
for i = 1:N
    for j = 1:N
      if(i ~= j)
        k = k+1;
        f(k) = t0_ij(i,j)-Tij(i,j);
        B(k,i) = dtij_dxi(i,j);
        B(k,j) = dtij_dxj(i,j);
        B(k,N+i) = dtij_dyi(i,j);
        B(k,N+j) = dtij_dyj(i,j);
      end
    end
end

dx = B\f;

for i = 1:N
        X(i) = X0(i)+dx(i);
        Y(i) = Y0(i)+dx(N+i);
end