/*

SGP4 is reverse engineered to calculate the SGP4/SDP4 mean orbital elements
from a satellite geocentric position vector, rr2, and associated velocity
vector, vv2. The position and velocity vectors are passed as arguments to
the routine in standard SGP4 units of earth radii and earth radii per minute.

The first section calculates the six classical osculating orbital elements:

 ik  inclination
 ok  node
 ek  eccentricity
 wk  perigee
 mk  anomaly
 xn  motion

In addition, some other osculating quantities are found:

 rk       geocentric distance
 uk       longitude
 rdotk    vv2 component parallel to rr2
 rfdotk   vv2 component perpendicular to rr2
 aodp     semi major axis
 pl       semi parameter 
 
Supporting subroutines, found at the bottom of this listing, include:

 acose
 norm
 dot
 cross
 unitv
 vadd
 smult
 fmod2p

*/


  // vectors to SGP4 mean elements
void Satellite :: rvel(double* rr2, double* vv2)
{
    //double twopi = 2 * 3.14159265358979323846;
    int i;

    /* classical osculating orbit elements calculated from vectors rr2, vv2  */
    double xinck, xnodek, ek, mk, wk, xn, rk, uk, aodp, pl, rdotk, rfdotk, temp;

    double h[3], n[3], vec[3], vk[3];
    smult(1. / xke, vv2, vk);
    cross(rr2, vk, h);
    pl = dot(h, h);
    double vz[] = {0, 0, 1};
    double vy[3], t;
    cross(vz, h, n);
    if(n[0] == 0. && n[1] == 0.) n[0] = 1.;
    unitv(n, n);
    rk = norm(rr2);
    rdotk = dot(rr2, vv2) / rk;
    rfdotk = norm(h) * xke / rk;
    temp = dot(rr2, n) / rk;
    uk = acose(temp);
    if(rr2[2] < 0.) uk = twopi - uk;
    cross(vk, h, vz);
    smult(-1. / rk, rr2, vy);
    vadd(vz, vy, vec);
    ek = norm(vec);
    if( ek >= 1.) return;         // open orbit
    xnodek = atan2( n[1], n[0]);
    if( xnodek < 0.)
       xnodek += twopi;
    temp = sqrt( h[0] * h[0] + h[1] * h[1]);
    xinck =  atan2( temp, h[2]);
    temp = dot(vec, n) / ek;
    wk = acose( temp);
    if(vec[2] < 0.)
      wk = fmod2p(twopi - wk);
    aodp = pl / (1. - ek*ek);
    xn = xke * pow(aodp, -1.5);

    double cosio, sinio, sin2u, cos2u, temp1, temp2,
     rdot, rfdot, theta2, betal, x3thm1, x1mth2, x7thm1,
     esine, ecose, elsq, cosepw, sinepw, axn, ayn,
     cosu, sinu, capu, /*a3ovk2,*/ xlcof, aycof, aynl, xll,
     xl, a0, a1, a2, d0, d1, beta, beta2, r, u;

/*
In the first loop the osculating elements rk, uk, xnodek, xinck, rdotk,
and rfdotk are used as anchors to find the corresponding final SGP4
mean elements r, u, xnodeo, xincl, rdot, and rfdot.  Several other final
mean values based on these are also found: betal, cosio, sinio, theta2,
cos2u, sin2u, x3thm1, x7thm1, x1mth2.  In addition, the osculating values
initially held by aodp, pl, and xn are replaced by intermediate
(not osculating and not mean) values used by SGP4.  The loop converges
on the value of pl in about four iterations.
*/

    /*  seed value for first loop */
    xincl = xinck;
    u = uk;

    for (i = 0; i < 99; ++i)
    {
      a2 = pl;
      betal = sqrt(pl / aodp);
      temp1 = ck2  / pl;
      temp2 = temp1 / pl;
      cosio = cos(xincl);
      sinio = sin(xincl);
      sin2u = sin(2.*u);
      cos2u = cos(2.*u);
      theta2 = cosio * cosio;
      x3thm1 = 3. * theta2 - 1.;
      x1mth2 = 1. - theta2;
      x7thm1 = 7. * theta2 - 1.;
      r = (rk - .5 * temp1 * x1mth2 * cos2u)
         / (1. - 1.5 * temp2 * betal * x3thm1);
      u = uk + .25 * temp2 * x7thm1 * sin2u;
      xnodeo = xnodek - 1.5 * temp2 * cosio * sin2u;
      xincl = xinck - 1.5 * temp2 * cosio * sinio * cos2u;
      rdot = rdotk + xn * temp1 * x1mth2 * sin2u;
      rfdot = rfdotk - xn * temp1 * (x1mth2 * cos2u + 1.5 * x3thm1);
      temp = r * rfdot / xke;
      pl = temp * temp;

      // vis-viva equation
      temp = 2. / r - (rdot*rdot + rfdot*rfdot) / (xke*xke);
      aodp = 1. / temp;

      xn = xke * pow(aodp, -1.5);
      if(fabs(a2 - pl) < 1.e-13) break;
    }

/*
The next values are calculated from constants and a combination of mean
and intermediate quantities from the first loop.  These values all remain
fixed and are used in the second loop.
*/


    // preliminary values for the second loop
    ecose = 1. - r / aodp;
    esine = r * rdot / (xke * sqrt(aodp));   /* needed for Kepler's eqn.  */
    elsq = 1. - pl / aodp;               /* intermediate eccentricity squared */
    //a3ovk2 = -xj3 / ck2;
    xlcof = .125 * a3ovk2 * sinio * (3. + 5. * cosio)
          / (1. + cosio);
    aycof = .25 * a3ovk2 * sinio;
    temp1 = esine / (1. + sqrt(1. - elsq));
    cosu = cos(u);
    sinu = sin(u);

/*
The second loop normally converges in about six iterations to the final
mean value for the eccentricity, eo.  The mean perigee, omegao, is also
determined.  Cosepw and sinepw are found to twelve decimal places and
are used to calculate an intermediate value for the eccentric anomaly,
temp2.  Temp2 is then used in Kepler's equation to find an intermediate
value for the true longitude, capu.
*/
    /*  seed values for loop  */
    eo = sqrt(elsq);
    omegao = wk;
    axn = eo * cos(omegao);

    for (i = 0; i < 99; ++i)
    {
       a2 = eo;
       beta = 1. - eo*eo;
       temp = 1. / (aodp * beta);
       aynl = temp * aycof;
       ayn = eo * sin(omegao) + aynl;
       cosepw = r * cosu / aodp + axn - ayn * temp1;
       sinepw = r * sinu / aodp + ayn + axn * temp1;
       axn = cosepw * ecose + sinepw * esine;
       ayn = sinepw * ecose - cosepw * esine;
       omegao = fmod2p(atan2(ayn - aynl, axn));
       if(eo > .5) eo = .9*eo + .1*(axn / cos(omegao));
       else eo = axn / cos(omegao);
       if(eo > .999) eo = .999;
       if(fabs(a2 - eo) < 1.e-9) break;
    }

    temp2 = atan2(sinepw, cosepw);
    capu = temp2 - esine;             /* Kepler's equation */
    xll = temp * xlcof * axn;

    /* xll adjusts the intermediate true longitude,  */
    /* capu, to the mean true longitude, xl          */
    xl = capu - xll;

    xmo = fmod2p(xl - omegao);        /* mean anomaly */

/*
The third loop usually converges after three iterations to the
mean semi-major axis, a1, which is then used to find the mean motion, xno.
*/

    a0 = aodp;
    a1 = a0;
    beta2 = sqrt(beta);
    temp = 1.5 * ck2 * x3thm1 / (beta * beta2);
    for (i = 0; i < 99; ++i)
    {
       a2 = a1;
       d0 = temp / (a0*a0);
       a0 = aodp * (1. - d0);
       d1 = temp / (a1*a1);
       a1 = a0 / (1. - d1 / 3. - d1*d1 - 134. * d1*d1*d1 / 81.);
       if(fabs(a2 - a1) < 1.e-13) break;
    }
    xno = xke * pow(a1 , -1.5);

} /* end rvel  */

