/*
 *  COLA -- a satellite close-approach finder
 *  Copyright (C) 1996 Matthew Francey
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *  Send bugs reports, comments, critique, etc, to mdf@angoss.com.
 *
 *  Tom Rothamel modified this file as part of the Satellite Tracking
 *  Suite.
 *
 */
#include <stdlib.h>
#include <math.h>

#include "sgp.h"

/*
 * values for ->type members
 */
#define	S_SGP	1
#define	S_SGP4	2
#define	S_SDP4	3

/*
 * SGP model constants
 */
typedef struct {
	double	ke;
	double	j2;
	double	j3;
	double	j4;
	double	q0ms4;
	double	s;
} NORAD;

/*
 * SDP4 model constants
 */
typedef struct {
	/*
	 * compatibility with NORAD
	 */
	double	ke;
	double	j2;
	double	j3;
	double	j4;
	double	q0ms4;
	double	s;

	/*
	 * deep-space constants
	 */
	double	q22;
	double	q31;
	double	q33;
	double	g22;
	double	g32;
	double	g44;
	double	g52;
	double	g54;
	double	root22;
	double	root32;
	double	root44;
	double	root52;
	double	root54;
	double	thdt;
	double	zns;
	double	zes;
	double	c1ss;
	double	znl;
	double	zel;
	double	c1l;
	double	zsinis;
	double	zcosis;
	double	zsings;
	double	zcosgs;
	double	fasx2;
	double	fasx4;
	double	fasx6;
} DSNORAD;

static NORAD *
model(void)
{
	NORAD	*m;

	if((m = (NORAD *)calloc(1, sizeof(NORAD))) == NULL)
		return m;
	m->ke     =  107.0883592;
	m->j2     = 1082.61600e-6;
	m->j3     =   -2.53881e-6;
	m->j4     =   -1.65597e-6;
	m->q0ms4  =    1.88027916e-9;
	m->s      =    1.01222928;
	return m;
}

static DSNORAD *
dsmodel(void)
{
	DSNORAD *m;

	if((m = (DSNORAD *)calloc(1, sizeof(DSNORAD))) == NULL)
		return m;
	m->ke     =  107.0883592;
	m->j2     = 1082.61600e-6;
	m->j3     =   -2.53881e-6;
	m->j4     =   -1.65597e-6;
	m->q0ms4  =    1.88027916e-9;
	m->s      =    1.01222928;
	m->q22    =    1.7891679e-6;
	m->q31    =    2.1460748e-6;
	m->q33    =    2.2123015e-7;
	m->g22    =    5.7686396;
	m->g32    =    0.95240898;
	m->g44    =    1.8014998;
	m->g52    =    1.050833;
	m->g54    =    4.4108898;
	m->root22 =    1.7891679e-6;
	m->root32 =    3.7393792e-7;
	m->root44 =    7.3636953e-9;
	m->root52 =    1.1428639e-7;
	m->root54 =    2.1765803e-9;
	m->thdt   =    0.0043752691*1440.0;	/* earth rotation rate */
	m->zes    =    0.01675;			/* ecc. of "solar orbit" */
	m->zns    =    1.19459e-5*1440.0;	/* mean motion of Sun  */
	m->c1ss   =    2.9864797e-6*1440.0;
	m->znl    =    1.5835218e-4*1440.0;	/* mean motion of Moon */
	m->zel    =    0.0549;			/* ecc. of lunar orbit */
	m->c1l    =    4.7968065e-7*1440.0;
	m->zsinis =    0.39785416;		/* sin(obliquity) */
	m->zcosis =    0.91744867;		/* cos(obliquity) */
	m->zsings =   -0.98088458;
	m->zcosgs =    0.19459050;
	m->fasx2  =    0.13130908;
	m->fasx4  =    2.88431980;
	m->fasx6  =    0.37448087;
	return m;
}

/*
 * pre-computed data for SGP
 */
typedef struct {
	int	type;
	double	a0, q0;
	double	T2, T3, L1, L2, Q1, Q2, Q3, Q4, Q5, Q6;
} SGP;

/*
 * finish off a calculation -- all models get here
 */
static SAT *
finish(SAT *s)
{
	SGP	*ss = (SGP   *)s->idata;
	NORAD	*m  = (NORAD *)s->mdata;
	double	p, axn, ayn, u, du, eL2, pL, pL2, onr;
	double	sin2u, cos2u, sinu, cosu, ecosE, esinE;
	int	nn;

	/*
	 * long period
	 */
	axn      = s->ecc*cos(s->peri);
	p        = s->semi*(1.0 - s->ecc*s->ecc);
	s->mean += s->peri + ss->L1*axn/p;
	ayn      = s->ecc*sin(s->peri) + ss->L2/p;

	/*
	 * solve kepler's equation
	 */
	du = 1.0;
	 u = s->mean = fmod(s->mean, 2.0*M_PI);
	nn = 0;
	for(;;) {
		cosu  = cos(u);
		sinu  = sin(u);
		ecosE = axn*cosu + ayn*sinu;
		esinE = axn*sinu - ayn*cosu;
		if(fabs(du) < 1.0e-6 || nn++ > 10)
			break;
		du    = (s->mean + esinE - u)/(1.0 - ecosE);
		if(fabs(du) > 1.0)
			du /= fabs(du);
		u += du;
	}

	/*
	 * intermediate values
	 */
	eL2    = axn*axn + ayn*ayn;
	pL     = s->semi*(1.0 - eL2);
	pL2    = pL*pL;
	s->r   = s->semi*(1.0 - ecosE);
	onr    = 1.0/s->r;
	s->dr  = m->ke*sqrt(s->semi)*esinE*onr;
	s->rdu = m->ke*sqrt(pL)*onr;
	sin2u  = esinE/(1.0 + sqrt(1.0 - eL2));
	sinu   = s->semi*onr*(sinu - ayn - axn*sin2u);
	cosu   = s->semi*onr*(cosu - axn + ayn*sin2u);
	s->u   = atan2(sinu, cosu);

	/*
	 * short period
	 */
	sin2u    = (cosu + cosu)*sinu;
	cos2u    = 1.0 - (sinu + sinu)*sinu;
	s->u    += ss->Q2*sin2u/pL2;
	s->node += ss->Q3*sin2u/pL2;
	s->incl += ss->Q4*cos2u/pL2;
	s->dr   += ss->Q5*s->n*sin2u/pL;
	s->rdu  += (- ss->Q5*cos2u + ss->Q6)*s->n/pL;
	s->r     = s->r*(1.0 - ss->Q6*sqrt(1.0 - eL2)/pL2) + ss->Q1*cos2u/pL;
	sinu     = sin(s->u);
	cosu     = cos(s->u);

	/*
	 * orientation vectors
	 */
	p       =  cos(s->incl);
	s->N[0] =  cos(s->node);
	s->N[1] =  sin(s->node);
	s->N[2] =  0.0;
	s->M[0] = -s->N[1]*p;
	s->M[1] =  s->N[0]*p;
	s->M[2] =          sin(s->incl);
	s->U[0] = s->M[0]*sinu + s->N[0]*cosu;
	s->U[1] = s->M[1]*sinu + s->N[1]*cosu;
	s->U[2] = s->M[2]*sinu;				/* N[2] == 0.0 */
	s->V[0] = s->M[0]*cosu - s->N[0]*sinu;
	s->V[1] = s->M[1]*cosu - s->N[1]*sinu;
	s->V[2] = s->M[2]*cosu;				/* N[2] == 0.0 */

	/*
	 * and -- finally -- position and velocity
	 */
	s->x[0] = s->r*s->U[0];	s->dx[0] = s->dr*s->U[0] + s->rdu*s->V[0];
	s->x[1] = s->r*s->U[1];	s->dx[1] = s->dr*s->U[1] + s->rdu*s->V[1];
	s->x[2] = s->r*s->U[2];	s->dx[2] = s->dr*s->U[2] + s->rdu*s->V[2];
	return s;
}

