function [x,y,z,e, counter] = dMinimize( x,y,z, d, teta )

% dMinimize - minimizes square error of distances 
%	[x,y,z, e,counter] = dMinimize( x0,y0,z0, d, teta )
%	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
%	teta  - NxN matrix with values [0..1], 0 representing bad  data in matrix d
%                                              1 representing good data in matrix d
%	x,y,z - result points
%	e - sum of errors
%	counter - number of iterations done

% 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);

% Levenbergh-Marquardt method
% 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 = 0.1;	niy = 0.1; 	niz = 0.1;
% Solving for changes
counter = 0;

e0 = dErrorF(x,y,z, d, teta);


while( counter < 20 ),
disp([counter]);
[e, gx,gy,gz, Gx,Gy,Gz] = dErrorF(x,y,z, d, teta);

if det(Gx) <= 0,
	while( det(Gx+nix*eye(n))<=0 ), nix = 4*nix;	end;
	Gx = Gx+nix*eye(n);
end;
dx = - Gx\gx; 
ex = dErrorF(x+dx,y,z,d, teta);
rx = ( ex-e )/( dx'*gx + 0.5*dx'*Gx*dx );
if (rx<0.25), nix=4*nix; elseif (rx>0.75), nix=0.5*nix; end;
if (ex < e), x = x + dx; e = ex; end;

if det(Gy) <= 0,
	while( det(Gy+niy*eye(n))<=0 ), niy = 4*niy;	end;
	Gy = Gy+niy*eye(n);
end;
dy = - Gy\gy;
ey = dErrorF(x,y+dy,z,d, teta);
ry = ( ey-e )/( dy'*gy + 0.5*dy'*Gy*dy );
if (ry<0.25), niy=4*niy; elseif (ry>0.75), niy=0.5*niy; end;
if (ey < e), y = y + dy; e = ey; end;

if det(Gz) <= 0,
	while( det(Gz+niz*eye(n))<=0 ), niz = 4*niz;	end;
	Gz = Gz+niz*eye(n);
end;
dz = - Gz\gz;
ez = dErrorF(x,y,z+dz,d, teta);
rz = ( ez-e )/( dz'*gz + 0.5*dz'*Gz*dz );
if (rz<0.25), niz=4*niz; elseif (rz>0.75), niz=0.5*niz; end;
if (ez < e), z = z + dz; e = ez; end;

counter = counter + 1;

if (counter > 500),
	break;
end;
end; %while

return;

%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))