function [f, gx,gy,gz, Gx,Gy,Gz] = dErrorF(x,y,z, d, teta)

% dErrorF is error function for set of points and distances between them
%	[error, gx,gy,gz, Gx,Gy,Gz] = ErrorF(x,y,z, d, teta)
%	x,y,z - vectors of cortesian coordinates of the points
%	d     - NxN matrix of distances between points
%	teta  - NxN matrix with values [0..1], 0 representing bad  data in matrix d
%                                              1 representing good data in matrix d
%	e = sum(sum(error*error'))
%	gx,gy,gz - first  derivative of squared error with respect to x(i), y(i), z(i)
%	Gx,Gy,Gz - second derivative of squared error with respect to x(i) & x(j), ..
%	error = sum_i sum_j [(xi-xj)^2+(yi-yj)^2+(zi-zj)^2 - d2(i,j)^2]^2

n = size(teta,1);
alpha = n*n / sum(sum(teta));
deltax = x*ones(1,n) - ones(n,1)*x';	%creating matrix: deltax(i,j)=x(i)-x(j)
deltay = y*ones(1,n) - ones(n,1)*y';
deltaz = z*ones(1,n) - ones(n,1)*z';
e  = deltax.^2 + deltay.^2 + deltaz.^2 - d.^2;
f  = alpha*sum(sum(teta.*(e.^2)));

if (nargout > 1 ),
e  = teta.*e + (teta.*e)';
teta = teta + teta';
gx = - alpha * 4*sum( deltax .* e )';  	%MATLAB sums comlumnwise	   
gy = - alpha * 4*sum( deltay .* e )';	   
gz = - alpha * 4*sum( deltaz .* e )';	   
Gx = alpha * (-4*e - 8*teta.*deltax.^2); Gx = Gx - diag(sum(Gx));   
Gy = alpha * (-4*e - 8*teta.*deltay.^2); Gy = Gy - diag(sum(Gy));   
Gz = alpha * (-4*e - 8*teta.*deltaz.^2); Gz = Gz - diag(sum(Gz));   
end; %if

return

% e(i,j) = (xi-xj)^2+(yi-yj)^2+(zi-zj)^2 - d2(i,j)^2
% F = error = alpha * sum_i sum_j teta(i,j)*e(i,j)^2
% 	alpha = (sum_i sum_j 1 ) / (sum_i sum_j teta(i,j) );
% =>
% d   F/dx(k)
%	= sum_i sum_j teta(i,j) d(e(i,j)^2)/dx(k) 
%	= sum_i[ teta(i,k) d(e(i,k)^2)/dx(k) ] + sum_j[ teta(k,j) d(e(k,j)^2)/dx(k) ]
%		   =   alpha * sum_j 4*(x(k)-x(j))*(teta(k,j)*e(k,j)+teta(j,k)*e(j,k))
% d^2 F/dx(k)dx(l) = d/dx(l) [ dF/dx(k) ] 
%	= alpha * sum_j{ d/dx(l)[ 4*(x(k)-x(j))*((teta(k,j)*e(k,j)+teta(j,k)*e(jl,k)) ]}
%    for k <> l
%	= alpha * d/dx(l)[ 4*(x(k)-x(l))*(teta(k,l)*e(k,l)+teta(l,k)*e(l,k)) ]
%	= alpha * { -4*( teta(k,l)*e(k,l) + teta(l,k)*e(l,k) )   -
%		    -8*( x(k)-x(l))^2 * ( teta(k,l) + teta(l,k) )  }
%    for k == l
%	= alpha * sum_j[  (teta(k,j)*e(k,j) + teta(j,k)*e(jl,k)) +
%			+ (x(k)-x(j))*(teta(k,j)*de(k,j)/dx(k) + teta(j,k)*de(j,k)/dx(k) ) ]
%	= alpha * 4 * sum_j[ (teta(k,j)*e(k,j) + teta(j,k)*e(jl,k)) + 
%			+ 2* (teta(k,j) + teta(j,k)) * (x(x)-x(j))^2]
%
%
%