/*
 * compute position/velocity via SGP
 */
SAT *
sgp(SAT *s, double dt)
{
	SGP	*ss;
	NORAD	*m;

	/*
	 * get initial model constants if not already done
	 */
	dt -= s->t0;
	if((m = (NORAD *)s->mdata) == NULL
	&& (s->mdata = (void *)(m = model())) == NULL)
		return NULL;

	/*
	 * run through the pre-computation
	 */
	
	if((ss = (SGP *)s->idata) == NULL
	   || ss->type != S_SGP) {
		double	a, p, d1, e2, th, th2;

		if(ss != NULL)
			free((void *)ss);
		s->idata = (void *)(ss = (SGP *)calloc(1, sizeof(SGP)));
		if(ss == NULL)
			return NULL;
		ss->type  = S_SGP;
		th        = cos(s->incl0);
		th2       = th*th;
		a         = pow(m->ke/s->n0, 2.0/3.0);
		d1        = 0.750*m->j2*(3.0*th2 - 1.0);
		e2        = 1.0 - s->ecc0*s->ecc0;
		d1       /= (a*a*e2*sqrt(e2));
		ss->a0    = a*(1.0 - d1*((1.0/3.0) + d1*(1.0 + d1*(134.0/81.0))));
		ss->q0    = ss->a0*(1.0 - s->ecc0);
		p         = ss->a0*e2;
		p         = (m->j2*s->n0)/(p*p);
		ss->T3    = -1.500*p*th;
		ss->T2    =  0.750*p*(5.0*th2 - 1.0);
		ss->L2    = -0.500*(m->j3/m->j2)*sin(s->incl0);
		ss->L1    =  0.500*ss->L2*(3.0 + 5.0*th)/(1.0 + th);
		ss->Q1    =  0.250*m->j2*(1.0 - th2);
		ss->Q2    = -0.125*m->j2*(7.0*th2 - 1.0);
		ss->Q3    =  0.750*m->j2*th;
		ss->Q4    =  0.750*m->j2*th*sin(s->incl0);
		ss->Q5    =  0.0;
		ss->Q6    =  0.0;
	}

	/*
	 * "The secular effects of atmospheric drag and gravitation are
	 *  included through the equations
	 */
	s->n    = s->n0 + dt*(2.0*s->dn0 + 3.0*dt*s->ddn0);
	s->semi = ss->a0*pow(s->n0/s->n, 2.0/3.0);
	s->ecc  = (s->semi > ss->q0) ? 1.0 - ss->q0/s->semi : 1.0e-6;
	s->node = s->node0 + dt*ss->T3;
	s->peri = s->peri0 + dt*ss->T2;
	s->incl = s->incl0;
	s->mean = s->M0 + dt*(s->n0 + dt*(s->dn0 + dt*s->ddn0));

	/*
	 * and let the finisher take over
	 */
	return finish(s);
}

/*
 * pre-computed values for SGP4
 */
typedef	struct {
	/*
	 * for compatibility with SGP
	 */
	int	type;
	double	a0pp, n0pp;
	double	T2, T3, L1, L2, Q1, Q2, Q3, Q4, Q5, Q6;

	/*
	 * needed by SGP4
	 */
	double	T1, E1, C1, C2, C4, M2, O2;
	double	D2, D3, D4, E2, E3, M3, M4, M5, T4, T5, T6, T7;
} SGP4;

/*
 * pre-computed values for SDP4
 */
typedef struct {
	/*
	 * stuff used by SGP -- note that order is critical
	 */
	int	type;
	double	a0pp, n0pp;
	double	T2, T3, L1, L2, Q1, Q2, Q3, Q4, Q5, Q6;

	/*
	 * needed by SGP4
	 */
	double	T1, E1, C1, C2, C4, M2, O2;

	/*
	 * used by deep_secular()
	 */
	int	iresfl, isynfl;
	double	thgr, xlamo;
	double	ssl, ssg, ssh, sse, ssi;
	double	del1, del2, del3;
	double	xfact;
	double	d2201, d2211, d3210, d3222, d4410;
	double	d4422, d5220, d5232, d5421, d5433;
	double	g22, g32, g44, g52, g54;
	double	xni, xli, atime, step, step2;

	/*
	 * used by deep_perturb()
	 */
	double	siniq, cosiq;

	double	zmos;			/* solar terms */
	double	se2, se3;
	double	si2, si3;
	double	sl2, sl3, sl4;
	double	sgh2, sgh3, sgh4;
	double	sh2, sh3;

	double	zmol;			/* lunar terms */
	double	ee2, e3;
	double	xi2, xi3;
	double	xl2, xl3, xl4;
	double	xgh2, xgh3, xgh4;
	double	xh2, xh3;
} SDP4;

