#include <stdlib.h>
#include <math.h>
#undef SING

#include "sattypes.h"
#include "vector.h"
#include "satspec.h"

/*      SDP4                                               3 NOV 80 */
/*      SUBROUTINE SDP4(IFLAG,TSINCE)
      COMMON/E1/XMO,XNODEO,OMEGAO,EO,XINCL,XNO,XNDT2O,
     1           XNDD6O,BSTAR,X,Y,Z,XDOT,YDOT,ZDOT,EPOCH,DS50
      COMMON/C1/CK2,CK4,E6A,QOMS2T,S,TOTHRD,
     1           XJ3,XKE,XKMPER,XMNPDA,AE
      DOUBLE PRECISION EPOCH, DS50
      */

#define XMO	(sat->elem->se_XMO)
#define XNODEO	(sat->elem->se_XNODEO)
#define OMEGAO	(sat->elem->se_OMEGAO)
#define EO	(sat->elem->se_EO)
#define XINCL	(sat->elem->se_XINCL)
#define XNO	(sat->elem->se_XNO)
#define XNDT20	(sat->elem->se_XNDT20)
#define XNDD60	(sat->elem->se_XNDD60)
#define BSTAR	(sat->elem->se_BSTAR)
#define EPOCH	(sat->elem->se_EPOCH)

#define CK2	(5.413080e-04)
#define CK4	(6.209887e-07)
#define QOMS2T	(1.880279e-09)
#define S	(1.012229e+00)

#define AE (1.0)
#define DE2RA (.174532925E-1)
#define E6A (1.E-6)
#define PI (3.14159265)
#define PIO2 (1.57079633)
#define QO (120.0)
#define SO (78.0)
#define TOTHRD (.66666667)
#define TWOPI (6.2831853)
#define X3PIO2 (4.71238898)
#define XJ2 (1.082616E-3)
#define XJ3 (-.253881E-5)
#define XJ4 (-1.65597E-6)
#define XKE (.743669161E-1)
#define XKMPER (6378.135)
#define XMNPDA (1440.)

/* int IFLAG; */

#define X	(pos->sl_X)
#define XDOT	(pos->sl_XDOT)
#define Y	(pos->sl_Y)
#define YDOT	(pos->sl_YDOT)
#define Z	(pos->sl_Z)
#define ZDOT	(pos->sl_ZDOT)

/* sat->prop.sdp4-> */
#define AODP	(sat->prop.sdp4->sdp4_AODP)
#define AYCOF	(sat->prop.sdp4->sdp4_AYCOF)
#define BETAO	(sat->prop.sdp4->sdp4_BETAO)
#define BETAO2	(sat->prop.sdp4->sdp4_BETAO2)
#define C1	(sat->prop.sdp4->sdp4_C1)
#define C4	(sat->prop.sdp4->sdp4_C4)
#define COSG	(sat->prop.sdp4->sdp4_COSG)
#define COSIO	(sat->prop.sdp4->sdp4_COSIO)
#define EOSQ	(sat->prop.sdp4->sdp4_EOSQ)
#define OMGDOT	(sat->prop.sdp4->sdp4_OMGDOT)
#define SING	(sat->prop.sdp4->sdp4_SING)
#define SINIO	(sat->prop.sdp4->sdp4_SINIO)
#define T2COF	(sat->prop.sdp4->sdp4_T2COF)
#define THETA2	(sat->prop.sdp4->sdp4_THETA2)
#define X1MTH2	(sat->prop.sdp4->sdp4_X1MTH2)
#define X3THM1	(sat->prop.sdp4->sdp4_X3THM1)
#define X7THM1	(sat->prop.sdp4->sdp4_X7THM1)
#define XLCOF	(sat->prop.sdp4->sdp4_XLCOF)
#define XMDOT	(sat->prop.sdp4->sdp4_XMDOT)
#define XNODCF	(sat->prop.sdp4->sdp4_XNODCF)
#define XNODOT	(sat->prop.sdp4->sdp4_XNODOT)
#define XNODP	(sat->prop.sdp4->sdp4_XNODP)

#define XMDF_seco   (sat->prop.sdp4->sdp4_XMDF_seco)
#define OMGADF_seco (sat->prop.sdp4->sdp4_OMGADF_seco)
#define XNODE_seco  (sat->prop.sdp4->sdp4_XNODE_seco)
#define EM_seco     (sat->prop.sdp4->sdp4_EM_seco)
#define XINC_seco   (sat->prop.sdp4->sdp4_XINC_seco)
#define XN_seco     (sat->prop.sdp4->sdp4_XN_seco)