// vectors to SDP4 mean elements
void Satellite :: rv2el(double* rr, double* vv)
{
   double ik, ok, ek, wk, mk, nk;
   double iz, oz, ez, wz, mz, nz;
   double rr1[3], vv1[3];

   rvel(rr, vv);              // SGP4 elements

   if(xno / nocon < 6.4)
   {
      rr1[0] = rr[0];
      rr1[1] = rr[1];
      rr1[2] = rr[2];
      vv1[0] = vv[0];
      vv1[1] = vv[1];
      vv1[2] = vv[2];

      ik = xincl,
      ok = xnodeo,
      ek = eo,
      wk = omegao,
      mk = xmo,
      nk = xno;

      sdp4(0.0);              // SDP4 propagation to rr, vv
      rvel(rr, vv);           // SDP4 elements first correction

      xincl  = 2*ik - xincl;
      xnodeo = 2*ok - xnodeo;
      eo     = 2*ek - eo;
      omegao = 2*wk - omegao;
      xmo    = 2*mk - xmo;
      xno    = 2*nk - xno;

      // Loop ---->
      for(int i = 0; i < 2; i++)
      {
        // These elements are pretty close.  Save these
        // as the "pretty close" reference elements
        iz = xincl,
        oz = xnodeo,
        ez = eo,
        wz = omegao,
        mz = xmo,
        nz = xno;

        sdp4(0.0);              // SDP4 propagation to rr, vv
        rvel(rr, vv);           // SDP4 elements second correction

        // Correct the "pretty close" reference elements
        xincl  = iz - (xincl - ik);
        if(xincl < 0.) xincl  *= -1.;
        xnodeo = oz - (xnodeo - ok);
        eo     = ez - (eo - ek);
        omegao = wz - (omegao - wk);
        xmo    = mz - (xmo - mk);
        xno    = nz - (xno - nk);

      } //  <---- end Loop

      xincl  = fmod2p(xincl);
      xnodeo = fmod2p(xnodeo);
      omegao = fmod2p(omegao);
      xmo    = fmod2p(xmo);

      rr[0] = rr1[0];
      rr[1] = rr1[1];
      rr[2] = rr1[2];
      vv[0] = vv1[0];
      vv[1] = vv1[1];
      vv[2] = vv1[2];
   }
} /* end rv2el  */


  double acose(double x)
  {
     double rval;

     if( x >= 1.) rval = 0.;
     else if( x <= -1.) rval = pi;
     else rval = acos( x);
     return( rval);
  }

  //   v1 . v2
  double dot(double* v1, double* v2)
  {
    double sum = 0;
    for (int i = 0; i < 3; i++)
       sum += v1[i] * v2[i];
    return sum;
  }

  //   ||v||
  double norm(double* v)
  {
    return sqrt(dot(v, v));
  }

  //  a * v = av
  void smult(double a, double* v, double* av)
  {
    for (int i = 0; i < 3; i++)
      av[i] = a * v[i];
  }

  //   v1 + v2 = s
  void vadd(double* v1, double* v2, double* s)
  {
    for (int i = 0; i < 3; i++)
      s[i] = v1[i] + v2[i];
  }

  //   v1 x v2 = b
  void cross(double v1[3], double v2[3], double b[3])
  {
    b[0] = v1[1] * v2[2] - v1[2] * v2[1];
    b[1] = v1[2] * v2[0] - v1[0] * v2[2];
    b[2] = v1[0] * v2[1] - v1[1] * v2[0];
  }

  //   u = v / ||v||
  void unitv(double v[], double u[])
  {
    double no = norm(v);
    for (int i = 0; i < 3; i++)
      u[i] = v[i] / no;
  }

  double fmod2p( const double x)
  {
     double twopi = 2 * 3.14159265358979323846;
     double rval = fmod( x, twopi);

     if(rval < 0.)
        rval += twopi;
     return(rval);
  }