/*
 * see below
 */
static	void	deep_init(SAT *);
static	void	deep_secular(SAT *, double);
static	void	deep_perturb(SAT *, double);

/*
 * compute position/velocity via SGP4
 */
SAT *
sgp4(SAT *s, double dt)
{
	double	dt2, dt3, dt4, dt5;
	SGP4	*ss;
	NORAD	*m;

	/*
	 * get initial model constants if not already done.
	 */
	dt -= s->t0;
	if((m = (NORAD *)s->mdata) == NULL) {
		/*
		 * 225min orbit == mean motion of 40.212386radians/day
		 */
		if(s->n0 < 40.212386)
			s->mdata = (void *)dsmodel();	/* SDP4 */
		else
			s->mdata = (void *)model();	/* SGP4 */
		if((m = (NORAD *)s->mdata) == NULL)
			return NULL;
	}

	/*
	 * crank through the precomputation if necessary
	 */
	if((ss = (SGP4 *)s->idata) == NULL
	|| (ss->type != S_SGP4 && ss->type != S_SDP4)) {
		double	a, d0, d1, S, q0ms4, beta0, t3, t4, t7, t8;
		double	th, th2, th4, tsi, tsi2, tsi3, tsi4, tsi5;
		double	eta, eta2, eta3, eta4, c12, c13, c14, C3, C5;
		int	low;

		/*
		 * allocate the block -- determine type.
		 */
		free((void *)ss);
		low = (s->n0 < 40.212386) ? sizeof(SDP4) : sizeof(SGP4);
		if((ss = (SGP4 *)(s->idata = calloc(1, low))) == NULL)
			return NULL;
		ss->type = (s->n0 < 40.212386) ? S_SDP4 : S_SGP4;

		/*
		 * "The original mean motion (ss->n0pp) and semimajor axis
		 *  (ss->a0pp) are first recovered from the input elements
		 *  by the equations
		 */
		th       = cos(s->incl0);
		th2      = th*th;
		th4      = th2*th2;
		a        = pow(m->ke/s->n0, 2.0/3.0);
		d1       = 0.750*m->j2*(3.0*th2 - 1.0);
		d0       = sqrt(1.0 - s->ecc0*s->ecc0);
		d1      /= (d0*d0*d0);
		d0       = d1;
		d1      /= (a*a);
		a       *= 1.0 - d1*((1.0/3.0) + d1*(1.0 + d1*(134.0/81.0)));
		d0      /= (a*a);
		ss->n0pp = s->n0/(1.0 + d0);
		ss->a0pp =     a/(1.0 - d0);

		/*
		 * "For perigee between 98 kilometres and 156 kilometres, the
		 * value of the constant (ss->s) used in SGP4 is changed to
		 *
		 *	s* = a0pp*(1.0 - e0) - s + ae
		 *
		 * For perigee below 98 kilometres, the value of s is changed
		 * to
		 *
		 *	s* = 1.003135713	[ 20/XKMPER + ae ]
		 *
		 * If the value of 's' is changed, then the valye of
		 * (q0 - s)^4 must be replaced by
		 *
		 *	(q0 - s*)^4 = (((q0 - s)^4)^(1/4) + s - s*)^4
		 */
		S     = m->s;
		q0ms4 = m->q0ms4;
		a     = ss->a0pp*(1.0 - s->ecc0);	/* perigee */
		low   = (ss->type == S_SGP4);
		if(a < 1.01536499) {			/* 98km    */
			S = 1.003135713;
		} else if(a < 1.02445856) {		/* 156km   */
			S = a - S + 1.0;
		} else if(a >= 1.03449284)		/* 220km   */
			low = 0;
		if(m->s != S) {
			q0ms4 = sqrt(sqrt(m->q0ms4)) + m->s - S;
			q0ms4 *= q0ms4;
			q0ms4 *= q0ms4;
		}

		/*
		 * "Then calculate the constants (using appropriate values of
		 * s and (q0 - s)^4)
		 */
		tsi   = 1.0/(ss->a0pp - S);
		tsi2  = tsi*tsi;
		tsi3  = tsi*tsi2;
		tsi4  = tsi2*tsi2;
		tsi5  = tsi3*tsi2;
		beta0 = sqrt(1.0 - s->ecc0*s->ecc0);
		eta   = ss->a0pp*s->ecc0*tsi;
		eta2  = eta*eta;
		eta3  = eta*eta2;
		eta4  = eta2*eta2;

		/*
		 * common factor for C2, C4 and C5
		 */
		C5 = q0ms4*tsi4/pow(1.0 - eta2, 3.5);

		/*
		 * compute C2
		 */
		ss->C2  = 8.0 + 24.0*eta2 + 3.0*eta4;
		ss->C2 *= 0.5*(3.0*th2 - 1.0);
		ss->C2 *= (3.0/4.0)*m->j2*tsi/(1.0 - eta2);
		ss->C1  = 1.0 + (3.0/2.0)*eta2 + s->ecc0*eta*(4.0 + eta2);
		ss->C2 += ss->a0pp*ss->C1;
		ss->C2 *= C5*ss->n0pp;

		/*
		 * compute C1
		 */
		ss->C1 = s->bstar*ss->C2;

		/*
		 * compute C3 -- i hate singularities!
		 */
		C3 = -2.0*q0ms4*tsi5*m->j3*ss->n0pp*sin(s->incl0)
			/(m->j2*(s->ecc0 == 0.0 ? 1.0e-6 : s->ecc0));

		/*
		 * compute C4
		 */
		ss->C4  = 0.750
			*(1.0 - th2)
			*(2.0*eta2 - s->ecc0*eta - s->ecc0*eta3)
			*cos(2.0*s->peri0);
		ss->C4 += 3.0
			*(1.0 - 3.0*th2)
			*(1.0 + 1.5*eta2 - 2.0*s->ecc0*eta - 0.5*s->ecc0*eta3);
		ss->C4 *= m->j2*tsi;
		ss->C4 /= ss->a0pp*(eta2 - 1.0);
		ss->C4 += 2.0*eta*(1.0 + s->ecc0*eta) + 0.5*s->ecc0 + 0.5*eta3;
		ss->C4 *= 2.0*ss->n0pp*C5*ss->a0pp*beta0*beta0;

		/*
		 * compute C5
		 */
		C5 *= 2.0*ss->a0pp*beta0*beta0
			*(1.0 + (11.0/4.0)*eta*(eta + s->ecc0) + s->ecc0*eta3);

		/*
		 * compute D2, D3 and D4
		 */
		c12 = ss->C1*ss->C1;
		c13 = ss->C1*c12;
		c14 = c12*c12;
		ss->D2 =       4.0*ss->a0pp*tsi*c12;
		ss->D3 = (4.0/3.0)*ss->a0pp*tsi2*c13*(17.0*ss->a0pp + S);
		ss->D4 = (2.0/3.0)*ss->a0pp*tsi3*c14*(221.0*ss->a0pp + 31.0*S);

		/*
		 * for "secular effects"
		 */
		t3 = 1.0/(ss->a0pp*ss->a0pp*beta0*beta0*beta0);
		t4 = t3/beta0;
		t7 = t3*t4;
		t8 = t4*t4;
		ss->T1 = 1.0
		       + (3.0/4.0)*m->j2*(3.0*th2 - 1.0)*t3
		       + (3.0/64.0)*m->j2*m->j2*(13.0 - 78.0*th2 + 137.0*th4)*t7;
		ss->T1 *= ss->n0pp;
		ss->T2 = -(3.0/4.0)*m->j2*(1.0 - 5.0*th2)*t4
		       + (3.0/64.0)*m->j2*m->j2*(7.0 - 114.0*th2 + 395.0*th4)*t8
		       - (15.0/32.0)*m->j4*(3.0 - 36.0*th2 + 49.0*th4)*t8;
		ss->T2 *= ss->n0pp;
		ss->T3 = -(3.0/2.0)*m->j2*th*t4
		       + (3.0/8.0)*m->j2*m->j2*th*(4.0 - 19.0*th2)*t8
		       - (15.0/16.0)*m->j4*th*(3.0 - 7.0*th2)*t8;
		ss->T3 *= ss->n0pp;
		ss->T4 = s->bstar*C3*cos(s->peri0);
		ss->T5 = -(2.0/3.0)*q0ms4*s->bstar*tsi4/(s->ecc0*eta);
		ss->T6 = eta;
		ss->T7 = -(1.0 + eta*cos(s->M0));
		ss->T7 = ss->T7*ss->T7*ss->T7;

		/*
		 * for 'node'
		 */
		ss->O2 = -21.0*ss->n0pp*m->j2*th*ss->C1
			/(4.0*ss->a0pp*ss->a0pp*beta0*beta0);

		/*
		 * eccentricity
		 */
		ss->E1 = -s->bstar*ss->C4;
		ss->E2 = -s->bstar*C5;
		ss->E3 =  s->bstar*C5*sin(s->M0);

		/*
		 * for the "L" equation (#6 on page 17)
		 */
		ss->M2 = ss->n0pp*(3.0/2.0)*ss->C1;
		ss->M3 = ss->n0pp*(ss->D2 + 2.0*c12);
		ss->M4 = 0.25*ss->n0pp*(3.0*ss->D3
					+ 12.0*ss->C1*ss->D2 + 10.0*c13);
		ss->M5 = 0.20*ss->n0pp*(3.0*ss->D4 + 12.0*ss->C1*ss->D3
			+ 6.0*ss->D2*ss->D2 + 30.0*c12*ss->D2 + 15.0*c14);

		/*
		 * for long-period
		 */
		ss->L1 = -0.25*sin(s->incl0)*m->j3*(3.0 + 5.0*th)
			/(m->j2*(1.0 + th));
		ss->L2 = -0.50*sin(s->incl0)*m->j3/m->j2;

		/*
		 * for short-period
		 */
		ss->Q1 =  0.250*m->j2*(1.0 - th2);
		ss->Q2 = -0.125*m->j2*(7.0*th2 - 1.0);
		ss->Q3 =  0.750*m->j2*th;
		ss->Q4 =  0.750*m->j2*th*sin(s->incl0);
		ss->Q5 = -0.500*m->j2*(1.0 - th2);
		ss->Q6 = -0.750*m->j2*(1.0 - 3.0*th2);

		/*
		 * if the perigee is "low", we ignore a few terms.
		 */
		if(low) {
			ss->T4 = ss->T5 = ss->T6 = ss->T7 = 0.0;
			ss->E2 = ss->E3 = 0.0;
			ss->D2 = ss->D3 = ss->D4 = 0.0;
			ss->M3 = ss->M4 = ss->M5 = 0.0;
		}

		/*
		 * if the orbit is high, we initialize for deep-space
		 */
		if(ss->type == S_SDP4)
			deep_init(s);
	}

	/*
	 * various powers of time
	 */
	dt2 = dt*dt;
	dt3 = dt*dt2;
	dt4 = dt2*dt2;
	dt5 = dt2*dt3;

	/*
	 * secular motion
	 */
	s->mean  = s->M0    + ss->T1*dt;
	s->peri  = s->peri0 + ss->T2*dt;
	s->node  = s->node0 + ss->T3*dt + ss->O2*dt2;

	/*
	 * SGP4 or SDP4
	 */
	if(ss->type == S_SGP4) {
		double	x;

		s->incl  = s->incl0;
		x        = 1.0 + ss->T6*cos(s->mean);
		x        = ss->T4*dt + ss->T5*(x*x*x + ss->T7);
		s->mean += x;
		s->peri -= x;
		s->ecc   = s->ecc0 + ss->E1*dt + ss->E2*sin(s->mean) + ss->E3;
		s->mean += ss->M2*dt2 + ss->M3*dt3
			 + ss->M4*dt4 + ss->M5*dt5;
		x        = 1.0 - ss->C1*dt  - ss->D2*dt2
			       - ss->D3*dt3 - ss->D4*dt4;
		s->semi  = (x *= x*ss->a0pp);
		s->n     = m->ke/sqrt(x*x*x);
	} else {
		double	x;

		/*
		 * initial values for deep_secular() to change
		 */
		s->ecc  = s->ecc0;
		s->incl = s->incl0;
		s->n    = ss->n0pp;
		s->semi = ss->a0pp;
		deep_secular(s, dt);

		/*
		 * apply a limited SGP4
		 */
		x        = 1.0 - ss->C1*dt;
		s->semi  = pow(m->ke/s->n, 2.0/3.0)*x*x;
		s->ecc  += ss->E1*dt;
		s->mean += ss->M2*dt2;

		/*
		 * then let the Sun and Moon take a kick
		 */
		deep_perturb(s, dt);
	}

	/*
	 * and then the finisher finisher() handles the rest
	 */
	s->n = m->ke/sqrt(s->semi*s->semi*s->semi);

	/* Added by Tom */
	/* I'm not sure how accurate this is? */
/* 	s->rev = 1 + s->rev0 + (s->n * dt) / (2 * M_PI);  */
/* 	s->revpercent = (s->n * dt) / (2 * M_PI); */
/* 	s->revpercent -= floor(s->revpercent); */
/* 	s->revpercent *= 100; */
        /* End added by Tom */


	return finish(s);
}

