#======================================================================
#                    L I B L O O P F I T . P L 
#                    doc: Thu Mar 12 08:05:26 2015
#                    dlm: Sun Mar 15 19:46:49 2015
#                    (c) 2015 A.M. Thurnherr
#                    uE-Info: 47 22 NIL 0 0 72 2 2 4 NIL ofnI
#======================================================================

# HISTORY:
#	Mar 12, 2015: - created
#	Mar 15, 2015: - added [libQZ.pl]

require "$ANTS/nrutil.pl";
require "$ANTS/libSVD.pl";										# for circular fit
require "$ANTS/libQZ.pl";										# for ellipse fit

#----------------------------------------------------------------------
# fitCircle(\@x,\@y) => ($r,$cx,$cy)
#	\@x,\@y		references to list of data values
#	$r			circle radius
#	$cx,$cy		circle center
#----------------------------------------------------------------------

sub fitCircle($$)
{
	my($xR,$yR) = @_;
	my(@A,@V,@W);
	my($nSamp) = scalar(@{$xR});
	my($nParam) = 3;
	
	matrix(\@A,1,$nSamp,1,$nParam); 							# rows, then columns
	matrix(\@V,1,$nParam,1,$nParam);							# 3x3 matrix for circle fitting
	vector(\@W,1,$nParam);										# diag matrix of singular values output as vector
	vector(\@b,1,$nSamp);										# rhs
	vector(\@coeff,1,$nParam);									# fit coefficients
	
	for ($r=1; $r<=$nSamp; $r++) {								# circle equation c1*x + c2*y + c3 = -(x^2 + y^2)
		$A[$r][1] = $xR->[$r-1];								#	r  = sqrt((c1^2 + c2^2)/4 - c3)
		$A[$r][2] = $yR->[$r-1];								#	cx = -c1/2
		$A[$r][3] = 1;											#	cy = -c2/2
		$b[$r] = -($xR->[$r-1]**2+$yR->[$r-1]**2);
	}
	
	svdcmp(\@A,\@W,\@V);										# solve Ax = b, with x == coeff
	svbksb(\@A,\@W,\@V,\@b,\@coeff);
	
	return ($coeff[1],$coeff[2],$coeff[3]);
	my($r)	= sqrt(($coeff[1]**2+$coeff[2]**2)/4-$coeff[3]);
	my($cx) = -0.5*$coeff[1];
	my($cy) = -0.5*$coeff[2];
	return ($r,$cx,$cy);
}

#----------------------------------------------------------------------
# fitEllipse(\@x,\@y) => ?
#	\@x,\@y		references to list of data values
#
#	Direct Least Square Fitting of Ellipses
#	Andrew Fitzgibbon, Maurizio Pilu, and Robert B. Fisher, MAY 1999
# 	IEEE transactions on pattern analysis and machine intelligence 21(5): 476-480
#----------------------------------------------------------------------

sub fitEllipse($$)
{
	my($xR,$yR) = @_;
	my($nSamp) = scalar(@{$xR});
	
	# design matrix D = [ x.*x x.*y y.*y x y ones(size(x)) ]; 
	my(@D);
	for (my($r)=0; $r<$nSamp; $r++) {
		$D[$r][0] = $xR->[$r]**2;
		$D[$r][1] = $xR->[$r] * $yR->[$r];
		$D[$r][2] = $yR->[$r]**2;
		$D[$r][3] = $xR->[$r];
		$D[$r][4] = $yR->[$r];
		$D[$r][5] = 1;
	}

	# scatter matrix S = D' * D;
	my(@S);
	for (my($i)=0; $i<6; $i++) {
		for (my($j)=0; $j<6; $j++) {
			for (my($k)=0; $k<$nSamp; $k++) {
				$S[$i][$j] += $D[$k][$j] * $D[$k][$i];
			}
		}
	}

	# 6x6 constraint matrix C
	my(@C);
	for (my($r)=0; $r<6; $r++) {
		for (my($c)=0; $c<6; $c++) {
			$C[$r][$c] = 0;
		}
	}
	$C[0][2] = -2; $C[1][1] = 1; $C[2][0] = -2;

	# solve generalized eigensystem
	my(@geRe,@geImg,@geVec);
	eig(\@S,\@C,\@geRe,\@geImg,\@geVec);

	# find only negative eigenvalue
	my($i);
	for ($i=0; $i<@geRe; $i++) {
		last if (numberp($geRe[$i]) && ($geRe[$i] < 0));
	}
	croak("internal error") unless ($geRe[$i] < 0);

	# get fitted parameters
	return @{$geVec[$i]};

}

1;