/* Program to solve Keplerian orbit
 * given orbital parameters and the time.
 * Returns Heliocentric equatorial rectangular coordinates of
 * the object.
 *
 * This program detects several cases of given orbital elements.
 * If a program for perturbations is pointed to, it is called
 * to calculate all the elements.
 * If there is no program, then the mean longitude is calculated
 * from the mean anomaly and daily motion.
 * If the daily motion is not given, it is calculated
 * by Kepler's law.
 * If the eccentricity is given to be 1.0, it means that
 * meandistance is really the perihelion distance, as in a comet
 * specification, and the orbit is parabolic.
 *
 * Reference: Taff, L.G., "Celestial Mechanics, A Computational
 * Guide for the Practitioner."  Wiley, 1985.
 */

#include "kep.h"
static double PI=3.14159265358979323846;

#if !BandS
extern struct orbit earth;	/* orbital elements of the earth */
#endif

extern double eps, coseps, sineps; /* obliquity of ecliptic */
double sinh(), cosh(), tanh();

kepler(J, e, rect, polar)
double J, rect[], polar[];
struct orbit *e;
{
double alat, E, M, W, temp;
double epoch, inclination, ascnode, argperih;
double meandistance, dailymotion, eccent, meananomaly;
double r, coso, sino, cosa, sina;
int k;


/* Compute orbital elements if a program for doing so
 * is supplied
 */
if( e->oelmnt )
	{
	k = (*(e->oelmnt) )(e,J);
	if( k == -1 )
		goto dobs;
	}
else if( e->celmnt )
	{ /* call B & S algorithm */
dobs:
	(*(e->celmnt) )( J, polar );
	polar[0] = modtp( polar[0] );
	E = polar[0]; /* longitude */
	e->L = E;
	W = polar[1]; /* latitude */
	r = polar[2]; /* radius */
	e->r = r;
	e->epoch = J;
	e->equinox = J;
	goto kepdon;
	}

/* Decant the parameters from the data structure
 */
epoch = e->epoch;
inclination = e->i;
ascnode = e->W * DTR;
argperih = e->w;
meandistance = e->a; /* semimajor axis */
dailymotion = e->dm;
eccent = e->ecc;
meananomaly = e->M;
/* Check for parabolic orbit. */
if( eccent == 1.0 )
	{
/* meandistance = perihelion distance, q
 * epoch = perihelion passage date
 */
	temp = meandistance * sqrt(meandistance);
	W = (J - epoch ) * 0.0364911624 / temp;
/* The constant above is 3 k / sqrt(2),
 * k = Gaussian gravitational constant = 0.01720209895 . */
	E = 0.0;
	M = 1.0;
	while( fabs(M) > 1.0e-11 )
		{
		temp = E * E;
		temp = (2.0 * E * temp + W)/( 3.0 * (1.0 + temp));
		M = temp - E;
		if( temp != 0.0 )
			M /= temp;
		E = temp;
		}
	r = meandistance * (1.0 + E * E );
	M = atan( E );
	M = 2.0 * M;
	alat = M + DTR*argperih;
	goto parabcon;
	}
if( eccent > 1.0 )
	{
/* The equation of the hyperbola in polar coordinates r, theta
 * is r = a(e^2 - 1)/(1 + e cos(theta))
 * so the perihelion distance q = a(e-1),
 * the "mean distance"  a = q/(e-1).
 */
	meandistance = meandistance/(eccent - 1.0);
	temp = meandistance * sqrt(meandistance);
	W = (J - epoch ) * 0.01720209895 / temp;
/* solve M = -E + e sinh E */
	E = W/(eccent - 1.0);
	M = 1.0;
	while( fabs(M) > 1.0e-11 )
		{
		M = -E + eccent * sinh(E) - W;
		E += M/(1.0 - eccent * cosh(E));
		}
	r = meandistance * (-1.0 + eccent * cosh(E));
	temp = (eccent + 1.0)/(eccent - 1.0);
	M = sqrt(temp) * tanh( 0.5*E );
	M = 2.0 * atan(M);	
	alat = M + DTR*argperih;
	goto parabcon;
	}
/* Calculate the daily motion, if it is not given.
 */
if( dailymotion == 0.0 )
	{
/* The constant is 180 k / pi, k = Gaussian gravitational constant.
 * Assumes object in heliocentric orbit is massless.
 */
	dailymotion = 0.9856076686/(e->a*sqrt(e->a));
	}
dailymotion *= J - epoch;
/* M is proportional to the area swept out by the radius
 * vector of a circular orbit during the time between
 * perihelion passage and Julian date J.
 * It is the mean anomaly at time J.
 */
M = DTR*( meananomaly + dailymotion );
M = modtp(M);
/* If mean longitude was calculated, adjust it also
 * for motion since epoch of elements.
 */
if( e->L )
	{
	e->L += dailymotion;
	e->L = mod360( e->L );
	}

/* By Kepler's second law, M must be equal to
 * the area swept out in the same time by an
 * elliptical orbit of same total area.
 * Integrate the ellipse expressed in polar coordinates
 *     r = a(1-e^2)/(1 + e cosW)
 * with respect to the angle W to get an expression for the
 * area swept out by the radius vector.  The area is given
 * by the mean anomaly; the angle is solved numerically.
 * 
 * The answer is obtained in two steps.  We first solve
 * Kepler's equation
 *    M = E - eccent*sin(E)
 * for the eccentric anomaly E.  Then there is a
 * closed form solution for W in terms of E.
 */

E = M; /* Initial guess is same as circular orbit. */
temp = 1.0;
do
	{
/* The approximate area swept out in the ellipse */
	temp = E - eccent * sin(E)
/* ...minus the area swept out in the circle */
		- M;
/* ...should be zero.  Use the derivative of the error
 * to converge to solution by Newton's method.
 */
	E -= temp/(1.0 - eccent*cos(E));
	}
while( fabs(temp) > 1.0e-11 );

/* The exact formula for the area in the ellipse is
 *    2.0*atan(c2*tan(0.5*W)) - c1*eccent*sin(W)/(1+e*cos(W))
 * where
 *    c1 = sqrt( 1.0 - eccent*eccent )
 *    c2 = sqrt( (1.0-eccent)/(1.0+eccent) ).
 * Substituting the following value of W
 * yields the exact solution.
 */
temp = sqrt( (1.0+eccent)/(1.0-eccent) );
W = 2.0 * atan( temp * tan(0.5*E) );

/* The true anomaly.
 */
W = modtp(W);

meananomaly *= DTR;
/* Orbital longitude measured from node
 * (argument of latitude)
 */
if( e->L )
	alat = (e->L)*DTR + W - meananomaly - ascnode;
else
	alat = W + DTR*argperih; /* mean longitude not given */

/* From the equation of the ellipse, get the
 * radius from central focus to the object.
 */
r = meandistance*(1.0-eccent*eccent)/(1.0+eccent*cos(W));

parabcon:

/* The heliocentric ecliptic longitude of the object
 * is given by
 *   tan( longitude - ascnode )  =  cos( inclination ) * tan( alat ).
 */
coso = cos( alat );
sino = sin( alat );
inclination *= DTR;
W = sino * cos( inclination );
E = zatan2( coso, W ) + ascnode;

/* The ecliptic latitude of the object
 */
W = sino * sin( inclination );
W = asin(W);

/* Apply perturbations, if a program is supplied.
 */
if( e->celmnt )
	{
	e->L = E;
	e->r = r;
	(*(e->celmnt) )(e);
	E = e->L;
	r = e->r;
	W += e->plat;
	}

/* If earth, Adjust from earth-moon barycenter to earth
 * by AA page E2
 * unless orbital elements are calculated by formula.
 * (The Meeus perturbation formulas include this term for the moon.)
 */
#if !BandS
if( (e == &earth) && (e->oelmnt == 0) )
	{
	embofs( J, &E, &W, &r ); /* see below */
	}
#endif

/* Output the polar cooordinates
 */
polar[0] = E; /* longitude */
polar[1] = W; /* latitude */
polar[2] = r; /* radius */


kepdon:


/* Convert to rectangular coordinates,
 * using the perturbed latitude.
 */
rect[2] = r * sin(W);
cosa = cos(W);
rect[1] = r * cosa * sin(E);
rect[0] = r * cosa * cos(E);

/* Convert from heliocentric ecliptic rectangular
 * to heliocentric equatorial rectangular coordinates
 * by rotating eps radians about the x axis.
 */
epsiln( e->equinox );
W = coseps*rect[1] - sineps*rect[2];
M = sineps*rect[1] + coseps*rect[2];
rect[1] = W;
rect[2] = M;

/* Precess the position
 * to ecliptic and equinox of J2000.0
 * if not already there.
 */
precess( rect, e->equinox, 1 );
}