/*
 * pre-computation for SDP4
 */
static void
deep_init(SAT *s)
{
	SDP4	*ss;
	DSNORAD	*m;
	int	who;
	double	siniq, cosiq, cosq2, sinomo, cosomo, eqsq, bsq, rteqsq;
	double	day, xnodce, stem, ctem, zcosil, zsinil, zsinhl, zcoshl;
	double	c, gam, zx, zy, zcosgl, zsingl, bfact, sinq, cosq, eq;
	double	aqnv, zcosg, zsing, zcosi, zsini, zcosh, zsinh;
	double	se, si, sl, cc, zn, ze, zmo, sgh, sh;

	if((ss = (SDP4 *)s->idata) == NULL
	|| (m  = (DSNORAD *)s->mdata) == NULL)
		return;
	ss->siniq  = siniq = sin(s->incl0);
	ss->cosiq  = cosiq = cos(s->incl0);
	cosq2      = cosiq*cosiq;
	sinomo     = sin(s->peri0);
	cosomo     = cos(s->peri0);
	sinq       = sin(s->node0);
	cosq       = cos(s->node0);
	eq         = s->ecc0;
	eqsq       = eq*eq;
	bsq        = 1.0 - eqsq;
	rteqsq     = sqrt(bsq);
	ss->thgr   = sgp_gmst(s->t0);
	aqnv       = 1.0/ss->a0pp;

	/*
	 * initialize lunar solar terms
	 */
	day      = s->t0 + 36525.0;
	xnodce   = 4.523602 - day * 9.2422029e-4;
	stem     = sin(xnodce);
	ctem     = cos(xnodce);
	zcosil   = .91375164 - ctem * .03568096;
	zsinil   = sqrt(1.0 - zcosil * zcosil);
	zsinhl   = stem * .089683511 / zsinil;
	zcoshl   = sqrt(1.0 - zsinhl * zsinhl);
	c        = day * .2299715 + 4.7199672;
	gam      = day * .001944368 + 5.8351514;
	ss->zmol = fmod(c - gam, 2.0*M_PI);
	zx       = stem * m->zsinis / zsinil;
	zy       = zcoshl * ctem + zsinhl * m->zcosis * stem;
	zx       = atan2(zx, zy);
	zx       = gam + zx - xnodce;
	zcosgl   = cos(zx);
	zsingl   = sin(zx);
	ss->zmos = fmod(day * .017201977 + 6.2565837, 2.0*M_PI);

	/*
	 * do solar terms
	 */
	zcosg = m->zcosgs;
	zsing = m->zsings;
	zcosi = m->zcosis;
	zsini = m->zsinis;
	zcosh = cosq;
	zsinh = sinq;
	cc    = m->c1ss;
	zn    = m->zns;
	ze    = m->zes;
	zmo   = ss->zmos;
	for(who = 0; ; ++who) {
		double	a1, a2, a3, a4, a5, a6, a7, a8, a9, a10;
		double	x1, x2, x3, x4, x5, x6, x7, x8;
		double	z1, z2, z3;
		double	z11, z12, z13, z21, z22, z23, z31, z32, z33;
		double	s1, s2, s3, s4, s5, s6, s7;

		a1  =  zcosg*zcosh + zsing*zcosi*zsinh;
		a3  = -zsing*zcosh + zcosg*zcosi*zsinh;
		a7  = -zcosg*zsinh + zsing*zcosi*zcosh;
		a8  =  zsing*zsini;
		a9  =  zsing*zsinh + zcosg*zcosi*zcosh;
		a10 =  zcosg*zsini;
		a2  =  cosiq*a7 + siniq*a8;
		a4  =  cosiq*a9 + siniq*a10;
		a5  = -siniq*a7 + cosiq*a8;
		a6  = -siniq*a9 + cosiq*a10;

		x1  =  a1*cosomo + a2*sinomo;
		x2  =  a3*cosomo + a4*sinomo;
		x3  = -a1*sinomo + a2*cosomo;
		x4  = -a3*sinomo + a4*cosomo;
		x5  =  a5*sinomo;
		x6  =  a6*sinomo;
		x7  =  a5*cosomo;
		x8  =  a6*cosomo;

		z31 = x1*12.*x1 - x3*3.*x3;
		z32 = x1*24.*x2 - x3*6.*x4;
		z33 = x2*12.*x2 - x4*3.*x4;
		z1  = 3.0*(a1*a1 + a2*a2) + z31*eqsq;
		z2  = 6.0*(a1*a3 + a2*a4) + z32*eqsq;
		z3  = 3.0*(a3*a3 + a4*a4) + z33*eqsq;
		z11 =  -6.0*a1*a5 + eqsq*(-24.0*x1*x7 - 6.0*x3*x5);
		z12 =  -6.0*(a1*a6 + a3*a5)
		    +  eqsq*(-24.0*(x2*x7 + x1*x8) - 6.0*(x3*x6 + x4*x5));
		z13 = -6.0*a3*a6 + eqsq*(-24.0*x2*x8 - 6.0*x4*x6);
		z21 =  6.0*a2*a5 + eqsq*( 24.0*x1*x5 - 6.0*x3*x7);
		z22 =  6.0*(a4*a5 + a2*a6)
		    + eqsq*(24.0*(x2*x5 + x1*x6) - 6.0*(x4*x7 + x3*x8));
		z23 =  6.0*a4*a6 + eqsq*(24.0*x2*x6 -  6.0*x4*x8);
		z1  = z1 + z1 + bsq*z31;
		z2  = z2 + z2 + bsq*z32;
		z3  = z3 + z3 + bsq*z33;
		s3  = cc/ss->n0pp;
		s2  = -0.5*s3/rteqsq;
		s4  =      s3*rteqsq;
		s1  = -15.0*eq*s4;
		s5  = x1*x3 + x2*x4;
		s6  = x2*x3 + x1*x4;
		s7  = x2*x4 - x1*x3;
		se  =  zn*s1*s5;
		si  =  zn*s2*(z11 + z13);
		sl  = -zn*s3*(z1 + z3 - 14.0 - 6.0*eqsq);
		sgh =  zn*s4*(z31 + z33 - 6.0);
		sh  = -zn*s2*(z21 + z23);
		if(s->incl0 < .052359877)
			sh = 0.0;
		ss->ee2  =   2.0*s1*s6;
		ss->e3   =   2.0*s1*s7;
		ss->xi2  =   2.0*s2*z12;
		ss->xi3  =   2.0*s2*(z13 - z11);
		ss->xl2  =  -2.0*s3*z2;
		ss->xl3  =  -2.0*s3*(z3 - z1);
		ss->xl4  =  -2.0*s3*(-21.0 - 9.0*eqsq)*ze;
		ss->xgh2 =   2.0*s4*z32;
		ss->xgh3 =   2.0*s4*(z33 - z31);
		ss->xgh4 = -18.0*s4*ze;
		ss->xh2  =  -2.0*s2*z22;
		ss->xh3  =  -2.0*s2*(z23 - z21);

		if(who == 1)
			break;

		/*
		 * do lunar terms
		 */
		ss->sse  = se;
		ss->ssi  = si;
		ss->ssl  = sl;
		ss->ssh  = sh/siniq;
		ss->ssg  = sgh - cosiq * ss->ssh;
		ss->se2  = ss->ee2;
		ss->si2  = ss->xi2;
		ss->sl2  = ss->xl2;
		ss->sgh2 = ss->xgh2;
		ss->sh2  = ss->xh2;
		ss->se3  = ss->e3;
		ss->si3  = ss->xi3;
		ss->sl3  = ss->xl3;
		ss->sgh3 = ss->xgh3;
		ss->sh3  = ss->xh3;
		ss->sl4  = ss->xl4;
		ss->sgh4 = ss->xgh4;
		zcosg    = zcosgl;
		zsing    = zsingl;
		zcosi    = zcosil;
		zsini    = zsinil;
		zcosh    = zcoshl * cosq + zsinhl * sinq;
		zsinh    = sinq * zcoshl - cosq * zsinhl;
		zn       = m->znl;
		cc       = m->c1l;
		ze       = m->zel;
		zmo      = ss->zmol;
	}
	ss->sse += se;
	ss->ssi += si;
	ss->ssl += sl;
	if(sh != 0.0) {
		ss->ssg += sgh - cosiq/siniq*sh;
		ss->ssh += sh/siniq;
	} else
		ss->ssg += sgh;

	/*
	 * geopotential resonance initialization for 12 hour orbits
	 */
	ss->iresfl = 0;
	ss->isynfl = 0;
	if(ss->n0pp <= 5.026548240 || ss->n0pp >= 7.539822288) {
		double	eoc, g201, g211, g310, g322, g410, g422, g520;
		double	g533, g521, g532, f220, f221, f321, f322, f441;
		double	f442, f522, f523, f542, f543, ainv2, temp1;
		double	temp, sini2;

		if(ss->n0pp < 11.8944 || ss->n0pp > 13.3056 || eq < 0.5)
			return;
		ss->iresfl = 1;
		eoc = eq * eqsq;
		g201 = -0.306 - 0.44*(eq - 0.64);
		if(eq <= .65) {
			g211 = 3.616 - eq*13.247 + eqsq*16.29;
			g310 = eq*117.39 - 19.302 - eqsq*228.419 + eoc*156.591;
			g322 = eq*109.7927 - 18.9068 - eqsq*214.6334 + eoc*146.5816;
			g410 = eq*242.694 - 41.122 - eqsq*471.094 + eoc*313.953;
			g422 = eq*841.88 - 146.407 - eqsq*1629.014 + eoc*1083.435;
			g520 = eq*3017.977 - 532.114 - eqsq*5740 + eoc*3708.276;
		} else {
			g211 = eq*331.819 - 72.099 - eqsq*508.738 + eoc*266.724;
			g310 = eq*1582.851 - 346.844 - eqsq*2415.925 + eoc*1246.113;
			g322 = eq*1554.908 - 342.585 - eqsq*2366.899 + eoc*1215.972;
			g410 = eq*4758.686 - 1052.797 - eqsq*7193.992 + eoc*3651.957;
			g422 = eq*16178.11 - 3581.69 - eqsq*24462.77 + eoc*12422.52;
			if(eq <= .715)
				g520 = 1464.74 - eq*4664.75 + eqsq*3763.64;
			else
				g520 = eq*29936.92 - 5149.66 - eqsq*54087.36 + eoc*31324.56;
		}
		if (eq < .7) {
			g533 = eq*4988.61 - 919.2277 - eqsq*9064.77 + eoc*5542.21;
			g521 = eq*4568.6173 - 822.71072 - eqsq*8491.4146 + eoc*5337.524;
			g532 = eq*4690.25 - 853.666 - eqsq*8624.77 + eoc*5341.4;
		} else {
			g533 = eq*161616.52 - 37995.78 - eqsq*229838.2 + eoc*109377.94;
			g521 = eq*218913.95 - 51752.104 - eqsq*309468.16 + eoc*146349.42;
			g532 = eq*170470.89 - 40023.88 - eqsq*242699.48 + eoc*115605.82;
		}
		sini2 = siniq * siniq;
		f220 = (cosiq * 2. + 1. + cosq2) * .75;
		f221 = sini2 * 1.5;
		f321 = siniq * 1.875 * (1. - cosiq * 2. - cosq2 * 3.);
		f322 = siniq * -1.875 * (cosiq * 2. + 1. - cosq2 * 3.);
		f441 = sini2 * 35. * f220;
		f442 = sini2 * 39.375 * sini2;
		f522 = siniq * 9.84375 * (sini2 * (1. - cosiq * 2.
		    - cosq2 * 5.) + (cosiq * 4. - 2. + cosq2 *
		     6.) * .33333333);
		f523 = siniq * (sini2 * 4.92187512 * (-2. - cosiq *
		    4. + cosq2 * 10.) + (cosiq * 2. + 1.
		    - cosq2 * 3.) * 6.56250012);
		f542 = siniq * 29.53125 * (2. - cosiq * 8. +
		    cosq2 * (cosiq * 8. - 12. + cosq2 * 10.));
		f543 = siniq * 29.53125 * (-2. - cosiq * 8. +
		    cosq2 * (cosiq * 8. + 12. - cosq2 * 10.));
		ainv2 = aqnv * aqnv;
		temp1 = ss->n0pp * ss->n0pp * 3.0 * ainv2;
		temp = temp1 * m->root22;
		ss->d2201 = temp * f220 * g201;
		ss->d2211 = temp * f221 * g211;
		temp1 *= aqnv;
		temp = temp1 * m->root32;
		ss->d3210 = temp * f321 * g310;
		ss->d3222 = temp * f322 * g322;
		temp1 *= aqnv;
		temp = temp1 * 2. * m->root44;
		ss->d4410 = temp * f441 * g410;
		ss->d4422 = temp * f442 * g422;
		temp1 *= aqnv;
		temp = temp1 * m->root52;
		ss->d5220 = temp * f522 * g520;
		ss->d5232 = temp * f523 * g532;
		temp = temp1 * 2. * m->root54;
		ss->d5421 = temp * f542 * g521;
		ss->d5433 = temp * f543 * g533;
		ss->xlamo = s->M0 + 2.0*(s->node0 - ss->thgr);
		bfact = ss->T1 + 2.0*(ss->T3 - m->thdt);
		bfact = bfact + ss->ssl + ss->ssh + ss->ssh;
	} else {
		double	g200, g310, g300, f220, f311, f330;

		/*
		 * synchronous resonance terms initialization
		 */
		ss->iresfl = 1;
		ss->isynfl = 1;
		g200 = eqsq*(eqsq*0.8125 - 2.5) + 1.0;
		g310 = eqsq*2.0 + 1.0;
		g300 = eqsq*(eqsq*6.60937 - 6.0) + 1.0;
		f220 = (ss->cosiq + 1.) * .75 * (ss->cosiq + 1.0);
		f311 = siniq * .9375 * siniq * (cosiq * 3. + 1.) - (cosiq + 1.) * .75;
		f330 = cosiq + 1.0;
		f330 = f330 * 1.875 * f330 * f330;
		ss->del1 = ss->n0pp * 3.0 * ss->n0pp * aqnv * aqnv;
		ss->del2 = ss->del1 * 2. * f220 * g200 * m->q22;
		ss->del3 = ss->del1 * 3. * f330 * g300 * m->q33 * aqnv;
		ss->del1 = ss->del1 * f311 * g310 * m->q31 * aqnv;
		ss->xlamo = s->M0 + s->node0 + s->peri0 - ss->thgr;
		bfact = ss->T1 + ss->T2 + ss->T3 - m->thdt;
		bfact = bfact + ss->ssl + ss->ssg + ss->ssh;
	}
	ss->xfact = bfact - ss->n0pp;

	/*
	 * initialize integrator
	 */
	ss->xli   = ss->xlamo;
	ss->xni   = ss->n0pp;
	ss->atime = 0.0;
	ss->step  = 0.5;
	ss->step2 = 0.5*ss->step*ss->step;
	return;
}

