/*******************************************************************************
*$ Component_name:
*	XKep_TrueAnom
*$ Abstract:
*	Calculates the true anomaly of a satellite based on its mean anomaly,
*	for internal use by other Kepler Library routines.
*$ Keywords:
*	KEPLER, ORBITAL_MOTION
*	C, INTERNAL_ROUTINE
*$ Declarations:
*	double		XKep_TrueAnom( e, eccRatio, meanAnom, eccAnom )
*	double		e, eccRatio, meanAnom, *eccAnom;
*$ Inputs:
*	e		orbital eccentricity.
*	eccRatio	value of sqrt((1+e)/(1-e)).
*	meanAnom	mean anomaly in radians.
*$ Outputs:
*	&eccAnom	eccentric anomaly in radians.
*$ Returns:
*	true anomaly in radians, between -pi and pi.
*$ Detailed_description:
*	This function returns the true anomaly of a satellite based on its given
*	eccentricity and mean anomaly.  It also passes back the value of the
*	eccentric anomaly.  It is intended for internal use by the Kepler
*	Library.  Note that the accuracy of the result is controlled by
*	Kep_SetError().
*$ External_references:
*	kep_error
*$ Examples:
*	none
*$ Error_handling:
*	none
*$ Limitations:
*	This is solved using an iterative procedure.  The loop repeats until the
*	change in eccentric anomaly psi is smaller than the error target, or
*	until MAX_COUNT iterations have occurred without an improvement.  In
*	this way, the termination of the loop is essentially guaranteed.  The
*	latter termination will only occur in cases where e is near unity and M
*	is near zero, so that high-order cancellation limits the accuracy of
*	the calculation.
*$ Author_and_institution:
*	Mark R. Showalter
*	NASA/Ames Research Center
*$ Version_and_date:
*	1991 June 18
*$ Change_history:
*	none
*******************************************************************************/
#include <math.h>
#include "kepler.h"
#include "pi.h"

#define		PI2		(PI+PI)
#define		MAX_COUNT	5

extern double kep_error;		/* defined by Kep_SetError() */


double		XKep_TrueAnom( e, eccRatio, meanAnom, eccAnom )
double		e, eccRatio, meanAnom, *eccAnom;
{
double		M, psi, old, dM, dMdP, delta, minDelta, best,
		tanHalfTheta, theta;
int		countdown;

/*******************************************************************************
* Solve for the eccentric anomaly psi using Newton's method:
*		meanLon = psi - e * sin(psi) .
*******************************************************************************/

	M = meanAnom - PI2 * floor(meanAnom/PI2);
	psi = M;
	minDelta = PI2;
	while (1) {
		dM = psi - e*sin(psi) - M;
		dMdP = 1. - e*cos(psi);

/* Generate a new guess and compare to the previous one */
		old = psi;
		psi = psi - dM/dMdP;
		delta = fabs(psi - old);

		if (delta < kep_error) break;

/* Guarantee termination */
		if (delta < minDelta) {
			minDelta = delta;
			best = psi;
			countdown = MAX_COUNT;
		}
		else {
			--countdown;
			if (countdown <= 0) {
				psi = best;
				break;
			}
		}
	}

/* Return eccentric anomaly */
	*eccAnom = psi;

/*******************************************************************************
* Now calculate the true anomaly theta:
*		tan(theta/2) = sqrt((1+e)/(1-e)) tan(psi/2)
*******************************************************************************/

	tanHalfTheta = eccRatio * tan(0.5*psi);
	theta = 2. * atan(tanHalfTheta);

	return theta;
}

/******************************************************************************/