#define E_pero      (sat->prop.sdp4->sdp4_E_pero)
#define XINC_pero   (sat->prop.sdp4->sdp4_XINC_pero)
#define OMGADF_pero (sat->prop.sdp4->sdp4_OMGADF_pero)
#define XNODE_pero  (sat->prop.sdp4->sdp4_XNODE_pero)
#define XMAM_pero   (sat->prop.sdp4->sdp4_XMAM_pero)

void
sdp4 (SatData *sat, Vec3 *pos, Vec3 *dpos, double TSINCE)
{
    int i;

    /* private temporary variables used only in init section */
    double A1,A3OVK2,AO,C2,COEF,COEF1,DEL1,DELO,EETA,ETA,
	ETASQ,PERIGE,PINVSQ,PSISQ,QOMS24,S4,THETA4,TSI,X1M5TH,XHDOT1;

    /* private temporary variables */
    double A,AXN,AYN,AYNL,BETA,BETAL,CAPU,COS2U,COSEPW=0,
	COSIK,COSNOK,COSU,COSUK,E,ECOSE,ELSQ,EM=0,EPW,ESINE,OMGADF,PL,
	R,RDOT,RDOTK,RFDOT,RFDOTK,RK,SIN2U,SINEPW=0,SINIK,SINNOK,
	SINU,SINUK,TEMP,TEMP1,TEMP2,TEMP3=0,TEMP4=0,TEMP5=0,TEMP6=0,TEMPA,
	TEMPE,TEMPL,TSQ,U,UK,UX,UY,UZ,VX,VY,VZ,XINC=0,XINCK,XL,XLL,XLT,
	XMAM,XMDF,XMX,XMY,XN,XNODDF,XNODE,XNODEK;

#if 0
    A1=A3OVK2=AO=C2=COEF=COEF1=DEL1=DELO=EETA=ETA = signaling_nan();
    ETASQ=PERIGE=PINVSQ=PSISQ=QOMS24=S4=THETA4=TSI=X1M5TH=XHDOT1 = signaling_nan();

     A=AXN=AYN=AYNL=BETA=BETAL=CAPU=COS2U=COSEPW = signaling_nan();
     COSIK=COSNOK=COSU=COSUK=E=ECOSE=ELSQ=EM=EPW=ESINE=OMGADF=PL = signaling_nan();
     R=RDOT=RDOTK=RFDOT=RFDOTK=RK=SIN2U=SINEPW=SINIK=SINNOK = signaling_nan();
     SINU=SINUK=TEMP=TEMP1=TEMP2=TEMP3=TEMP4=TEMP5=TEMP6=TEMPA = signaling_nan();
     TEMPE=TEMPL=TSQ=U=UK=UX=UY=UZ=VX=VY=VZ=XINC=XINCK=XL=XLL=XLT = signaling_nan();
     XMAM=XMDF=XMX=XMY=XN=XNODDF=XNODE=XNODEK = signaling_nan();
#endif

    if(TSINCE != 0.0 && !sat->prop.sdp4) {
	/*
	 * Yes, this is a recursive call.
	 */
	sdp4(sat, pos, dpos, 0.0);
    }

/*      IF  (IFLAG .EQ. 0) GO TO 100 */
/*    if(!IFLAG) */
    if(!sat->prop.sdp4) {
	sat->prop.sdp4 = (struct sdp4_data *) malloc(sizeof(struct sdp4_data));

/*	init_sdp4(sat->prop.sdp4); */

/*      RECOVER ORIGINAL MEAN MOTION (XNODP) AND SEMIMAJOR AXIS (AODP) */
/*      FROM INPUT ELEMENTS */

	A1=pow((XKE/XNO), TOTHRD);
	COSIO=cos(XINCL);
	THETA2=COSIO*COSIO;
	X3THM1=3.0 * THETA2 - 1.0;
	EOSQ = EO * EO;
	BETAO2 = 1.0 - EOSQ;
	BETAO = sqrt(BETAO2);
	DEL1 = 1.5 * CK2 * X3THM1 / (A1 * A1 * BETAO * BETAO2);
	AO = A1 * (1.0 - DEL1 * (0.5 * TOTHRD +
				DEL1 * (1.0 + 134.0 / 81.0 * DEL1)));
	DELO = 1.5 * CK2 * X3THM1 / (AO * AO * BETAO * BETAO2);
	XNODP = XNO / (1.0 + DELO);
	AODP = AO / (1.0 - DELO);

/*      INITIALIZATION */

/*      FOR PERIGEE BELOW 156 KM, THE VALUES OF
*      S AND QOMS2T ARE ALTERED */

	S4 = S;
	QOMS24 = QOMS2T;
	PERIGE = (AODP * (1.0 - EO) - AE) * XKMPER;

/*      IF(PERIGE .GE. 156.) GO TO 10 */

	if(PERIGE < 156.0) {
	    S4 = PERIGE - 78.0;

	    if(PERIGE <= 98.0)  { /* GO TO 9 */
		S4 = 20.0;
	    }

	    QOMS24 = pow((120.0 - S4) * AE / XKMPER, 4.0); /* 9 */
	    S4 = S4 / XKMPER + AE;
	}
	PINVSQ = 1.0 / (AODP * AODP * BETAO2 * BETAO2); /* 10 */
	SING = sin(OMEGAO);
	COSG = cos(OMEGAO);
	TSI = 1.0 / (AODP - S4);
	ETA = AODP * EO * TSI;
	ETASQ = ETA * ETA;
	EETA = EO * ETA;
	PSISQ = fabs(1.0 - ETASQ);
	COEF = QOMS24 * pow(TSI, 4.0);
	COEF1 = COEF / pow(PSISQ, 3.5);
	C2 = COEF1 * XNODP * (AODP * (1.0 + 1.5 * ETASQ +
				      EETA * (4.0 + ETASQ)) +
			      .75 * CK2 * TSI / PSISQ * X3THM1 *
			      (8.0 + 3.0 * ETASQ * (8.0 + ETASQ)));
	C1 = BSTAR * C2;
	SINIO = sin(XINCL);
	A3OVK2 = -XJ3 / CK2 * AE * AE * AE; /* A3OVK2=-XJ3/CK2*AE**3; */
	X1MTH2 = 1.0 - THETA2;
	C4 = 2.0 * XNODP * COEF1 * AODP * BETAO2 *
	    (ETA * (2.0 + .5 * ETASQ) + EO * (.5 + 2.0 * ETASQ) -
	     2.0 * CK2 * TSI / (AODP * PSISQ) *
	     (-3.0 * X3THM1 * (1.0 - 2.0 * EETA + ETASQ *
			       (1.5 - .5 * EETA)) +
	      .75 * X1MTH2 * (2.0 * ETASQ - EETA *
			      (1.0 + ETASQ)) * cos(2.0 * OMEGAO)));
	THETA4 = THETA2 * THETA2;
	TEMP1 = 3.0 * CK2 * PINVSQ * XNODP;
	TEMP2 = TEMP1 * CK2 * PINVSQ;
	TEMP3 = 1.25 * CK4 * PINVSQ * PINVSQ * XNODP;
	XMDOT = XNODP + 0.5 * TEMP1 * BETAO * X3THM1 + .0625 * TEMP2 * BETAO *
	    (13.0 - 78.0 * THETA2 + 137.0 * THETA4);
	X1M5TH=1.0 - 5.0 * THETA2;
	OMGDOT = -.5 * TEMP1 * X1M5TH + .0625 * TEMP2 * 
	    (7.0 - 114.0 * THETA2 + 395.0 * THETA4) +
	    TEMP3 * (3.0 - 36.0 * THETA2 + 49.0 * THETA4);
	XHDOT1 = -TEMP1 * COSIO;
	XNODOT = XHDOT1 + (.5 * TEMP2 * (4.0 - 19.0 * THETA2) +
			   2.0 * TEMP3 * (3.0 - 7.0 * THETA2)) * COSIO;
	XNODCF = 3.5 * BETAO2 * XHDOT1 * C1;
	T2COF = 1.5 * C1;
	XLCOF = .125 * A3OVK2 * SINIO * (3.0 + 5.0 * COSIO) / (1.0 + COSIO);
	AYCOF = .25 * A3OVK2 * SINIO;
	X7THM1 = 7.0 * THETA2 - 1.0;
/*   90 IFLAG=0 */

#ifdef SDP_DEEP_DEBUG
	printf("calling dpinit\n");
	printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
	       EOSQ,SINIO,COSIO,BETAO,AODP,THETA2,
	       SING,COSG,BETAO2,XMDOT,OMGDOT,XNODOT,XNODP);
#endif
	dpinit(sat, EOSQ, SINIO, COSIO, BETAO, AODP, THETA2,
	       SING, COSG, BETAO2, XMDOT, OMGDOT, XNODOT, XNODP);

/*      CALL DPINIT(EOSQ,SINIO,COSIO,BETAO,AODP,THETA2,
	1         SING,COSG,BETAO2,XMDOT,OMGDOT,XNODOT,XNODP) */

/*      UPDATE FOR SECULAR GRAVITY AND ATMOSPHERIC DRAG */
    }

    XMDF = XMO + XMDOT * TSINCE; /* 100 */
    OMGADF = OMEGAO + OMGDOT * TSINCE;
    XNODDF = XNODEO + XNODOT * TSINCE;
    TSQ = TSINCE * TSINCE;
    XNODE = XNODDF + XNODCF * TSQ;
    TEMPA = 1.0 - C1 * TSINCE;
    TEMPE = BSTAR * C4 * TSINCE;
    TEMPL = T2COF * TSQ;
    XN = XNODP;

    if(TSINCE == 0.0) {
	XMDF_seco   = XMDF;
	OMGADF_seco = OMGADF;
	XNODE_seco  = XNODE;
	EM_seco     = EM;
	XINC_seco   = XINC;
	XN_seco     = XN;
    }

    dpsec(sat, &XMDF, &OMGADF, &XNODE, &EM, &XINC, &XN, TSINCE);

    if(TSINCE == 0.0) {
	XMDF_seco   = XMDF - XMDF_seco;
	OMGADF_seco = OMGADF - OMGADF_seco;
	XNODE_seco  = XNODE - XNODE_seco;
	EM_seco     = EM - EM_seco;
	XINC_seco   = XINC - XINC_seco;
	XN_seco     = XN - XN_seco;

#if 0
	printf("XMDF_seco   = %e\n", XMDF_seco);
	printf("OMGADF_seco = %e\n", OMGADF_seco);
	printf("XNODE_seco  = %e\n", XNODE_seco);
	printf("EM_seco     = %e\n", EM_seco);
	printf("XINC_seco   = %e\n", XINC_seco);
	printf("XN_seco     = %e\n", XN_seco);
#endif
    }

    /*
    XMDF   -= XMDF_seco;
    OMGADF -= OMGADF_seco;
    XNODE  -= XNODE_seco;
    EM     -= EM_seco;
    XINC   -= XINC_seco;
    XN     -= XN_seco;
    */

    A = pow(XKE/XN, TOTHRD) * TEMPA * TEMPA;
    E = EM - TEMPE;
