/* SUMFUNC.C
 *
 * Computes sum-of-squares for lobing model.
 */

#include <math.h>
#include "fitlobes.h"

/* LOCAL FUNCTIONS */
static OneResidualFnAnon oneResidual;

double sumFunc( void *x, const Vector p )
	/* Calculate sum of squares of model for L-M algorithm */
{
	FitData *fd = (FitData *)x;

	/* Set up lobe angles so that once we know which lobe, the rest is easy */
	/* Index corresponds to cusp number */
	setupFitData( fd, p );

	/* Now loop through everything */
	return sumOfSquaresAnon( x, dsLength(&fd->ds), p, oneResidual );
}

#pragma argsused /* const Vector q */
static double oneResidual( void *x, int i, const Vector q )
	/* Calculate one residual for combined model */
{
	const FitData *fd = (FitData *)x;
	const Vector p = fd->fitParms;
	const struct Dataset_ *pts = &fd->ds;
	double x_i, y_i, z_i, r_i, azimuth_i, decl_i;
	double pretravel;

	/* Get spherical coordinates for point i */
	ptInfo( dsAt(pts,i), p, &x_i, &y_i, &z_i, &r_i, &azimuth_i, &decl_i );

	if ( decl_i < fd->capAngle ) {
		/* Within friction cap */
		pretravel = K(p);
	} else {
		/* Outside the friction cap. */
		int lobe;
		double cusp, lobeAngle, sDecl = sin(decl_i);

		/* Get the lobe index and the relative angle of azimuth_i in the lobe */
		lobe = lobeIndex( azimuth_i, decl_i, p, fd, &lobeAngle );

		/* Calculate the "cusp" function (my name) */
		cusp = sDecl * cos(azimuth_i-lobeAngle) + BETA(p) * cos(decl_i);

		pretravel = ALPHA(p) * cusp + GAMMA(p)*fd->bigF[lobe]*sDecl*sDecl / cusp;
	}

	return r_i - RAD(p) + pretravel;
}