function [X Y nu] = ComputePositions( Tij, C, X0, Y0)
% - This computes an estimate of the relative orientation of a number of
% transducers based upon the two-way travel times of sound between each 
% transponder and a prescribed speed of sound.  An inital guess is
% required.  The translation and orientation of the pattern that is output
% is arbitrary.  
% - Tij is a matrix that includes the measured travel times between all
% transponders.  Row i and Column j represents the two way travel time of
% the sound that originates and i and is "bounced off" j.  Elements on the
% diagonal have no meaning and are not used.
% - X0 and Y0 are vectors of the same length that specify an initial guess 
% of the transponder locations. The length of these vectors (N) is equal to
% the number of transponders.
% - C is the prescribed speed of sound.
% - X and Y are returned values that describe the relative position of the
% transponders to within an arbitrary rotation and translation. 
% - nu_x is a NXN matrix that represents the residual errors in the travel 
% times for measurement i-j.  Elements on the diagonal are meaningless.

epsilon = .01;  %Iterate until the norm of the change 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);
dtij_dxi = zeros(N,N);
dtij_dxj = zeros(N,N);
dtij_dyi = zeros(N,N);
dtij_dyj = zeros(N,N);

dx = 2*epsilon*ones(2*N,1);  %Make larger than epsilon initially

while(norm(dx) > epsilon)
 
  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) = 4*(X0(j)-X0(i))/(t0_ij(i,j)*(C^2));
          dtij_dxj(i,j) = -dtij_dxi(i,j);
          dtij_dyi(i,j) = 4*(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
        X0(i) = X0(i)+dx(i);
        Y0(i) = Y0(i)+dx(N+i);
  end

  if(m > 100) 
    error('Not converged after 100 iterations');
  end

end

X = X0;
Y = Y0;



nu = 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.