#include <math.h>
#include "norad.h"
#include "norad_in.h"

#define zns      1.19459E-5
#define zes      0.01675
#define znl      1.5835218E-4
#define zel      0.05490
#define thdt     4.3752691E-3

      /* Previously,  the integration step was given as two variables:      */
      /* 'stepp' (positive step = +720) and 'stepn' (negative step = -720). */
      /* Exactly why this should be made a variable,  much less _different_ */
      /* variables for positive and negative,  is entirely unclear...       */
      /* (8 Apr 2003) INTEGRATION_STEP is now a maximum integration step.   */
      /* The code in 'dpsec' splits the integration range into equally-sized */
      /* pieces of 720 minutes (half a day) or smaller.                      */
#define INTEGRATION_STEP 720.
#define STEP2 (INTEGRATION_STEP * INTEGRATION_STEP / 2.)

static double ThetaG( double jd);

/* DEEP */
void Deep_dpinit( const tle_t *tle, deep_arg_t *deep_arg)
{
   const double sinq = sin(tle->xnodeo);
   const double cosq = cos(tle->xnodeo);
   const double aqnv = 1/deep_arg->aodp;
   const double c1ss   =  2.9864797E-6;
   const double day = tle->epoch - 2415020.;
                       /* days since 1900 Jan 0.5 = JD 2415020. */
   double zcosi =  0.91744867;
   double zsini =  0.39785416;
   double zsing = -0.98088458;
   double zcosg =  0.1945905;
   double bfact, cc = c1ss, se;
   double ze = zes, zn = zns;
   double sgh, sh, si;
   double zsinh = sinq, zcosh = cosq;
   double sl;
   int iteration;

   deep_arg->thgr = ThetaG( tle->epoch);
   deep_arg->xnq = deep_arg->xnodp;
   deep_arg->xqncl = tle->xincl;
   deep_arg->omegaq = tle->omegao;

   /* If the epoch has changed,  recompute (or initialize) the lunar and */
   /* solar terms... except that now that zcosil, etc. are within the    */
   /* deep_arg structure,  instead of static,  they must _always_ be     */
   /* recomputed.  So I've commented out the 'if' part.  (Revision made  */
   /* 14 May 2005)                                                       */

/* if (day != deep_arg->preep)   */
      {
      const double xnodce = 4.5236020 - 9.2422029E-4 * day;
      const double stem = sin(xnodce);
      const double ctem = cos(xnodce);
      const double c_minus_gam = 0.228027132 * day - 1.1151842;
      const double gam = 5.8351514 + 0.0019443680 * day;
      double zx, zy;

      deep_arg->preep = day;
      deep_arg->zcosil = 0.91375164 - 0.03568096 * ctem;
      deep_arg->zsinil = sqrt(1. - deep_arg->zcosil * deep_arg->zcosil);
      deep_arg->zsinhl = 0.089683511 * stem / deep_arg->zsinil;
      deep_arg->zcoshl = sqrt(1. - deep_arg->zsinhl*deep_arg->zsinhl);
      deep_arg->zmol = FMod2p( c_minus_gam);
      zx = 0.39785416 * stem / deep_arg->zsinil;
      zy = deep_arg->zcoshl * ctem + 0.91744867 * deep_arg->zsinhl * stem;
      zx = atan2( zx, zy) + gam - xnodce;
      deep_arg->zcosgl = cos( zx);
      deep_arg->zsingl = sin( zx);
      deep_arg->zmos = FMod2p( 6.2565837 + 0.017201977 * day);
      } /* End if(day != deep_arg->preep) */

   /* Do solar terms */
   deep_arg->savtsn = 1E20;

   /* There was previously some convoluted logic here,  but it boils    */
   /* down to this:  we compute the solar terms,  then the lunar terms. */
   /* On a second pass,  we recompute the solar terms,  taking advantage */
   /* of the improved data that resulted from computing lunar terms.     */
   for( iteration = 0; iteration < 2; iteration++)
      {
      const double c1l = 4.7968065E-7;
      const double a1 = zcosg * zcosh + zsing * zcosi * zsinh;
      const double a3 = -zsing * zcosh + zcosg * zcosi * zsinh;
      const double a7 = -zcosg * zsinh + zsing * zcosi * zcosh;
      const double a8 = zsing * zsini;
      const double a9 = zsing * zsinh + zcosg * zcosi * zcosh;
      const double a10 = zcosg * zsini;
      const double a2 = deep_arg->cosio * a7 + deep_arg->sinio * a8;
      const double a4 = deep_arg->cosio * a9 + deep_arg->sinio * a10;
      const double a5 = -deep_arg->sinio * a7 + deep_arg->cosio * a8;
      const double a6 = -deep_arg->sinio * a9 + deep_arg->cosio * a10;
      const double x1 = a1 * deep_arg->cosg + a2 * deep_arg->sing;
      const double x2 = a3 * deep_arg->cosg + a4 * deep_arg->sing;
      const double x3 = -a1 * deep_arg->sing + a2 * deep_arg->cosg;
      const double x4 = -a3 * deep_arg->sing + a4 * deep_arg->cosg;
      const double x5 = a5 * deep_arg->sing;
      const double x6 = a6 * deep_arg->sing;
      const double x7 = a5 * deep_arg->cosg;
      const double x8 = a6 * deep_arg->cosg;
      const double z31 = 12 * x1 * x1 - 3 * x3 * x3;
      const double z32 = 24 * x1 * x2 - 6 * x3 * x4;
      const double z33 = 12 * x2 * x2 - 3 * x4 * x4;
      const double z11 = -6 * a1 * a5 + deep_arg->eosq * (-24 * x1 * x7 - 6 * x3 * x5);
      const double z12 = -6 * (a1 * a6 + a3 * a5) +  deep_arg->eosq *
                (-24 * (x2 * x7 + x1 * x8) - 6 * (x3 * x6 + x4 * x5));
      const double z13 = -6 * a3 * a6 + deep_arg->eosq * (-24 * x2 * x8 - 6 * x4 * x6);
      const double z21 = 6 * a2 * a5 + deep_arg->eosq * (24 * x1 * x5 - 6 * x3 * x7);
      const double z22 = 6 * (a4 * a5 + a2 * a6) +  deep_arg->eosq *
                (24 * (x2 * x5 + x1 * x6) - 6 * (x4 * x7 + x3 * x8));
      const double z23 = 6 * a4 * a6 + deep_arg->eosq * (24 * x2 * x6 - 6 * x4 * x8);
      const double s3 = cc / deep_arg->xnq;
      const double s2 = -0.5 * s3 / deep_arg->betao;
      const double s4 = s3 * deep_arg->betao;
      const double s1 = -15 * tle->eo * s4;
      const double s5 = x1 * x3 + x2 * x4;
      const double s6 = x2 * x3 + x1 * x4;
      const double s7 = x2 * x4 - x1 * x3;
      double z1 = 3 * (a1 * a1 + a2 * a2) + z31 * deep_arg->eosq;
      double z2 = 6 * (a1 * a3 + a2 * a4) + z32 * deep_arg->eosq;
      double z3 = 3 * (a3 * a3 + a4 * a4) + z33 * deep_arg->eosq;

      z1 = z1 + z1 + deep_arg->betao2 * z31;
      z2 = z2 + z2 + deep_arg->betao2 * z32;
      z3 = z3 + z3 + deep_arg->betao2 * z33;
      se = s1*zn*s5;
      si = s2*zn*(z11+z13);
      sl = -zn*s3*(z1+z3-14-6*deep_arg->eosq);
      sgh = s4*zn*(z31+z33-6);
      if (deep_arg->xqncl < 5.2359877E-2)
         sh = 0;
      else
         sh = -zn*s2*(z21+z23);
      deep_arg->ee2 = 2*s1*s6;
      deep_arg->e3 = 2*s1*s7;
      deep_arg->xi2 = 2*s2*z12;
      deep_arg->xi3 = 2*s2*(z13-z11);
      deep_arg->xl2 = -2*s3*z2;
      deep_arg->xl3 = -2*s3*(z3-z1);
      deep_arg->xl4 = -2*s3*(-21-9*deep_arg->eosq)*ze;
      deep_arg->xgh2 = 2*s4*z32;
      deep_arg->xgh3 = 2*s4*(z33-z31);
      deep_arg->xgh4 = -18*s4*ze;
      deep_arg->xh2 = -2*s2*z22;
      deep_arg->xh3 = -2*s2*(z23-z21);

      if( !iteration)   /* we compute lunar terms only on the first pass: */
         {
         deep_arg->sse = se;
         deep_arg->ssi = si;
         deep_arg->ssl = sl;
         deep_arg->ssh = sh/deep_arg->sinio;
         deep_arg->ssg = sgh-deep_arg->cosio*deep_arg->ssh;
         deep_arg->se2 = deep_arg->ee2;
         deep_arg->si2 = deep_arg->xi2;
         deep_arg->sl2 = deep_arg->xl2;
         deep_arg->sgh2 = deep_arg->xgh2;
         deep_arg->sh2 = deep_arg->xh2;
         deep_arg->se3 = deep_arg->e3;
         deep_arg->si3 = deep_arg->xi3;
         deep_arg->sl3 = deep_arg->xl3;
         deep_arg->sgh3 = deep_arg->xgh3;
         deep_arg->sh3 = deep_arg->xh3;
         deep_arg->sl4 = deep_arg->xl4;
         deep_arg->sgh4 = deep_arg->xgh4;
         zcosg = deep_arg->zcosgl;
         zsing = deep_arg->zsingl;
         zcosi = deep_arg->zcosil;
         zsini = deep_arg->zsinil;
         zcosh = deep_arg->zcoshl*cosq+deep_arg->zsinhl*sinq;
         zsinh = sinq*deep_arg->zcoshl-cosq*deep_arg->zsinhl;
         zn = znl;
         cc = c1l;
         ze = zel;
         }
      }

   deep_arg->sse += se;
   deep_arg->ssi += si;
   deep_arg->ssl += sl;
   deep_arg->ssg += sgh - deep_arg->cosio / deep_arg->sinio * sh;
   deep_arg->ssh += sh / deep_arg->sinio;

   if( deep_arg->xnq >= 0.00826 && deep_arg->xnq <= 0.00924 && tle->eo >= .5)
      {           /* start of 12-hour orbit, e >.5 section */
      const double root22 = 1.7891679E-6;
      const double root32 = 3.7393792E-7;
      const double root44 = 7.3636953E-9;
      const double root52 = 1.1428639E-7;
      const double root54 = 2.1765803E-9;
      const double g201 = -0.306 - (tle->eo - 0.64) * 0.440;
      const double eoc = tle->eo * deep_arg->eosq;
      const double sini2 = deep_arg->sinio*deep_arg->sinio;
      const double f220 = 0.75*(1+2*deep_arg->cosio+deep_arg->theta2);
      const double f221 = 1.5 * sini2;
      const double f321 = 1.875 * deep_arg->sinio * (1 - 2 *\
               deep_arg->cosio - 3 * deep_arg->theta2);
      const double f322 = -1.875*deep_arg->sinio*(1+2*
               deep_arg->cosio-3*deep_arg->theta2);
      const double f441 = 35 * sini2 * f220;
      const double f442 = 39.3750 * sini2 * sini2;
      const double f522 = 9.84375*deep_arg->sinio*(sini2*(1-2*deep_arg->cosio-5*
                 deep_arg->theta2)+0.33333333*(-2+4*deep_arg->cosio+
                 6*deep_arg->theta2));
      const double f523 = deep_arg->sinio*(4.92187512*sini2*(-2-4*
                 deep_arg->cosio+10*deep_arg->theta2)+6.56250012
                 *(1+2*deep_arg->cosio-3*deep_arg->theta2));
      const double f542 = 29.53125*deep_arg->sinio*(2-8*
                 deep_arg->cosio+deep_arg->theta2*
                 (-12+8*deep_arg->cosio+10*deep_arg->theta2));
      const double f543 = 29.53125*deep_arg->sinio*(-2-8*deep_arg->cosio+
                 deep_arg->theta2*(12+8*deep_arg->cosio-10*
                 deep_arg->theta2));
      double g410, g422, g520, g521, g532, g533;
      double g211, g310, g322;
      double temp, temp1;

      deep_arg->resonance_flag = 1;       /* it _is_ resonant... */
      deep_arg->synchronous_flag = 0;     /* but it's not synchronous */
             /* Geopotential resonance initialization for 12 hour orbits: */
      if (tle->eo <= 0.65)
         {
         g211 = 3.616-13.247*tle->eo+16.290*deep_arg->eosq;
         g310 = -19.302+117.390*tle->eo-228.419*
                    deep_arg->eosq+156.591*eoc;
         g322 = -18.9068+109.7927*tle->eo-214.6334*
                    deep_arg->eosq+146.5816*eoc;
         g410 = -41.122+242.694*tle->eo-471.094*
                    deep_arg->eosq+313.953*eoc;
         g422 = -146.407+841.880*tle->eo-1629.014*
                    deep_arg->eosq+1083.435*eoc;
         g520 = -532.114+3017.977*tle->eo-5740*
                    deep_arg->eosq+3708.276*eoc;
         }
      else
         {
         g211 = -72.099+331.819*tle->eo-508.738*
                    deep_arg->eosq+266.724*eoc;
         g310 = -346.844+1582.851*tle->eo-2415.925*
                    deep_arg->eosq+1246.113*eoc;
         g322 = -342.585+1554.908*tle->eo-2366.899*
                    deep_arg->eosq+1215.972*eoc;
         g410 = -1052.797+4758.686*tle->eo-7193.992*
                    deep_arg->eosq+3651.957*eoc;
         g422 = -3581.69+16178.11*tle->eo-24462.77*
                    deep_arg->eosq+ 12422.52*eoc;
         if (tle->eo <= 0.715)
            g520 = 1464.74-4664.75*tle->eo+3763.64*deep_arg->eosq;
         else
            g520 = -5149.66+29936.92*tle->eo-54087.36*
                         deep_arg->eosq+31324.56*eoc;
         } /* End if (tle->eo <= 0.65) */

      if (tle->eo < 0.7)
         {
         g533 = -919.2277+4988.61*tle->eo-9064.77*
                    deep_arg->eosq+5542.21*eoc;
         g521 = -822.71072+4568.6173*tle->eo-8491.4146*
                    deep_arg->eosq+5337.524*eoc;
         g532 = -853.666+4690.25*tle->eo-8624.77*
                    deep_arg->eosq+ 5341.4*eoc;
         }
      else
         {
         g533 = -37995.78+161616.52*tle->eo-229838.2*
                    deep_arg->eosq+109377.94*eoc;
         g521 = -51752.104+218913.95*tle->eo-309468.16*
                    deep_arg->eosq+146349.42*eoc;
         g532 = -40023.88+170470.89*tle->eo-242699.48*
                    deep_arg->eosq+115605.82*eoc;
         } /* End if (tle->eo <= 0.7) */

      temp1 = 3 * deep_arg->xnq * deep_arg->xnq * aqnv * aqnv;
      temp = temp1*root22;
      deep_arg->d2201 = temp * f220 * g201;
      deep_arg->d2211 = temp * f221 * g211;
      temp1 *= aqnv;
      temp = temp1*root32;
      deep_arg->d3210 = temp * f321 * g310;
      deep_arg->d3222 = temp * f322 * g322;
      temp1 *= aqnv;
      temp = 2*temp1*root44;
      deep_arg->d4410 = temp * f441 * g410;
      deep_arg->d4422 = temp * f442 * g422;
      temp1 *= aqnv;
      temp = temp1*root52;
      deep_arg->d5220 = temp * f522 * g520;
      deep_arg->d5232 = temp * f523 * g532;
      temp = 2*temp1*root54;
      deep_arg->d5421 = temp * f542 * g521;
      deep_arg->d5433 = temp * f543 * g533;
      deep_arg->xlamo = tle->xmo+tle->xnodeo+tle->xnodeo-deep_arg->thgr-deep_arg->thgr;
      bfact = deep_arg->xmdot + deep_arg->xnodot+
                   deep_arg->xnodot - thdt - thdt;
      bfact += deep_arg->ssl + deep_arg->ssh + deep_arg->ssh;
      }        /* end of 12-hour orbit, e >.5 section */
   else if( (deep_arg->xnq < 0.0052359877) && (deep_arg->xnq > 0.0034906585))
      {
      const double q22    =  1.7891679E-6;
      const double q31    =  2.1460748E-6;
      const double q33    =  2.2123015E-7;
      const double cosio_plus_1 = 1. + deep_arg->cosio;
      const double g200 = 1+deep_arg->eosq*(-2.5+0.8125*deep_arg->eosq);
      const double g300 = 1+deep_arg->eosq*(-6+6.60937*deep_arg->eosq);
      const double f311 = 0.9375*deep_arg->sinio*deep_arg->sinio*
             (1+3*deep_arg->cosio)-0.75*cosio_plus_1;
      const double g310 = 1+2*deep_arg->eosq;
      const double f220 = 0.75 * cosio_plus_1 * cosio_plus_1;
      const double f330 = 2.5 * f220 * cosio_plus_1;