function [x,y,z] = dMinLin( x,y,z, d, tol )

% dMinimize - minimizes square error of distances 
%	[x,y,z] = dMinimize( x0,y0,z0, d, epsilon )
%	x0, y0, z0 - 1D column vectors, initial estimates of the points coordinates.
%	d -  matrix of distances between points, d(i,j)=distance between points i and j
%	epsilon - required precision
%	x,y,z - result points

% A matrix with distances between n 3D points is given (d). 
% The problem is to find cartesian coordinates of the points.
% For further explanations look at the end of the file

n = size(d,1);


% Newton method - works when close to solution
% Algorithm:
% 1. Calculate g, G where g=d(e^2)/dx, G=d^2(e^2)/(dxi dxj),  e is error
% 2. While G+ni*I is not positive definite, ni=ni*4
% 3. Solve (G + ni*I) * d = -g    for d
% 4. r = (f(x+d)-f(x))/(q(x+d)-q(x)), where f is squared error, q is quadratic approximation of f
% 5. if r<0.25, ni=4*ni; elseif r>0.75, ni=0.5*ni;
% 6. if r>0, x = x + d
% 7. Goto 1 until error<epsilon
% 
%
nix = 500000;	niy = 500000; 	niz = 500000;

% Solving for changes
[e, gx,gy,gz, Gx,Gy,Gz] = dErrorF(x,y,z,d,tt);
epsilon=tol;
disp([e epsilon]);
counter = 0;
while( maxe>epsilon ),

while( det(Gx+nix*eye(n))<0 ), nix = 4*nix;	end;
while( det(Gy+niy*eye(n))<0 ), niy = 4*niy;	end;
while( det(Gz+niz*eye(n))<0 ), niz = 4*niz;	end;
dx = - 1/nix*gx; 
dy = - 1/niy*gy;
dz = - 1/niz*gz;
rx = ( dErrorF(x+dx,y,z,d,tt)-e )/( dx'*gx + 0.5*dx'*Gx*dx );
ry = ( dErrorF(x,y+dy,z,d,tt)-e )/( dy'*gy + 0.5*dy'*Gy*dy );
rz = ( dErrorF(x,y,z+dz,d,tt)-e )/( dz'*gz + 0.5*dz'*Gz*dz );
if (rx<0.25), nix=4*nix; elseif (rx>0.75), nix=0.5*nix; end;
if (ry<0.25), niy=4*niy; elseif (ry>0.75), niy=0.5*niy; end;
if (rz<0.25), niz=4*niz; elseif (rz>0.75), niz=0.5*niz; end;
if (rx>0 & 0*rx<100), x = x + dx;  else, nix = 4*nix; end;
if (ry>0 & 0*ry<100), y = y + dy;  else, niy = 4*niy; end;
if (rz>0 & 0*rz<100), z = z + dz;  else, niz = 4*niz; end;
[e, gx,gy,gz, Gx,Gy,Gz, maxe] = dErrorF(x,y,z,d);
counter = counter + 1;
disp([e, rx, nix, counter] );

end; %while

%Additional coments:
% Problem definition:
%	A n*n matrix (d) with distances between the points is given so that
%	d(i,j) = distance between point i and point j
%	d2(i,j) = d(i,j)^2 = (x(i)-x(j))^2+(y(i)-y(j))^2+(z(i)-z(j))^2 + noise
%	matrix d2 should be idealy symetric, but due to the noise, it is not.
%
%       The minimization function is squared error: 
%		f = sum_i(sum_j( e(i,j)^2 ))
%	The first derivative 
%		df(x)/dx(i) = g(i) = sum_j( 4*(x(i)-x(j))*e(i,j) ); 
%	The second derivative 
%		d^2 f(x)/ dx(i)dx(j) = G(i,j) = -4*[ e(i,j) + 2*(x(i)-x(j))^2 ]
%	Taylor series is:
%		f(x+h) = f(x) + h'*g(x) + 1/2*h'*G(x)*h + o(h'*h)
%	The quadratic approximation is:
%		q(x)   = f(x)
%		q(x+h) = f(x) + h'*g(x) + 1/2*h'*G(x)*h
%	=>	Dq     = h'*g(x) + 1/2*h'*G(x)*h
%	Convergence ratio:
%		r = Df / Dq = ( f(x+h)-f(x) ) / Dq
%
% 	Accuracy: (xi-xj) = x0+2*eps =>
%	(x(i)-x(j))^2+(y(i)-y(j))^2+(z(i)-z(j))^2-d(i,j)^2=
%	= (x0+2*eps)^2+(y0+2*eps)^2+(z0+2*eps)^2 - d^2 = 
%	= (x0^2+y0^2+z0^2 - d^2) + 4*eps*(x0+y0+z0) + 3*4*eps^2 =~
%	=~ 4*eps*(x0+y0+z0) =~ 3*4*eps*mean(x) =~ 12*eps*mean(d/2)=6*eps*mean(d) =>
%	=> sum(sum( e^2 )) =~ (6*eps)^2 * sum(sum(d^2))