#ifdef SDP_DEEP_DEBUG
    printf("*** E = %f\n", E);
#endif
    XMAM = XMDF + XNODP * TEMPL;
/*      CALL DPPER(E,XINC,OMGADF,XNODE,XMAM) */

#ifdef SDP_DEEP_DEBUG
    printf("%12s %12s %12s %12s %12s\n",
	   "E", "XINC", "OMGADF", "XNODE", "XMAM");
    printf("%12f %12f %12f %12f %12f\n",
	   E, XINC, OMGADF, XNODE, XMAM);
#endif

    if(TSINCE == 0.0) {
	E_pero      = E;
	XINC_pero   = XINC;
	OMGADF_pero = OMGADF;
	XNODE_pero  = XNODE;
	XMAM_pero   = XMAM;
    }

    dpper(sat, &E, &XINC, &OMGADF, &XNODE, &XMAM, TSINCE);

    if(TSINCE == 0.0) {
	E_pero      = E - E_pero;
	XINC_pero   = XINC - XINC_pero;
	OMGADF_pero = OMGADF - OMGADF_pero;
	XNODE_pero  = XNODE - XNODE_pero;
	XMAM_pero   = XMAM - XMAM_pero;

#if 0
	printf("E_pero      = %e\n", E_pero);
	printf("XINC_pero   = %e\n", XINC_pero);
	printf("OMGADF_pero = %e\n", OMGADF_pero);
	printf("XNODE_pero  = %e\n", XNODE_pero);
	printf("XMAM_pero   = %e\n\n", XMAM_pero);
#endif
    }

    /*
    E      -= E_pero;
    XINC   -= XINC_pero;
    OMGADF -= OMGADF_pero;
    XNODE  -= XNODE_pero;
    XMAM   -= XMAM_pero;
    */
    XL = XMAM + OMGADF + XNODE;
    BETA = sqrt(1.0 - E * E);
    XN = XKE / pow(A, 1.5);

