/*     ----------------------------------------------------------------
*
*                               sgp4unit.cpp
*
*    this file contains the sgp4 procedures for analytical propagation
*    of a satellite. the code was originally released in the 1980 and 1986
*    spacetrack papers. a detailed discussion of the theory and history
*    may be found in the 2006 aiaa paper by vallado, crawford, hujsak,
*    and kelso.
*
*                            companion code for
*               fundamentals of astrodynamics and applications
*                                    2007
*                              by david vallado
*
*       (w) 719-573-2600, email dvallado@agi.com
*
*    current :
*              16 nov 07  david vallado
*                           misc fixes for better compliance
*    changes :
*              20 apr 07  david vallado
*                           misc fixes for constants
*              11 aug 06  david vallado
*                           chg lyddane choice back to strn3, constants, misc doc
*              15 dec 05  david vallado
*                           misc fixes
*              26 jul 05  david vallado
*                           fixes for paper
*                           note that each fix is preceded by a
*                           comment with "sgp4fix" and an explanation of
*                           what was changed
*              10 aug 04  david vallado
*                           2nd printing baseline working
*              14 may 01  david vallado
*                           2nd edition baseline
*                     80  norad
*                           original baseline
*       ----------------------------------------------------------------      */

#include "sgp4unit.h"

const char help = 'n';
FILE *dbgfile;

#define pi 3.14159265358979323846


/* ----------- local functions - only ever used internally by sgp4 ---------- */
static void dpper
     (
       double e3,     double ee2,    double peo,     double pgho,   double pho,
       double pinco,  double plo,    double se2,     double se3,    double sgh2,
       double sgh3,   double sgh4,   double sh2,     double sh3,    double si2,
       double si3,    double sl2,    double sl3,     double sl4,    double t,
       double xgh2,   double xgh3,   double xgh4,    double xh2,    double xh3,
       double xi2,    double xi3,    double xl2,     double xl3,    double xl4,
       double zmol,   double zmos,   double inclo,
       char init,
       double& ep,    double& inclp, double& nodep,  double& argpp, double& mp
     );

static void dscom
     (
       double epoch,  double ep,     double argpp,   double tc,     double inclp,
       double nodep,  double np,
       double& snodm, double& cnodm, double& sinim,  double& cosim, double& sinomm,
       double& cosomm,double& day,   double& e3,     double& ee2,   double& em,
       double& emsq,  double& gam,   double& peo,    double& pgho,  double& pho,
       double& pinco, double& plo,   double& rtemsq, double& se2,   double& se3,
       double& sgh2,  double& sgh3,  double& sgh4,   double& sh2,   double& sh3,
       double& si2,   double& si3,   double& sl2,    double& sl3,   double& sl4,
       double& s1,    double& s2,    double& s3,     double& s4,    double& s5,
       double& s6,    double& s7,    double& ss1,    double& ss2,   double& ss3,
       double& ss4,   double& ss5,   double& ss6,    double& ss7,   double& sz1,
       double& sz2,   double& sz3,   double& sz11,   double& sz12,  double& sz13,
       double& sz21,  double& sz22,  double& sz23,   double& sz31,  double& sz32,
       double& sz33,  double& xgh2,  double& xgh3,   double& xgh4,  double& xh2,
       double& xh3,   double& xi2,   double& xi3,    double& xl2,   double& xl3,
       double& xl4,   double& nm,    double& z1,     double& z2,    double& z3,
       double& z11,   double& z12,   double& z13,    double& z21,   double& z22,
       double& z23,   double& z31,   double& z32,    double& z33,   double& zmol,
       double& zmos
     );

static void dsinit
     (
       gravconsttype whichconst,
       double cosim,  double emsq,   double argpo,   double s1,     double s2,
       double s3,     double s4,     double s5,      double sinim,  double ss1,
       double ss2,    double ss3,    double ss4,     double ss5,    double sz1,
       double sz3,    double sz11,   double sz13,    double sz21,   double sz23,
       double sz31,   double sz33,   double t,       double tc,     double gsto,
       double mo,     double mdot,   double no,      double nodeo,  double nodedot,
       double xpidot, double z1,     double z3,      double z11,    double z13,
       double z21,    double z23,    double z31,     double z33,    double ecco,
       double eccsq,  double& em,    double& argpm,  double& inclm, double& mm,
       double& nm,    double& nodem,
       int& irez,
       double& atime, double& d2201, double& d2211,  double& d3210, double& d3222,
       double& d4410, double& d4422, double& d5220,  double& d5232, double& d5421,
       double& d5433, double& dedt,  double& didt,   double& dmdt,  double& dndt,
       double& dnodt, double& domdt, double& del1,   double& del2,  double& del3,
       double& xfact, double& xlamo, double& xli,    double& xni
     );