/*
 * deep-space secular
 */
static void
deep_secular(SAT *s, double dt)
{
	SDP4	*ss;
	DSNORAD	*m;

	/*
	 * no adjustment for non-initialized objects
	 */
	if((ss = (SDP4 *)s->idata) == NULL
	|| (m  = (DSNORAD *)s->mdata) == NULL)
		return;
	s->mean += ss->ssl*dt;
	s->peri += ss->ssg*dt;
	s->node += ss->ssh*dt;
	s->ecc  += ss->sse*dt;
	s->incl += ss->ssi*dt;
	if(s->incl < 0.0) {
		s->incl = -s->incl;
		s->node += M_PI;
		s->peri -= M_PI;
	}
	if(ss->iresfl == 0)
		return;

	/*
	 * 12/24hr resonance -- some weird sort of numerical integration
	 * is undertaken.  warning:  the original fortran code was fairly
	 * disgusting.  this is a re-write to some degree.
	 */
	for(;;) {
		double	ft, delt, xldot, xndot, xnddt;

		/*
		 * re-start from the epoch if things get out of hand
		 */
		if(ss->atime == 0.0
		|| (dt >= 0.0 && ss->atime < 0.0)
		|| (dt <  0.0 && ss->atime >= 0.0)) {
			/*
			 * epoch restart
			 */
			ss->atime = 0.0;
			ss->xni   = ss->n0pp;
			ss->xli   = ss->xlamo;
		}
		ft   = dt - ss->atime;
		delt = (ft > 0.0) ? ss->step : -ss->step;

		/*
		 * dot terms calculated
		 */
		if(ss->isynfl != 0) {
			double	t1, t2, t3;

			xndot	= ss->del1*sin(t1 =      ss->xli - m->fasx2 )
				+ ss->del2*sin(t2 = 2.0*(ss->xli - m->fasx4))
				+ ss->del3*sin(t3 = 3.0*(ss->xli - m->fasx6));
			xnddt	= ss->del1*    cos(t1)
				+ ss->del2*2.0*cos(t2)
				+ ss->del3*3.0*cos(t3);
		} else {
			double	t1, t2, t3, t4, t5, t6, t7, t8, t9, ta;
			double	xomi, x2omi, x2li;

			xomi  = s->peri0 + ss->T2*ss->atime;
			x2omi = xomi + xomi;
			x2li  = ss->xli + ss->xli;
			xndot	= ss->d2201*sin(t1 = x2omi + ss->xli - m->g22)
				+ ss->d2211*sin(t2 =         ss->xli - m->g22)
				+ ss->d3210*sin(t3 =  xomi + ss->xli - m->g32)
				+ ss->d3222*sin(t4 = -xomi + ss->xli - m->g32)
				+ ss->d4410*sin(t5 = x2omi +    x2li - m->g44)
				+ ss->d4422*sin(t6 =            x2li - m->g44)
				+ ss->d5220*sin(t7 =  xomi + ss->xli - m->g52)
				+ ss->d5232*sin(t8 = -xomi + ss->xli - m->g52)
				+ ss->d5421*sin(t9 =  xomi +    x2li - m->g54)
				+ ss->d5433*sin(ta = -xomi +    x2li - m->g54);
			xnddt	= ss->d2201*cos(t1)
				+ ss->d2211*cos(t2)
				+ ss->d3210*cos(t3)
				+ ss->d3222*cos(t4)
				+ ss->d5220*cos(t7)
				+ ss->d5232*cos(t8)
				+(ss->d4410*cos(t5)
				+ ss->d4422*cos(t6)
				+ ss->d5421*cos(t9)
				+ ss->d5433*cos(ta))*2.0;
		}
		xldot  = ss->xni + ss->xfact;
		xnddt *= xldot;

		/*
		 * if we are close enough, finish the calculation
		 */
		if(fabs(ft) < ss->step) {
			double	xl, temp;

			s->n    = ss->xni + ft*(xndot + 0.5*ft*xnddt);
			xl      = ss->xli + ft*(xldot + 0.5*ft*xndot);
			temp    = ss->thgr + dt*m->thdt - s->node;
			if(ss->isynfl == 0)
				s->mean = xl + temp + temp;
			else
				s->mean = xl - s->peri + temp;
			return;
		}

		/*
		 * otherwise, make a jong-lump in the needed direction
		 */
		ss->xli   += xldot*delt + xndot*ss->step2;
		ss->xni   += xndot*delt + xnddt*ss->step2;
		ss->atime += delt;
	}
}