/*      LONG PERIOD PERIODICS */

    AXN = E * cos(OMGADF);
    TEMP=1./(A*BETA*BETA);
    XLL=TEMP*XLCOF*AXN;
    AYNL=TEMP*AYCOF;
    XLT=XL+XLL;
    AYN=E*sin(OMGADF)+AYNL;

/*      SOLVE KEPLERS EQUATION */

    CAPU=fmod(XLT-XNODE, TWOPI);
    TEMP2=CAPU;
/*      DO 130 I=1,10*/
    for(i = 1; i < 10; i++) {
	SINEPW=sin(TEMP2);
      COSEPW=cos(TEMP2);
      TEMP3=AXN*SINEPW;
      TEMP4=AYN*COSEPW;
      TEMP5=AXN*COSEPW;
      TEMP6=AYN*SINEPW;
      EPW=(CAPU-TEMP4+TEMP3-TEMP2)/(1.-TEMP5-TEMP6)+TEMP2;
/*      IF(ABS(EPW-TEMP2) .LE. E6A) GO TO 140 */
      if(fabs(EPW-TEMP2) <= E6A)
	  break;
      TEMP2=EPW; /* 130 */
    }

/*      SHORT PERIOD PRELIMINARY QUANTITIES */

    ECOSE=TEMP5+TEMP6; /* 140  */
    ESINE=TEMP3-TEMP4;
    ELSQ=AXN*AXN+AYN*AYN;
    TEMP=1.-ELSQ;
    PL=A*TEMP;
    R=A*(1.-ECOSE);
    TEMP1=1./R;
    RDOT=XKE*sqrt(A)*ESINE*TEMP1;
    RFDOT=XKE*sqrt(PL)*TEMP1;
    TEMP2=A*TEMP1;
    BETAL=sqrt(TEMP);
    TEMP3=1./(1.+BETAL);
    COSU=TEMP2*(COSEPW-AXN+AYN*ESINE*TEMP3);
    SINU=TEMP2*(SINEPW-AYN-AXN*ESINE*TEMP3);
    U=actan(SINU,COSU);
    SIN2U=2.*SINU*COSU;
    COS2U=2.*COSU*COSU-1.0;
    TEMP=1./PL;
    TEMP1=CK2*TEMP;
    TEMP2=TEMP1*TEMP;

