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

% dMinSimple - minimizes square error of distances 
%	[x,y,z, e,counter] = dMinimize( x0,y0,z0, d, teta , MaxCount, MinChange)
%	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 < 7, MinChange = 0.001; end;
if nargin < 6, MaxCount  = 20;    end;


% Solving for changes

typical = 0.5*std(x);
counter = 0;  change = 5*MinChange;
e0 = dErrorNoZero(x,y,z, d, teta);

h = 10;

while( (counter < MaxCount) ),
	[e, gx,gy,gz] = dErrorNoZero(x,y,z, d, teta);
	flagx = 0; flagy = 0; flagz = 0;

gg = sqrt((gx'*gx+gy'*gy+gz'*gz));
dx = - h/gg*gx;dy = - h/gg*gy;dz = - h/gg*gz;
e2 = dErrorNoZero(x+dx,y+dy,z+dz,d, teta);
if( e2<e ), x=x+dx; y=y+dy; z=z+dz; h=h*1.25; 
else h = h/4;
end;

if(0),
	dx = - h/mean(abs(gx)+1)*gx; 	e2 = dErrorNoZero(x+dx,y,z,d, teta);
	if (e2 < e ), x = detrend(x + dx,0); e = e2;  flagx=1; end;
	dy = - h/mean(abs(gy)+1)*gy; 	e2 = dErrorNoZero(x,y+dy,z,d, teta);
	if (e2 < e ), y = detrend(y + dy,0); e = e2;  flagy=1; end;
	dz = - h/mean(abs(gz)+1)*gz; 	e2 = dErrorNoZero(x,y,z+dz,d, teta);
	if (e2 < e ), z = detrend(z + dz,0); e = e2;  flagz=1; end;

	if     ~(flagx | flagy | flagz),	h = h/4; 	
	elseif  (flagx & flagy & flagz),	h = h*1.25;
	else     	                        h = h/1.25;
  	end;
	
a=0;
	change = a*change + (1-a)*max( ...
		 flagx * (dx./max([x' ; typical*ones(1,n)])').^2 ...
		+flagy * (dy./max([y' ; typical*ones(1,n)])').^2 ...
		+flagz * (dz./max([x' ; typical*ones(1,n)])').^2  ) ;
end; %if(0)
	counter = counter + 1;
%flagx*max(abs(dx))+ flagy*max(abs(dy))+ flagz*max(abs(dz))
plot(counter,sqrt((dx'*dx+dy'*dy+dz'*dz)) ,'o');

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( teta(i,j)*e(i,j)^2 ))