/*
 * deep-space lunar-solar perturbations
 */
static void
deep_perturb(SAT *s, double dt)
{
	SDP4	*ss;
	DSNORAD	*m;
	double	sinis, cosis;
	double	sinok, cosok;
	double	zm, zf, sinzf, f2, f3;
	double	ph, pgh, dnode;

	/*
	 * if not initialized, perform no "corrections"
	 */
	if((ss = (SDP4 *)s->idata) == NULL
	|| (m  = (DSNORAD *)s->mdata) == NULL)
		return;

	/*
	 * solar components
	 */
	zm    = ss->zmos + m->zns*dt;
	zf    = zm + 2.0*m->zes*sin(zm);
	sinzf = sin(zf);
	f2    =  0.5*sinzf*sinzf - 0.25;
	f3    = -0.5*sinzf*cos(zf);
	s->ecc  += ss->se2*f2  + ss->se3*f3;
	s->incl += ss->si2*f2  + ss->si3*f3;
	s->mean += ss->sl2*f2  + ss->sl3*f3  + ss->sl4*sinzf;
	pgh      = ss->sgh2*f2 + ss->sgh3*f3 + ss->sgh4*sinzf;
	ph       = ss->sh2*f2  + ss->sh3*f3;

	/*
	 * lunar components
	 */
	zm       = ss->zmol + m->znl*dt;
	zf       = zm + 2.0*m->zel*sin(zm);
	sinzf    = sin(zf);
	f2       =  0.5*sinzf*sinzf - 0.25;
	f3       = -0.5*sinzf*cos(zf);
	s->ecc  += ss->ee2*f2  + ss->e3*f3;
	s->incl += ss->xi2*f2  + ss->xi3*f3;
	s->mean += ss->xl2*f2  + ss->xl3*f3  + ss->xl4*sinzf;
	pgh     += ss->xgh2*f2 + ss->xgh3*f3 + ss->xgh4*sinzf;
	ph      += ss->xh2*f2  + ss->xh3*f3;
	if(s->incl0 >= 0.2) {
		ph      /= ss->siniq;
		pgh     -= ph*ss->cosiq;
		s->peri += pgh;
		s->node += ph;
		return;
	}

	/*
	 * apply periodics with lyddane modification
	 */
	sinok = sin(s->node);
	cosok = cos(s->node);
	sinis = sin(s->incl);
	cosis = cos(s->incl);
	dnode = s->node;
	if((s->node = atan2(sinis*sinok + ph*cosok,
			    sinis*cosok - ph*sinok)) < 0.0)
		s->node += 2.0*M_PI;
	dnode -= s->node;
	while(dnode >= M_PI) dnode -= 2.0*M_PI;
	while(dnode < -M_PI) dnode += 2.0*M_PI;
	s->peri += pgh + cosis*dnode;
}