static void dspace
     (
       int irez,
       double d2201,  double d2211,  double d3210,   double d3222,  double d4410,
       double d4422,  double d5220,  double d5232,   double d5421,  double d5433,
       double dedt,   double del1,   double del2,    double del3,   double didt,
       double dmdt,   double dnodt,  double domdt,   double argpo,  double argpdot,
       double t,      double tc,     double gsto,    double xfact,  double xlamo,
       double no,
       double& atime, double& em,    double& argpm,  double& inclm, double& xli,
       double& mm,    double& xni,   double& nodem,  double& dndt,  double& nm
     );

static void initl
     (
       int satn,      gravconsttype whichconst,
       double ecco,   double epoch,  double inclo,   double& no,
       char& method,
       double& ainv,  double& ao,    double& con41,  double& con42, double& cosio,
       double& cosio2,double& eccsq, double& omeosq, double& posq,
       double& rp,    double& rteosq,double& sinio , double& gsto
     );

/* -----------------------------------------------------------------------------
*
*                           procedure dpper
*
*  this procedure provides deep space long period periodic contributions
*    to the mean elements.  by design, these periodics are zero at epoch.
*    this used to be dscom which included initialization, but it's really a
*    recurring function.
*
*  author        : david vallado                  719-573-2600   28 jun 2005
*
*  inputs        :
*    e3          -
*    ee2         -
*    peo         -
*    pgho        -
*    pho         -
*    pinco       -
*    plo         -
*    se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 -
*    t           -
*    xh2, xh3, xi2, xi3, xl2, xl3, xl4 -
*    zmol        -
*    zmos        -
*    ep          - eccentricity                           0.0 - 1.0
*    inclo       - inclination - needed for lyddane modification
*    nodep       - right ascension of ascending node
*    argpp       - argument of perigee
*    mp          - mean anomaly
*
*  outputs       :
*    ep          - eccentricity                           0.0 - 1.0
*    inclp       - inclination
*    nodep        - right ascension of ascending node
*    argpp       - argument of perigee
*    mp          - mean anomaly
*
*  locals        :
*    alfdp       -
*    betdp       -
*    cosip  , sinip  , cosop  , sinop  ,
*    dalf        -
*    dbet        -
*    dls         -
*    f2, f3      -
*    pe          -
*    pgh         -
*    ph          -
*    pinc        -
*    pl          -
*    sel   , ses   , sghl  , sghs  , shl   , shs   , sil   , sinzf , sis   ,
*    sll   , sls
*    xls         -
*    xnoh        -
*    zf          -
*    zm          -
*
*  coupling      :
*    none.
*
*  references    :
*    hoots, roehrich, norad spacetrack report #3 1980
*    hoots, norad spacetrack report #6 1986
*    hoots, schumacher and glover 2004
*    vallado, crawford, hujsak, kelso  2006
  ----------------------------------------------------------------------------*/

