function [x,y,z,e, counter] = dMinSimple( x,y,z, d, teta , MinChange, MaxCount)

% dMinSimple - minimizes square error of distances 
%	[x,y,z, e,counter] = dMinimize( x0,y0,z0, d, teta , MinChange, MaxCount)
%	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
%	MinChange (default=0.001) minimal change of x,y,z as condition to stop iterations
%	MaxCount  (default=50) maximal number of iteration
%	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);
if nargin < 6, MinChange = 0.001; end;
if nargin < 7, MaxCount  = 50;    end;

% Algorithm:
% 1. Calculate g, G where g=d(e^2)/dx, G=d^2(e^2)/(dxi dxj),  e is error
% 2. Solve G*dx = -g for dx
% 3. If error(x+dx)<error(x) Then x = x+dx
% 4. Else apply Levenbergh-Marquardt method:
%  4a. While G+ni*I is not positive definite, ni=10*ni
%  4b. Solve (G + ni*I) * dx = -g    for dx
%  4c. If error(x+dx)<error(x) Then x = x+dx
% 5. Goto 1 until dx<epsilon
% 
%

% Solving for changes
nix = 0.1;	niy = 0.1; 	niz = 0.1;
typical = 0.5*std(x);
counter = 0;  change = 5*MinChange;
e0 = dErrorF(x,y,z, d, teta);

while( (counter < MaxCount) & (change > MinChange) ),

	[e, gx,gy,gz, Gx,Gy,Gz] = dErrorF(x,y,z, d, teta);
	flagx = 0; flagy = 0; flagz = 0;

	dx = - Gx\gx; 	e2 = dErrorF(x+dx,y,z,d, teta);
	if (e2 < e), x = dtrend(x + dx); e = e2;  flagx=1;	end;

	dy = - Gy\gy; 	e2 = dErrorF(x,y+dy,z,d, teta);
	if (e2 < e), y = dtrend(y + dy); e = e2;  flagy=1;	end;

	dz = - Gz\gz; 	e2 = dErrorF(x,y,z+dz,d, teta);
	if (e2 < e), z = dtrend(z + dz); e = e2;  flagx=1;	end;

	if ~(flagx | flagy | flagz),
		nix = 1;	niy = 1; 	niz = 1;

		while( det(Gx+nix*eye(n))<=0 ), nix = 10*nix;	end;
		Gx = Gx+nix*eye(n);
		dx = - Gx\gx; 	e2 = dErrorF(x+dx,y,z,d, teta);
		if (e2 < e | 1), x = dtrend(x + dx); e = e2;  flagx=1;	end;
	
		while( det(Gy+niy*eye(n))<=0 ), niy = 10*niy;	end;
		Gy = Gy+niy*eye(n);
		dy = - Gy\gy; 	e2 = dErrorF(x,y+dy,z,d, teta);
		if (e2 < e | 1), y = dtrend(y + dy); e = e2;  flagy=1;	end;

		while( det(Gz+niz*eye(n))<=0 ), niz = 10*niz;	end;
		Gz = Gz+niz*eye(n);
		dz = - Gz\gz; 	e2 = dErrorF(x,y,z+dz,d, teta);
		if (e2 < e | 1), z = dtrend(z + dz); e = e2;  flagx=1;	end;
	end;
	if ~(flagx | flagy | flagz),
		h = 10;
		dx = - h/mean(gx)*gx; 	e2 = dErrorF(x+dx,y,z,d, teta);
		if (e2 < e | 1), x = dtrend(x + dx); e = e2;  flagx=1;	end;
		dy = - h/mean(gy)*gy; 	e2 = dErrorF(x+dx,y,z,d, teta);
		if (e2 < e | 1), y = dtrend(y + dy); e = e2;  flagy=1;	end;
		dz = - h/mean(gz)*gz; 	e2 = dErrorF(x+dx,y,z,d, teta);
		if (e2 < e | 1), z = dtrend(z + dz); e = e2;  flagx=1;	end;
	end;
	if ~(flagx | flagy | flagz),	break; 	end;
	
	change = 0.2*change + (1-0.2)*mean( ...
		 flagx * abs(dx./max([x' ; typical*ones(1,n)])') ...
		+flagy * abs(dy./max([y' ; typical*ones(1,n)])') ...
		+flagz * abs(dz./max([x' ; typical*ones(1,n)])')  ) ;
	counter = counter + 1;

end; %while

disp([flagx  flagy  flagz  nix niy niz counter-1]);

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