#if !BandS
/* Adjust position from Earth-Moon barycenter to Earth
 *
 * J = Julian day number
 * E = Earth's ecliptic longitude (radians)
 * W = Earth's ecliptic latitude (radians)
 * r = Earth's distance to the Sun (au)
 */
embofs( J, E, W, r )
double J;
double *E, *W, *r;
{
double T, M, a, L, B, p;
double smp, cmp, s2mp, c2mp, s2d, c2d, sf, cf;
double s2f, sx, cx;

/* Short series for position of the Moon
 */
T = (J-J1900)/36525.0;
/* Mean anomaly of moon (MP) */
a = ((1.44e-5*T + 0.009192)*T + 477198.8491)*T + 296.104608;
a *= DTR;
smp = sin(a);
cmp = cos(a);
s2mp = 2.0*smp*cmp;		/* sin(2MP) */
c2mp = cmp*cmp - smp*smp;	/* cos(2MP) */
/* Mean elongation of moon (D) */
a = ((1.9e-6*T - 0.001436)*T + 445267.1142)*T + 350.737486;
a  = 2.0 * DTR * a;
s2d = sin(a);
c2d = cos(a);
/* Mean distance of moon from its ascending node (F) */
a = (( -3.e-7*T - 0.003211)*T + 483202.0251)*T + 11.250889;
a  *= DTR;
sf = sin(a);
cf = cos(a);
s2f = 2.0*sf*cf;	/* sin(2F) */
sx = s2d*cmp - c2d*smp;	/* sin(2D - MP) */
cx = c2d*cmp + s2d*smp;	/* cos(2D - MP) */
/* Mean longitude of moon (LP) */
L = ((1.9e-6*T - 0.001133)*T + 481267.8831)*T + 270.434164;
/* Mean anomaly of sun (M) */
M = (( -3.3e-6*T - 1.50e-4)*T + 35999.0498)*T + 358.475833;
/* Ecliptic longitude of the moon */
L =	L
	+ 6.288750*smp
	+ 1.274018*sx
	+ 0.658309*s2d
	+ 0.213616*s2mp
	- 0.185596*sin( DTR * M );
	- 0.114336*s2f;
/* Ecliptic latitude of the moon */
a = smp*cf;
sx = cmp*sf;
B =	  5.128189*sf
	+ 0.280606*(a+sx)		/* sin(MP+F) */
	+ 0.277693*(a-sx)		/* sin(MP-F) */
	+ 0.173238*(s2d*cf - c2d*sf);	/* sin(2D-F) */
/* Parallax of the moon */
p =	 0.950724
	+0.051818*cmp
	+0.009531*cx
	+0.007843*c2d
	+0.002824*c2mp;

/* Elongation of Moon from Sun
 */
L = DTR * mod360(L) - (*E - PI);

/* Distance from Earth to Earth-Moon barycenter
*/
a = 5.18041e-7 / ( DTR * p );

/* Adjust the coordinates of the Earth
 */
sx = a / *r;		/* chord = R * dTheta */
*E += sx * sin(L);
*W -= sx * DTR * B;
*r += a * cos(L);
}
#endif