static void dpper
     (
       double e3,     double ee2,    double peo,     double pgho,   double pho,
       double pinco,  double plo,    double se2,     double se3,    double sgh2,
       double sgh3,   double sgh4,   double sh2,     double sh3,    double si2,
       double si3,    double sl2,    double sl3,     double sl4,    double t,
       double xgh2,   double xgh3,   double xgh4,    double xh2,    double xh3,
       double xi2,    double xi3,    double xl2,     double xl3,    double xl4,
       double zmol,   double zmos,   double inclo,
       char init,
       double& ep,    double& inclp, double& nodep,  double& argpp, double& mp
     )
{
     /* --------------------- local variables ------------------------ */
     const double twopi = 2.0 * pi;
     double alfdp, betdp, cosip, cosop, dalf, dbet, dls,
          f2,    f3,    pe,    pgh,   ph,   pinc, pl ,
          sel,   ses,   sghl,  sghs,  shll, shs,  sil,
          sinip, sinop, sinzf, sis,   sll,  sls,  xls,
          xnoh,  zf,    zm,    zel,   zes,  znl,  zns;

     /* ---------------------- constants ----------------------------- */
     zns   = 1.19459e-5;
     zes   = 0.01675;
     znl   = 1.5835218e-4;
     zel   = 0.05490;

     /* --------------- calculate time varying periodics ----------- */
     zm    = zmos + zns * t;
     // be sure that the initial call has time set to zero
     if (init == 'y')
         zm = zmos;
     zf    = zm + 2.0 * zes * sin(zm);
     sinzf = sin(zf);
     f2    =  0.5 * sinzf * sinzf - 0.25;
     f3    = -0.5 * sinzf * cos(zf);
     ses   = se2* f2 + se3 * f3;
     sis   = si2 * f2 + si3 * f3;
     sls   = sl2 * f2 + sl3 * f3 + sl4 * sinzf;
     sghs  = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf;
     shs   = sh2 * f2 + sh3 * f3;
     zm    = zmol + znl * t;
     if (init == 'y')
         zm = zmol;
     zf    = zm + 2.0 * zel * sin(zm);
     sinzf = sin(zf);
     f2    =  0.5 * sinzf * sinzf - 0.25;
     f3    = -0.5 * sinzf * cos(zf);
     sel   = ee2 * f2 + e3 * f3;
     sil   = xi2 * f2 + xi3 * f3;
     sll   = xl2 * f2 + xl3 * f3 + xl4 * sinzf;
     sghl  = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf;
     shll  = xh2 * f2 + xh3 * f3;
     pe    = ses + sel;
     pinc  = sis + sil;
     pl    = sls + sll;
     pgh   = sghs + sghl;
     ph    = shs + shll;

     if (init == 'n')
       {
       pe    = pe - peo;
       pinc  = pinc - pinco;
       pl    = pl - plo;
       pgh   = pgh - pgho;
       ph    = ph - pho;
       inclp = inclp + pinc;
       ep    = ep + pe;
       sinip = sin(inclp);
       cosip = cos(inclp);

       /* ----------------- apply periodics directly ------------ */
       //  sgp4fix for lyddane choice
       //  strn3 used original inclination - this is technically feasible
       //  gsfc used perturbed inclination - also technically feasible
       //  probably best to readjust the 0.2 limit value and limit discontinuity
       //  0.2 rad = 11.45916 deg
       //  use next line for original strn3 approach and original inclination
       //  if (inclo >= 0.2)
       //  use next line for gsfc version and perturbed inclination
       if (inclp >= 0.2)
         {
           ph     = ph / sinip;
           pgh    = pgh - cosip * ph;
           argpp  = argpp + pgh;
           nodep  = nodep + ph;
           mp     = mp + pl;
         }
         else
         {
           /* ---- apply periodics with lyddane modification ---- */
           sinop  = sin(nodep);
           cosop  = cos(nodep);
           alfdp  = sinip * sinop;
           betdp  = sinip * cosop;
           dalf   =  ph * cosop + pinc * cosip * sinop;
           dbet   = -ph * sinop + pinc * cosip * cosop;
           alfdp  = alfdp + dalf;
           betdp  = betdp + dbet;
           nodep  = fmod(nodep, twopi);
           //  sgp4fix for afspc written intrinsic functions
           // nodep used without a trigonometric function ahead
           if (nodep < 0.0)
               nodep = nodep + twopi;
           xls    = mp + argpp + cosip * nodep;
           dls    = pl + pgh - pinc * nodep * sinip;
           xls    = xls + dls;
           xnoh   = nodep;
           nodep  = atan2(alfdp, betdp);
           //  sgp4fix for afspc written intrinsic functions
           // nodep used without a trigonometric function ahead
           if (nodep < 0.0)
               nodep = nodep + twopi;
           if (fabs(xnoh - nodep) > pi)
             if (nodep < xnoh)
                nodep = nodep + twopi;
               else
                nodep = nodep - twopi;
           mp    = mp + pl;
           argpp = xls - mp - cosip * nodep;
         }
       }   // if init == 'n'

//#include "debug1.cpp"
}  // end dpper

/*-----------------------------------------------------------------------------