/* improved by rclark@lpl.arizona.edu (Richard Clark) */

#include <stdio.h>
#include <math.h>

#include "P_.h"
#include "astro.h"


#define TWOPI   (2*PI)

/* given the mean anomaly, ma, and the eccentricity, s, of elliptical motion,
 * find the true anomaly, *nu, and the eccentric anomaly, *ea.
 * all angles in radians.
 */
void
anomaly (ma, s, nu, ea)
double ma, s;
double *nu, *ea;
{
        double m, fea, corr;
        /* double m1 */

        if (s < 1.0) {
            /* elliptical */
            double dla;

            m = ma-TWOPI*(long)(ma/TWOPI);
            if (m > PI) m -= TWOPI;
            if (m < -PI) m += TWOPI;
	    /*  The current method is ok but smoe tempting mods require
	     *	0<M<PI. Substitute m1 for m if so.
	     *	m1 = fabs(m);
	     */
            fea = m;
            for (;;) {
                dla = fea-(s*sin(fea))-m;
                if (fabs(dla)<1e-6)
                    break;
                /* avoid runnaway corrections for e>.97 and M near 0*/
                corr = 1-(s*cos(fea));
                if (corr < .1) corr = .1;
                dla /= corr;
                fea -= dla;
            }
            *nu = 2*atan(sqrt((1+s)/(1-s))*tan(fea/2));
        } else {
            /* hyperbolic */
	    double fea1;

            m = fabs(ma);
            fea = m / (s-1.);
	    fea1 = pow(6*m/(s*s),1./3.);
            /* whichever is smaller is the better initial guess */
            if (fea1 < fea) fea = fea1;

	    corr = 1;
            while (fabs(corr) > 0.000001) {
		corr = (m - s * sinh(fea) + fea) / (s*cosh(fea) - 1);
		fea += corr;
            }
            if (ma < 0.) fea = -fea;
            *nu = 2*atan(sqrt((s+1)/(s-1))*tanh(fea/2));
        }
        *ea = fea;
}