/*      UPDATE FOR SHORT PERIODICS */

    RK=R*(1.-1.5*TEMP2*BETAL*X3THM1)+.5*TEMP1*X1MTH2*COS2U;
    UK=U - .25 * TEMP2 * X7THM1 * SIN2U;
    XNODEK=XNODE+1.5*TEMP2*COSIO*SIN2U;
    XINCK=XINC+1.5*TEMP2*COSIO*SINIO*COS2U;
    RDOTK=RDOT-XN*TEMP1*X1MTH2*SIN2U;
    RFDOTK=RFDOT+XN*TEMP1*(X1MTH2*COS2U+1.5*X3THM1);

/*      ORIENTATION VECTORS */
    SINUK=sin(UK);
    COSUK=cos(UK);
    SINIK=sin(XINCK);
    COSIK=cos(XINCK);
    SINNOK=sin(XNODEK);
    COSNOK=cos(XNODEK);
    XMX=-SINNOK*COSIK;
    XMY=COSNOK*COSIK;
    UX=XMX*SINUK+COSNOK*COSUK;
    UY=XMY*SINUK+SINNOK*COSUK;
    UZ=SINIK*SINUK;
    VX=XMX*COSUK-COSNOK*SINUK;
    VY=XMY*COSUK-SINNOK*SINUK;
    VZ=SINIK*COSUK;
#if 0
    printf("UX = %f VX = %f RK = %f RDOTK = %f RFDOTK = %f\n",
	   UX, VX, RK, RDOTK, RFDOTK);
#endif
/*      POSITION AND VELOCITY */

    pos->x = RK*UX;
    pos->y = RK*UY;
    pos->z = RK*UZ;
    dpos->x = RDOTK*UX+RFDOTK*VX;
    dpos->y = RDOTK*UY+RFDOTK*VY;
    dpos->z = RDOTK*UZ+RFDOTK*VZ;
/*      RETURN
      END */
}

/* For RCS Only -- Do Not Edit */
static char *rcsid[2] = {(char *)rcsid, "@(#) $RCSfile: sdp4.c,v $ $Date: 2002/12/26 05:43:07 $ $Revision: 1.5 $ $Name:  $"};
