YodaProfiler/src/sgp4.cpp

//
// globals.cpp
//
#include <sgp4.h>
#include <cstring>

//////////////////////////////////////////////////////////////////////////////
double sqr(const double x) 
{
  return (x * x);
}

//////////////////////////////////////////////////////////////////////////////
double Fmod2p(const double arg)
{
  double modu = fmod(arg, TWOPI);

  if (modu < 0.0)
    modu += TWOPI;

  return modu;
}

//////////////////////////////////////////////////////////////////////////////
// AcTan()
// ArcTangent of sin(x) / cos(x). The advantage of this function over arctan()
// is that it returns the correct quadrant of the angle.
double AcTan(const double sinx, const double cosx)
{
  double ret;

  if (cosx == 0.0)
    {
      if (sinx > 0.0)
        ret = PI / 2.0;
      else
        ret = 3.0 * PI / 2.0;
    }
  else
    {
      if (cosx > 0.0)
        ret = atan(sinx / cosx);
      else
        ret = PI + atan(sinx / cosx);
    }

  return ret;
}

//////////////////////////////////////////////////////////////////////////////
double rad2deg(const double r)
{
  const double DEG_PER_RAD = 180.0 / PI;
  return r * DEG_PER_RAD;
}

//////////////////////////////////////////////////////////////////////////////
double deg2rad(const double d)
{
  const double RAD_PER_DEG = PI / 180.0;
  return d * RAD_PER_DEG;
}

//
// coord.cpp
//
// Copyright (c) 2003 Michael F. Henry
//

//////////////////////////////////////////////////////////////////////
// cCoordGeo Class
//////////////////////////////////////////////////////////////////////

cCoordGeo::cCoordGeo()
{
  m_Lat = 0.0;
  m_Lon = 0.0;
  m_Alt = 0.0;
}

//////////////////////////////////////////////////////////////////////
// cCoordTopo Class
//////////////////////////////////////////////////////////////////////

cCoordTopo::cCoordTopo()
{
  m_Az = 0.0;
  m_El = 0.0;       
  m_Range = 0.0;    
  m_RangeRate = 0.0;

}


//
// cVector.cpp
//
// Copyright (c) 2001-2003 Michael F. Henry
//
//*****************************************************************************
// Multiply each component in the vector by 'factor'.
//*****************************************************************************
void cVector::Mul(double factor)
{
  m_x *= factor;
  m_y *= factor;
  m_z *= factor;
  m_w *= fabs(factor);
}

//*****************************************************************************
// Subtract a vector from this one.
//*****************************************************************************
void cVector::Sub(const cVector& vec)
{
  m_x -= vec.m_x;
  m_y -= vec.m_y;
  m_z -= vec.m_z;
  m_w -= vec.m_w;
}

//*****************************************************************************
// Calculate the angle between this vector and another
//*****************************************************************************
double cVector::Angle(const cVector& vec) const
{
  return acos(Dot(vec) / (Magnitude() * vec.Magnitude()));
}

//*****************************************************************************
//
//*****************************************************************************
double cVector::Magnitude() const
{
  return sqrt((m_x * m_x) + 
              (m_y * m_y) + 
              (m_z * m_z));
}

//*****************************************************************************
// Return the dot product
//*****************************************************************************
double cVector::Dot(const cVector& vec) const
{
  return (m_x * vec.m_x) +
    (m_y * vec.m_y) +
    (m_z * vec.m_z);
}
//
// cJulian.cpp
//
// This class encapsulates Julian dates with the epoch of 12:00 noon (12:00 UT)
// on January 1, 4713 B.C. Some epoch dates:
//    01/01/1990 00:00 UTC - 2447892.5
//    01/01/1990 12:00 UTC - 2447893.0
//    01/01/2000 00:00 UTC - 2451544.5
//    01/01/2001 00:00 UTC - 2451910.5
//
// Note the Julian day begins at noon, which allows astronomers to have all
// the dates in a single observing session the same.
//
// References:
// "Astronomical Formulae for Calculators", Jean Meeus
// "Satellite Communications", Dennis Roddy, 2nd Edition, 1995.
//
// Copyright (c) 2003 Michael F. Henry
//
// mfh 12/24/2003
//

//////////////////////////////////////////////////////////////////////////////
// Create a Julian date object from a time_t object. time_t objects store the
// number of seconds since midnight UTC January 1, 1970.
cJulian::cJulian(time_t time)
{
  struct tm *ptm = gmtime(&time);
  assert(ptm);

  int    year = ptm->tm_year + 1900;
  double day  = ptm->tm_yday + 1 +
    (ptm->tm_hour + 
     ((ptm->tm_min + 
       (ptm->tm_sec / 60.0)) / 60.0)) / 24.0;

  Initialize(year, day);
}

//////////////////////////////////////////////////////////////////////////////
// Create a Julian date object from a year and day of year.
// Example parameters: year = 2001, day = 1.5 (Jan 1 12h)
cJulian::cJulian(int year, double day)
{
  Initialize(year, day);
}

//////////////////////////////////////////////////////////////////////////////
// Create a Julian date object.
cJulian::cJulian(int year,               // i.e., 2004
                 int mon,                // 1..12
                 int day,                // 1..31
                 int hour,               // 0..23
                 int min,                // 0..59
                 double sec /* = 0.0 */) // 0..(59.999999...)

{
  // Calculate N, the day of the year (1..366)
  int N;
  int F1 = (int)((275.0 * mon) / 9.0);
  int F2 = (int)((mon + 9.0) / 12.0);

  if (IsLeapYear(year))
    {
      // Leap year
      N = F1 - F2 + day - 30;
    }
  else
    {
      // Common year
      N = F1 - (2 * F2) + day - 30;
    }
   
  double dblDay = N + (hour + (min + (sec / 60.0)) / 60.0) / 24.0;

  Initialize(year, dblDay);
}

//////////////////////////////////////////////////////////////////////////////
void cJulian::Initialize(int year, double day)
{
  // 1582 A.D.: 10 days removed from calendar
  // 3000 A.D.: Arbitrary error checking limit
  assert((year > 1582) && (year < 3000));
  assert((day >= 0.0) && (day <= 366.90)); // 366.5 // 366.7 // 366.81

  // Now calculate Julian date

  year--;

  // Centuries are not leap years unless they divide by 400
  int A = (year / 100);
  int B = 2 - A + (A / 4);

  double NewYears = (int)(365.25 * year) +
    (int)(30.6001 * 14)  + 
    1720994.5 + B;  // 1720994.5 = Oct 30, year -1

  m_Date = NewYears + day;
}

//////////////////////////////////////////////////////////////////////////////
// getComponent()
// Return requested components of date.
// Year : Includes the century.
// Month: 1..12
// Day  : 1..31 including fractional part
void cJulian::getComponent(int    *pYear, 
                           int    *pMon  /* = NULL */,
                           double *pDOM  /* = NULL */) const
{
  assert(pYear != NULL);

  double jdAdj = getDate() + 0.5;
  int    Z     = (int)jdAdj;  // integer part
  double F     = jdAdj - Z;   // fractional part
  double alpha = (int)((Z - 1867216.25) / 36524.25);
  double A     = Z + 1 + alpha - (int)(alpha / 4.0);
  double B     = A + 1524.0;
  int    C     = (int)((B - 122.1) / 365.25);
  int    D     = (int)(C * 365.25);
  int    E     = (int)((B - D) / 30.6001);

  double DOM   = B - D - (int)(E * 30.6001) + F;
  int    month = (E < 13.5) ? (E - 1) : (E - 13);
  int    year  = (month > 2.5) ? (C - 4716) : (C - 4715); 
   
  *pYear = year;

  if (pMon != NULL)
    *pMon = month;

  if (pDOM != NULL)
    *pDOM = DOM;
} 

//////////////////////////////////////////////////////////////////////////////
// toGMST()
// Calculate Greenwich Mean Sidereal Time for the Julian date. The return value
// is the angle, in radians, measuring eastward from the Vernal Equinox to the
// prime meridian. This angle is also referred to as "ThetaG" (Theta GMST).
// 
// References:
//    The 1992 Astronomical Almanac, page B6.
//    Explanatory Supplement to the Astronomical Almanac, page 50.
//    Orbital Coordinate Systems, Part III, Dr. T.S. Kelso, Satellite Times,
//       Nov/Dec 1995
double cJulian::toGMST() const
{
  const double UT = fmod(m_Date + 0.5, 1.0);
  const double TU = (FromJan1_12h_2000() - UT) / 36525.0;

  double GMST = 24110.54841 + TU * 
    (8640184.812866 + TU * (0.093104 - TU * 6.2e-06));

  GMST = fmod(GMST + SEC_PER_DAY * OMEGA_E * UT, SEC_PER_DAY);
   
  if (GMST < 0.0)
    GMST += SEC_PER_DAY;  // "wrap" negative modulo value

  return  (TWOPI * (GMST / SEC_PER_DAY));
}

//////////////////////////////////////////////////////////////////////////////
// toLMST()
// Calculate Local Mean Sidereal Time for given longitude (for this date).
// The longitude is assumed to be in radians measured west from Greenwich.
// The return value is the angle, in radians, measuring eastward from the
// Vernal Equinox to the given longitude.
double cJulian::toLMST(double lon) const
{
  return fmod(toGMST() + lon, TWOPI);
}

//////////////////////////////////////////////////////////////////////////////
// toTime()
// Convert to type time_t
// Avoid using this function as it discards the fractional seconds of the
// time component.
time_t cJulian::toTime() const
{
  int    nYear;
  int    nMonth;
  double dblDay;

  getComponent(&nYear, &nMonth, &dblDay);

  // dblDay is the fractional Julian Day (i.e., 29.5577).
  // Save the whole number day in nDOM and convert dblDay to
  // the fractional portion of day.
  int nDOM = (int)dblDay;

  dblDay -= nDOM;

  const int SEC_PER_MIN = 60;
  const int SEC_PER_HR  = 60 * SEC_PER_MIN;
  const int SEC_PER_DAY = 24 * SEC_PER_HR;

  int secs = (int)((dblDay * SEC_PER_DAY) + 0.5);

  // Create a "struct tm" type. 
  // NOTE:
  // The "struct tm" type has a 1-second resolution. Any fractional
  // component of the "seconds" time value is discarded.
  struct tm tGMT;
  memset(&tGMT, 0, sizeof(tGMT));

  tGMT.tm_year = nYear - 1900;  // 2001 is 101
  tGMT.tm_mon  = nMonth - 1;    // January is 0
  tGMT.tm_mday = nDOM;          // First day is 1
  tGMT.tm_hour = secs / SEC_PER_HR;
  tGMT.tm_min  = (secs % SEC_PER_HR) / SEC_PER_MIN;
  tGMT.tm_sec  = (secs % SEC_PER_HR) % SEC_PER_MIN;
  tGMT.tm_isdst = 0; // No conversion desired

  time_t tEpoch = mktime(&tGMT);

  if (tEpoch != -1)
    {
      // Valid time_t value returned from mktime().
      // mktime() expects a local time which means that tEpoch now needs 
      // to be adjusted by the difference between this time zone and GMT.
      tEpoch -= timezone;
    }

  return tEpoch;
}
//
// cTle.cpp
// This class encapsulates a single set of standard NORAD two line elements.
//
// Copyright 1996-2005 Michael F. Henry
//
// Note: The column offsets are ZERO based.

// Name
const int TLE_LEN_LINE_DATA      = 69; const int TLE_LEN_LINE_NAME      = 22;

// Line 1
const int TLE1_COL_SATNUM        =  2; const int TLE1_LEN_SATNUM        =  5;
const int TLE1_COL_INTLDESC_A    =  9; const int TLE1_LEN_INTLDESC_A    =  2;
const int TLE1_COL_INTLDESC_B    = 11; const int TLE1_LEN_INTLDESC_B    =  3;
const int TLE1_COL_INTLDESC_C    = 14; const int TLE1_LEN_INTLDESC_C    =  3;
const int TLE1_COL_EPOCH_A       = 18; const int TLE1_LEN_EPOCH_A       =  2;
const int TLE1_COL_EPOCH_B       = 20; const int TLE1_LEN_EPOCH_B       = 12;
const int TLE1_COL_MEANMOTIONDT  = 33; const int TLE1_LEN_MEANMOTIONDT  = 10;
const int TLE1_COL_MEANMOTIONDT2 = 44; const int TLE1_LEN_MEANMOTIONDT2 =  8;
const int TLE1_COL_BSTAR         = 53; const int TLE1_LEN_BSTAR         =  8;
const int TLE1_COL_EPHEMTYPE     = 62; const int TLE1_LEN_EPHEMTYPE     =  1;
const int TLE1_COL_ELNUM         = 64; const int TLE1_LEN_ELNUM         =  4;

// Line 2
const int TLE2_COL_SATNUM        = 2;  const int TLE2_LEN_SATNUM        =  5;
const int TLE2_COL_INCLINATION   = 8;  const int TLE2_LEN_INCLINATION   =  8;
const int TLE2_COL_RAASCENDNODE  = 17; const int TLE2_LEN_RAASCENDNODE  =  8;
const int TLE2_COL_ECCENTRICITY  = 26; const int TLE2_LEN_ECCENTRICITY  =  7;
const int TLE2_COL_ARGPERIGEE    = 34; const int TLE2_LEN_ARGPERIGEE    =  8;
const int TLE2_COL_MEANANOMALY   = 43; const int TLE2_LEN_MEANANOMALY   =  8;
const int TLE2_COL_MEANMOTION    = 52; const int TLE2_LEN_MEANMOTION    = 11;
const int TLE2_COL_REVATEPOCH    = 63; const int TLE2_LEN_REVATEPOCH    =  5;

/////////////////////////////////////////////////////////////////////////////
cTle::cTle(string& strName, string& strLine1, string& strLine2)
{
  m_strName  = strName;
  m_strLine1 = strLine1;
  m_strLine2 = strLine2;

  Initialize();
}

/////////////////////////////////////////////////////////////////////////////
cTle::cTle(const cTle &tle)
{
  m_strName  = tle.m_strName;
  m_strLine1 = tle.m_strLine1;
  m_strLine2 = tle.m_strLine2;

  for (int fld = FLD_FIRST; fld < FLD_LAST; fld++)
    {
      m_Field[fld] = tle.m_Field[fld];
    }

  m_mapCache = tle.m_mapCache;
}

/////////////////////////////////////////////////////////////////////////////
cTle::~cTle()
{
}

/////////////////////////////////////////////////////////////////////////////
// getField()
// Return requested field as a double (function return value) or as a text
// string (*pstr) in the units requested (eUnit). Set 'bStrUnits' to true 
// to have units appended to text string.
//
// Note: numeric return values are cached; asking for the same field more
// than once incurs minimal overhead.
double cTle::getField(eField   fld, 
                      eUnits   units,    /* = U_NATIVE */
                      string  *pstr      /* = NULL     */,
                      bool     bStrUnits /* = false    */) const
{
  assert((FLD_FIRST <= fld) && (fld < FLD_LAST));
  assert((U_FIRST <= units) && (units < U_LAST));

  if (pstr)
    {
      // Return requested field in string form.
      *pstr = m_Field[fld];
      
      if (bStrUnits)
        *pstr += getUnits(fld);
      
      return 0.0;
    }
  else
    {
      // Return requested field in floating-point form.
      // Return cache contents if it exists, else populate cache
      FldKey key = Key(units, fld);

      if (m_mapCache.find(key) == m_mapCache.end())
        {
          // Value not in cache; add it
          double valNative = atof(m_Field[fld].c_str());
          double valConv   = ConvertUnits(valNative, fld, units); 
          m_mapCache[key]  = valConv;

          return valConv;
        }
      else
        {
          // return cached value
          return m_mapCache[key];
        }
    }
}

//////////////////////////////////////////////////////////////////////////////
// Convert the given field into the requested units. It is assumed that
// the value being converted is in the TLE format's "native" form.
double cTle::ConvertUnits(double valNative, // value to convert
                          eField fld,       // what field the value is
                          eUnits units)     // what units to convert to
{
  switch (fld)
    {
    case FLD_I:
    case FLD_RAAN:
    case FLD_ARGPER:
    case FLD_M:
      {
        // The native TLE format is DEGREES
        if (units == U_RAD)
          return valNative * RADS_PER_DEG;
      }
     
    case FLD_NORADNUM:
    case FLD_INTLDESC:
    case FLD_SET:
    case FLD_EPOCHYEAR:
    case FLD_EPOCHDAY:
    case FLD_ORBITNUM:
    case FLD_E:
    case FLD_MMOTION:
    case FLD_MMOTIONDT:
    case FLD_MMOTIONDT2:
    case FLD_BSTAR:
    case FLD_LAST:
      { // do nothing

      }

    }

  return valNative; // return value in unconverted native format
}

//////////////////////////////////////////////////////////////////////////////
string cTle::getUnits(eField fld) const 
{
  static const string strDegrees    = " degrees";
  static const string strRevsPerDay = " revs / day";
  static const string strNull;

  switch (fld)
    {
    case FLD_I:
    case FLD_RAAN:
    case FLD_ARGPER:
    case FLD_M:
      return strDegrees;

    case FLD_MMOTION:
      return strRevsPerDay;

    default:
      return strNull;   
    }
}

/////////////////////////////////////////////////////////////////////////////
// ExpToDecimal()
// Converts TLE-style exponential notation of the form [ |-]00000[+|-]0 to
// decimal notation. Assumes implied decimal point to the left of the first
// number in the string, i.e., 
//       " 12345-3" =  0.00012345
//       "-23429-5" = -0.0000023429   
//       " 40436+1" =  4.0436
string cTle::ExpToDecimal(const string &str)
{
  const int COL_EXP_SIGN = 6;
  const int LEN_EXP      = 2;
   
  const int LEN_BUFREAL  = 32;   // max length of buffer to hold floating point
                                 // representation of input string.
  int nMan;
  int nExp;      
   
  // sscanf(%d) will read up to the exponent sign
  sscanf(str.c_str(), "%d", &nMan);
   
  double dblMan = nMan;
  bool  bNeg    = (nMan < 0);
   
  if (bNeg)
    dblMan *= -1;
   
  // Move decimal place to left of first digit
  while (dblMan >= 1.0)
    dblMan /= 10.0;
   
  if (bNeg)
    dblMan *= -1;
   
  // now read exponent
  sscanf(str.substr(COL_EXP_SIGN, LEN_EXP).c_str(), "%d", &nExp);
   
  double dblVal = dblMan * pow(10.0, nExp);
  char szVal[LEN_BUFREAL];
   
  snprintf(szVal, sizeof(szVal), "%.9f", dblVal);
   
  string strVal = szVal;
   
  return strVal;
   
} // ExpToDecimal()

/////////////////////////////////////////////////////////////////////////////
// Initialize()
// Initialize the string array.
void cTle::Initialize()
{
  // Have we already been initialized?
  if (m_Field[FLD_NORADNUM].size())
    return;
   
  assert(!m_strName.empty());
  assert(!m_strLine1.empty());
  assert(!m_strLine2.empty());
   
  m_Field[FLD_NORADNUM] = m_strLine1.substr(TLE1_COL_SATNUM, TLE1_LEN_SATNUM);
  m_Field[FLD_INTLDESC] = m_strLine1.substr(TLE1_COL_INTLDESC_A,
                                            TLE1_LEN_INTLDESC_A +
                                            TLE1_LEN_INTLDESC_B +   
                                            TLE1_LEN_INTLDESC_C);   
  m_Field[FLD_EPOCHYEAR] = 
    m_strLine1.substr(TLE1_COL_EPOCH_A, TLE1_LEN_EPOCH_A);

  m_Field[FLD_EPOCHDAY] = 
    m_strLine1.substr(TLE1_COL_EPOCH_B, TLE1_LEN_EPOCH_B);
   
  if (m_strLine1[TLE1_COL_MEANMOTIONDT] == '-')
    {
      // value is negative
      m_Field[FLD_MMOTIONDT] = "-0";
    }
  else
    m_Field[FLD_MMOTIONDT] = "0";
   
  m_Field[FLD_MMOTIONDT] += m_strLine1.substr(TLE1_COL_MEANMOTIONDT + 1,
                                              TLE1_LEN_MEANMOTIONDT);
   
  // decimal point assumed; exponential notation
  m_Field[FLD_MMOTIONDT2] = ExpToDecimal(
                                         m_strLine1.substr(TLE1_COL_MEANMOTIONDT2,
                                                           TLE1_LEN_MEANMOTIONDT2));
  // decimal point assumed; exponential notation
  m_Field[FLD_BSTAR] = ExpToDecimal(m_strLine1.substr(TLE1_COL_BSTAR,
                                                      TLE1_LEN_BSTAR));
  //TLE1_COL_EPHEMTYPE      
  //TLE1_LEN_EPHEMTYPE   
  m_Field[FLD_SET] = m_strLine1.substr(TLE1_COL_ELNUM, TLE1_LEN_ELNUM);
   
  TrimLeft(m_Field[FLD_SET]);
   
  //TLE2_COL_SATNUM         
  //TLE2_LEN_SATNUM         
   
  m_Field[FLD_I] = m_strLine2.substr(TLE2_COL_INCLINATION,
                                     TLE2_LEN_INCLINATION);
  TrimLeft(m_Field[FLD_I]);
   
  m_Field[FLD_RAAN] = m_strLine2.substr(TLE2_COL_RAASCENDNODE,
                                        TLE2_LEN_RAASCENDNODE);
  TrimLeft(m_Field[FLD_RAAN]);

  // decimal point is assumed
  m_Field[FLD_E]   = "0.";
  m_Field[FLD_E]   += m_strLine2.substr(TLE2_COL_ECCENTRICITY,
                                        TLE2_LEN_ECCENTRICITY);
   
  m_Field[FLD_ARGPER] = m_strLine2.substr(TLE2_COL_ARGPERIGEE,
                                          TLE2_LEN_ARGPERIGEE);
  TrimLeft(m_Field[FLD_ARGPER]);
   
  m_Field[FLD_M]   = m_strLine2.substr(TLE2_COL_MEANANOMALY,
                                       TLE2_LEN_MEANANOMALY);
  TrimLeft(m_Field[FLD_M]);
   
  m_Field[FLD_MMOTION]   = m_strLine2.substr(TLE2_COL_MEANMOTION, 
                                             TLE2_LEN_MEANMOTION);
  TrimLeft(m_Field[FLD_MMOTION]);
   
  m_Field[FLD_ORBITNUM] = m_strLine2.substr(TLE2_COL_REVATEPOCH,
                                            TLE2_LEN_REVATEPOCH);
  TrimLeft(m_Field[FLD_ORBITNUM]);
   
} // InitStrVars()

/////////////////////////////////////////////////////////////////////////////
// IsTleFormat()
// Returns true if "str" is a valid data line of a two-line element set,
//   else false.
//
// To be valid a line must:
//      Have as the first character the line number
//      Have as the second character a blank
//      Be TLE_LEN_LINE_DATA characters long
//      Have a valid checksum (note: no longer required as of 12/96)
//      
bool cTle::IsValidLine(string& str, eTleLine line)
{
  TrimLeft(str);
  TrimRight(str);
   
  size_t nLen = str.size();
   
  if (nLen != (uint)TLE_LEN_LINE_DATA)
    return false;
   
  // First char in string must be line number
  if ((str[0] - '0') != line)
    return false;
   
  // Second char in string must be blank
  if (str[1] != ' ')
    return false;
   
  /*
    NOTE: 12/96 
    The requirement that the last char in the line data must be a valid 
    checksum is too restrictive. 
      
    // Last char in string must be checksum
    int nSum = CheckSum(str);
     
    if (nSum != (str[TLE_LEN_LINE_DATA - 1] - '0'))
    return false;
  */
   
  return true;
   
} // IsTleFormat()

/////////////////////////////////////////////////////////////////////////////
// CheckSum()
// Calculate the check sum for a given line of TLE data, the last character
// of which is the current checksum. (Although there is no check here,
// the current checksum should match the one we calculate.)
// The checksum algorithm: 
//      Each number in the data line is summed, modulo 10.
//    Non-numeric characters are zero, except minus signs, which are 1.
//
int cTle::CheckSum(const string& str)
{
  // The length is "- 1" because we don't include the current (existing)
  // checksum character in the checksum calculation.
  size_t len = str.size() - 1;
  int xsum = 0;
   
  for (size_t i = 0; i < len; i++)
    {
      char ch = str[i];
      if (isdigit(ch))
        xsum += (ch - '0');
      else if (ch == '-')
        xsum++;
    }
   
  return (xsum % 10);
   
} // CheckSum()

/////////////////////////////////////////////////////////////////////////////
void cTle::TrimLeft(string& s)
{
  while (s[0] == ' ')
    s.erase(0, 1);
}

/////////////////////////////////////////////////////////////////////////////
void cTle::TrimRight(string& s)
{
  while (s[s.size() - 1] == ' ')
    s.erase(s.size() - 1);
}

//
// cEci.cpp
//
// Copyright (c) 2002-2003 Michael F. Henry
//
//////////////////////////////////////////////////////////////////////
// cEci Class
//////////////////////////////////////////////////////////////////////
cEci::cEci(const cVector &pos, 
           const cVector &vel, 
           const cJulian &date,
           bool  IsAeUnits /* = true */)
{
  m_pos      = pos;
  m_vel      = vel;
  m_date     = date;
  m_VecUnits = (IsAeUnits ? UNITS_AE : UNITS_NONE);
}

//////////////////////////////////////////////////////////////////////
// cEci(cCoordGeo&, cJulian&)
// Calculate the ECI coordinates of the location "geo" at time "date".
// Assumes geo coordinates are km-based.
// Assumes the earth is an oblate spheroid as defined in WGS '72.
// Reference: The 1992 Astronomical Almanac, page K11
// Reference: www.celestrak.com (Dr. TS Kelso)
cEci::cEci(const cCoordGeo &geo, const cJulian &date)
{
  m_VecUnits = UNITS_KM;

  double mfactor = TWOPI * (OMEGA_E / SEC_PER_DAY);
  double lat = geo.m_Lat;
  double lon = geo.m_Lon;
  double alt = geo.m_Alt;

  // Calculate Local Mean Sidereal Time (theta)
  double theta = date.toLMST(lon);
  double c = 1.0 / sqrt(1.0 + F * (F - 2.0) * sqr(sin(lat)));
  double s = sqr(1.0 - F) * c;
  double achcp = (XKMPER_WGS72 * c + alt) * cos(lat);

  m_date = date;

  m_pos.m_x = achcp * cos(theta);               // km
  m_pos.m_y = achcp * sin(theta);               // km
  m_pos.m_z = (XKMPER_WGS72 * s + alt) * sin(lat);   // km
  m_pos.m_w = sqrt(sqr(m_pos.m_x) + 
                   sqr(m_pos.m_y) + 
                   sqr(m_pos.m_z));            // range, km

  m_vel.m_x = -mfactor * m_pos.m_y;            // km / sec
  m_vel.m_y =  mfactor * m_pos.m_x;
  m_vel.m_z = 0.0;
  m_vel.m_w = sqrt(sqr(m_vel.m_x) +            // range rate km/sec^2
                   sqr(m_vel.m_y));
}

//////////////////////////////////////////////////////////////////////////////
// toGeo()
// Return the corresponding geodetic position (based on the current ECI
// coordinates/Julian date).
// Assumes the earth is an oblate spheroid as defined in WGS '72.
// Side effects: Converts the position and velocity vectors to km-based units.
// Reference: The 1992 Astronomical Almanac, page K12. 
// Reference: www.celestrak.com (Dr. TS Kelso)
cCoordGeo cEci::toGeo()
{
  ae2km(); // Vectors must be in kilometer-based units

  double theta = AcTan(m_pos.m_y, m_pos.m_x);
  double lon   = fmod(theta - m_date.toGMST(), TWOPI);
   
  if (lon < 0.0) 
    lon += TWOPI;  // "wrap" negative modulo

  double r   = sqrt(sqr(m_pos.m_x) + sqr(m_pos.m_y));
  double e2  = F * (2.0 - F);
  double lat = AcTan(m_pos.m_z, r);

  const double delta = 1.0e-07;
  double phi;
  double c;

  do   
    {
      phi = lat;
      c   = 1.0 / sqrt(1.0 - e2 * sqr(sin(phi)));
      lat = AcTan(m_pos.m_z + XKMPER_WGS72 * c * e2 * sin(phi), r);
    }
  while (fabs(lat - phi) > delta);
   
  double alt = r / cos(lat) - XKMPER_WGS72 * c;

  return cCoordGeo(lat, lon, alt); // radians, radians, kilometers
}

//////////////////////////////////////////////////////////////////////////////
// ae2km()
// Convert the position and velocity vector units from AE-based units
// to kilometer based units.
void cEci::ae2km()
{
  if (UnitsAreAe())
    {
      MulPos(XKMPER_WGS72 / AE);                       // km
      MulVel((XKMPER_WGS72 / AE) * (MIN_PER_DAY / 86400));  // km/sec
      m_VecUnits = UNITS_KM;
    }
}
//
// cNoradBase.cpp
//
// Historical Note:
// The equations used here (and in derived classes) to determine satellite 
// ECI coordinates/velocity come from the December, 1980 NORAD document 
// "Space Track Report No. 3". The report details 6 orbital models and 
// provides FORTRAN IV implementations of each. The classes here 
// implement only two of the orbital models: SGP4 and SDP4. These two models, 
// one for "near-earth" objects and one for "deep space" objects, are widely
// used in satellite tracking software and can produce very accurate results
// when used with current NORAD two-line element datum.
//
// The NORAD FORTRAN IV SGP4/SDP4 implementations were converted to Pascal by
// Dr. TS Kelso in 1995. In 1996 these routines were ported in a straight-
// forward manner to C++ by Varol Okan. The SGP4/SDP4 classes here were 
// written by Michael F. Henry in 2002-03 and are a modern C++ re-write of 
// the work done by Okan. In addition to introducing an object-oriented 
// architecture, the last residues of the original FORTRAN code (such as
// labels and gotos) were eradicated.
//
// For excellent information on the underlying physics of orbits, visible 
// satellite observations, current NORAD TLE data, and other related material,
// see http://www.celestrak.com which is maintained by Dr. TS Kelso.
//
// Copyright (c) 2003 Michael F. Henry
//
// mfh 12/07/2003
//
//////////////////////////////////////////////////////////////////////////////
cNoradBase::cNoradBase(const cOrbit &orbit) :
  m_Orbit(orbit)
{
  Initialize();
}

cNoradBase& cNoradBase::operator=(const cNoradBase &b)
{
  // m_Orbit is a "const" member var, so cast away its
  // "const-ness" in order to complete the assigment.
  *(const_cast<cOrbit*>(&m_Orbit)) = b.m_Orbit;

  return *this;
}

//////////////////////////////////////////////////////////////////////////////
// Initialize()
// Perform the initialization of member variables, specifically the variables
// used by derived-class objects to calculate ECI coordinates.
void cNoradBase::Initialize()
{
  // Initialize any variables which are time-independent when
  // calculating the ECI coordinates of the satellite.
  m_satInc = m_Orbit.Inclination();
  m_satEcc = m_Orbit.Eccentricity();

  m_cosio  = cos(m_satInc);
  m_theta2 = m_cosio * m_cosio;
  m_x3thm1 = 3.0 * m_theta2 - 1.0;
  m_eosq   = m_satEcc * m_satEcc;
  m_betao2 = 1.0 - m_eosq;
  m_betao  = sqrt(m_betao2);

  // The "recovered" semi-minor axis and mean motion.
  m_aodp  = m_Orbit.SemiMinor();   
  m_xnodp = m_Orbit.mnMotionRec();

  // For perigee below 156 km, the values of S and QOMS2T are altered.
  m_perigee = XKMPER_WGS72 * (m_aodp * (1.0 - m_satEcc) - AE);

  m_s4      = S;
  m_qoms24  = QOMS2T;

  if (m_perigee < 156.0)
    {
      m_s4 = m_perigee - 78.0;

      if (m_perigee <= 98.0)
        {
          m_s4 = 20.0;
        }

      m_qoms24 = pow((120.0 - m_s4) * AE / XKMPER_WGS72, 4.0);
      m_s4 = m_s4 / XKMPER_WGS72 + AE;
    }

  const double pinvsq = 1.0 / (m_aodp * m_aodp * m_betao2 * m_betao2);

  m_tsi   = 1.0 / (m_aodp - m_s4);
  m_eta   = m_aodp * m_satEcc * m_tsi;
  m_etasq = m_eta * m_eta;
  m_eeta  = m_satEcc * m_eta;

  const double psisq  = fabs(1.0 - m_etasq);

  m_coef  = m_qoms24 * pow(m_tsi,4.0);
  m_coef1 = m_coef / pow(psisq,3.5);

  const double c2 = m_coef1 * m_xnodp * 
    (m_aodp * (1.0 + 1.5 * m_etasq + m_eeta * (4.0 + m_etasq)) +
     0.75 * CK2 * m_tsi / psisq * m_x3thm1 * 
     (8.0 + 3.0 * m_etasq * (8.0 + m_etasq)));

  m_c1    = m_Orbit.BStar() * c2;
  m_sinio = sin(m_satInc);

  const double a3ovk2 = -XJ3 / CK2 * pow(AE,3.0);

  m_c3     = m_coef * m_tsi * a3ovk2 * m_xnodp * AE * m_sinio / m_satEcc;
  m_x1mth2 = 1.0 - m_theta2;
  m_c4     = 2.0 * m_xnodp * m_coef1 * m_aodp * m_betao2 * 
    (m_eta * (2.0 + 0.5 * m_etasq) +
     m_satEcc * (0.5 + 2.0 * m_etasq) - 
     2.0 * CK2 * m_tsi / (m_aodp * psisq) *
     (-3.0 * m_x3thm1 * (1.0 - 2.0 * m_eeta + m_etasq * (1.5 - 0.5 * m_eeta)) +
      0.75 * m_x1mth2 * 
      (2.0 * m_etasq - m_eeta * (1.0 + m_etasq)) * 
      cos(2.0 * m_Orbit.ArgPerigee())));

  const double theta4 = m_theta2 * m_theta2;
  const double temp1  = 3.0 * CK2 * pinvsq * m_xnodp;
  const double temp2  = temp1 * CK2 * pinvsq;
  const double temp3  = 1.25 * CK4 * pinvsq * pinvsq * m_xnodp;

  m_xmdot = m_xnodp + 0.5 * temp1 * m_betao * m_x3thm1 +
    0.0625 * temp2 * m_betao * 
    (13.0 - 78.0 * m_theta2 + 137.0 * theta4);

  const double x1m5th = 1.0 - 5.0 * m_theta2;

  m_omgdot = -0.5 * temp1 * x1m5th + 0.0625 * temp2 * 
    (7.0 - 114.0 * m_theta2 + 395.0 * theta4) +
    temp3 * (3.0 - 36.0 * m_theta2 + 49.0 * theta4);

  const double xhdot1 = -temp1 * m_cosio;

  m_xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * m_theta2) +
                       2.0 * temp3 * (3.0 - 7.0 * m_theta2)) * m_cosio;
  m_xnodcf = 3.5 * m_betao2 * xhdot1 * m_c1;
  m_t2cof  = 1.5 * m_c1;
  m_xlcof  = 0.125 * a3ovk2 * m_sinio * 
    (3.0 + 5.0 * m_cosio) / (1.0 + m_cosio);
  m_aycof  = 0.25 * a3ovk2 * m_sinio;
  m_x7thm1 = 7.0 * m_theta2 - 1.0;
}

//////////////////////////////////////////////////////////////////////////////
bool cNoradBase::FinalPosition(double incl, double  omega, 
                               double    e, double      a,
                               double   xl, double  xnode, 
                               double   xn, double tsince, 
                               cEci &eci)
{
  if ((e * e) > 1.0)
    {
      // error in satellite data
      return false;  
    }

  double beta = sqrt(1.0 - e * e);

  // Long period periodics 
  double axn  = e * cos(omega);
  double temp = 1.0 / (a * beta * beta);
  double xll  = temp * m_xlcof * axn;
  double aynl = temp * m_aycof;
  double xlt  = xl + xll;
  double ayn  = e * sin(omega) + aynl;

  // Solve Kepler's Equation 

  double capu   = Fmod2p(xlt - xnode);
  double temp2  = capu;
  double temp3  = 0.0;
  double temp4  = 0.0;
  double temp5  = 0.0;
  double temp6  = 0.0;
  double sinepw = 0.0;
  double cosepw = 0.0;
  bool   fDone  = false;

  for (int i = 1; (i <= 10) && !fDone; i++)
    {
      sinepw = sin(temp2);
      cosepw = cos(temp2);
      temp3 = axn * sinepw;
      temp4 = ayn * cosepw;
      temp5 = axn * cosepw;
      temp6 = ayn * sinepw;

      double epw = (capu - temp4 + temp3 - temp2) / 
        (1.0 - temp5 - temp6) + temp2;

      if (fabs(epw - temp2) <= E6A)
        fDone = true;
      else
        temp2 = epw;
    }

  // Short period preliminary quantities 
  double ecose = temp5 + temp6;
  double esine = temp3 - temp4;
  double elsq  = axn * axn + ayn * ayn;
  temp  = 1.0 - elsq;
  double pl = a * temp;
  double r  = a * (1.0 - ecose);
  double temp1 = 1.0 / r;
  double rdot  = XKE * sqrt(a) * esine * temp1;
  double rfdot = XKE * sqrt(pl) * temp1;
  temp2 = a * temp1;
  double betal = sqrt(temp);
  temp3 = 1.0 / (1.0 + betal);
  double cosu  = temp2 * (cosepw - axn + ayn * esine * temp3);
  double sinu  = temp2 * (sinepw - ayn - axn * esine * temp3);
  double u     = AcTan(sinu, cosu);
  double sin2u = 2.0 * sinu * cosu;
  double cos2u = 2.0 * cosu * cosu - 1.0;

  temp  = 1.0 / pl;
  temp1 = CK2 * temp;
  temp2 = temp1 * temp;

  // Update for short periodics 
  double rk = r * (1.0 - 1.5 * temp2 * betal * m_x3thm1) + 
    0.5 * temp1 * m_x1mth2 * cos2u;
  double uk = u - 0.25 * temp2 * m_x7thm1 * sin2u;
  double xnodek = xnode + 1.5 * temp2 * m_cosio * sin2u;
  double xinck  = incl + 1.5 * temp2 * m_cosio * m_sinio * cos2u;
  double rdotk  = rdot - xn * temp1 * m_x1mth2 * sin2u;
  double rfdotk = rfdot + xn * temp1 * (m_x1mth2 * cos2u + 1.5 * m_x3thm1);

  // Orientation vectors 
  double sinuk  = sin(uk);
  double cosuk  = cos(uk);
  double sinik  = sin(xinck);
  double cosik  = cos(xinck);
  double sinnok = sin(xnodek);
  double cosnok = cos(xnodek);
  double xmx = -sinnok * cosik;
  double xmy = cosnok * cosik;
  double ux  = xmx * sinuk + cosnok * cosuk;
  double uy  = xmy * sinuk + sinnok * cosuk;
  double uz  = sinik * sinuk;
  double vx  = xmx * cosuk - cosnok * sinuk;
  double vy  = xmy * cosuk - sinnok * sinuk;
  double vz  = sinik * cosuk;

  // Position
  double x = rk * ux;
  double y = rk * uy;
  double z = rk * uz;

  cVector vecPos(x, y, z);

  // Validate on altitude
  double altKm = (vecPos.Magnitude() * (XKMPER_WGS72 / AE));

  if ((altKm < XKMPER_WGS72) || (altKm > (2 * GEOSYNC_ALT)))
    return false;
   
  // Velocity
  double xdot = rdotk * ux + rfdotk * vx;
  double ydot = rdotk * uy + rfdotk * vy;
  double zdot = rdotk * uz + rfdotk * vz;

  cVector vecVel(xdot, ydot, zdot);

  cJulian gmt = m_Orbit.Epoch();
  gmt.addMin(tsince);

  eci = cEci(vecPos, vecVel, gmt);

  return true;
}

//
// cNoradSGP4.cpp
//
// NORAD SGP4 implementation. See historical note in cNoradBase.cpp
// Copyright (c) 2003 Michael F. Henry
//
// mfh 12/07/2003
//
//////////////////////////////////////////////////////////////////////////////
cNoradSGP4::cNoradSGP4(const cOrbit &orbit) :
  cNoradBase(orbit)
{
  m_c5     = 2.0 * m_coef1 * m_aodp * m_betao2 * 
    (1.0 + 2.75 * (m_etasq + m_eeta) + m_eeta * m_etasq);
  m_omgcof = m_Orbit.BStar() * m_c3 * cos(m_Orbit.ArgPerigee());
  m_xmcof  = -TWOTHRD * m_coef * m_Orbit.BStar() * AE / m_eeta;
  m_delmo  = pow(1.0 + m_eta * cos(m_Orbit.mnAnomaly()), 3.0);
  m_sinmo  = sin(m_Orbit.mnAnomaly());
}


//////////////////////////////////////////////////////////////////////////////
// getPosition() 
// This procedure returns the ECI position and velocity for the satellite
// in the orbit at the given number of minutes since the TLE epoch time
// using the NORAD Simplified General Perturbation 4, near earth orbit
// model.
//
// tsince - Time in minutes since the TLE epoch (GMT).
// eci    - ECI object to hold position information.
//           To convert the returned ECI position vector to km,
//           multiply each component by: 
//              (XKMPER_WGS72 / AE).
//           To convert the returned ECI velocity vector to km/sec, 
//           multiply each component by:
//              (XKMPER_WGS72 / AE) * (MIN_PER_DAY / 86400).

bool cNoradSGP4::getPosition(double tsince, cEci &eci)
{
  // For m_perigee less than 220 kilometers, the isimp flag is set and
  // the equations are truncated to linear variation in sqrt a and
  // quadratic variation in mean anomaly.  Also, the m_c3 term, the
  // delta omega term, and the delta m term are dropped.
  bool isimp = false;
  if ((m_aodp * (1.0 - m_satEcc) / AE) < (220.0 / XKMPER_WGS72 + AE))
    {
      isimp = true;
    }

  double d2 = 0.0;
  double d3 = 0.0;
  double d4 = 0.0;

  double t3cof = 0.0;
  double t4cof = 0.0;
  double t5cof = 0.0;

  if (!isimp)
    {
      double c1sq = m_c1 * m_c1;

      d2 = 4.0 * m_aodp * m_tsi * c1sq;

      double temp = d2 * m_tsi * m_c1 / 3.0;

      d3 = (17.0 * m_aodp + m_s4) * temp;
      d4 = 0.5 * temp * m_aodp * m_tsi * 
        (221.0 * m_aodp + 31.0 * m_s4) * m_c1;
      t3cof = d2 + 2.0 * c1sq;
      t4cof = 0.25 * (3.0 * d3 + m_c1 * (12.0 * d2 + 10.0 * c1sq));
      t5cof = 0.2 * (3.0 * d4 + 12.0 * m_c1 * d3 + 6.0 * 
                     d2 * d2 + 15.0 * c1sq * (2.0 * d2 + c1sq));
    }

  // Update for secular gravity and atmospheric drag. 
  double xmdf   = m_Orbit.mnAnomaly() + m_xmdot * tsince;
  double omgadf = m_Orbit.ArgPerigee() + m_omgdot * tsince;
  double xnoddf = m_Orbit.RAAN() + m_xnodot * tsince;
  double omega  = omgadf;
  double xmp    = xmdf;
  double tsq    = tsince * tsince;
  double xnode  = xnoddf + m_xnodcf * tsq;
  double tempa  = 1.0 - m_c1 * tsince;
  double tempe  = m_Orbit.BStar() * m_c4 * tsince;
  double templ  = m_t2cof * tsq;

  if (!isimp)
    {
      double delomg = m_omgcof * tsince;
      double delm = m_xmcof * (pow(1.0 + m_eta * cos(xmdf), 3.0) - m_delmo);
      double temp = delomg + delm;

      xmp = xmdf + temp;
      omega = omgadf - temp;

      double tcube = tsq * tsince;
      double tfour = tsince * tcube;

      tempa = tempa - d2 * tsq - d3 * tcube - d4 * tfour;
      tempe = tempe + m_Orbit.BStar() * m_c5 * (sin(xmp) - m_sinmo);
      templ = templ + t3cof * tcube + tfour * (t4cof + tsince * t5cof);
    }

  double a  = m_aodp * sqr(tempa);
  double e  = m_satEcc - tempe;


  double xl = xmp + omega + xnode + m_xnodp * templ;
  double xn = XKE / pow(a, 1.5);

  return FinalPosition(m_satInc, omgadf, e, a, xl, xnode, xn, tsince, eci);
}

//
// cNoradSDP4.cpp
//
// NORAD SDP4 implementation. See historical note in cNoradBase.cpp
// Copyright (c) 2003 Michael F. Henry
//
// mfh 12/07/2003
//

const double zns    =  1.19459E-5;     const double c1ss   =  2.9864797E-6;   
const double zes    =  0.01675;        const double znl    =  1.5835218E-4;   
const double c1l    =  4.7968065E-7;   const double zel    =  0.05490;
const double zcosis =  0.91744867;     const double zsinis =  0.39785416;     
const double zsings = -0.98088458;     const double zcosgs =  0.1945905;      
const double q22    =  1.7891679E-6;   const double q31    =  2.1460748E-6;   
const double q33    =  2.2123015E-7;   const double g22    =  5.7686396;      
const double g32    =  0.95240898;     const double g44    =  1.8014998;
const double g52    =  1.0508330;      const double g54    =  4.4108898;      
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 thdt   =  4.3752691E-3;

//////////////////////////////////////////////////////////////////////////////
cNoradSDP4::cNoradSDP4(const cOrbit &orbit) :
   cNoradBase(orbit)
{
   m_sing = sin(m_Orbit.ArgPerigee());
   m_cosg = cos(m_Orbit.ArgPerigee());
   
   dp_savtsn = 0.0;
   dp_zmos = 0.0;
   dp_se2 = 0.0;
   dp_se3 = 0.0;
   dp_si2 = 0.0;
   dp_si3 = 0.0;
   dp_sl2 = 0.0;
   dp_sl3 = 0.0;
   dp_sl4 = 0.0;
   dp_sghs = 0.0;
   dp_sgh2 = 0.0;
   dp_sgh3 = 0.0;
   dp_sgh4 = 0.0;
   dp_sh2 = 0.0;
   dp_sh3 = 0.0;
   dp_zmol = 0.0;
   dp_ee2 = 0.0;
   dp_e3 = 0.0;
   dp_xi2 = 0.0;
   dp_xi3 = 0.0;
   dp_xl2 = 0.0;
   dp_xl3 = 0.0;
   dp_xl4 = 0.0;
   dp_xgh2 = 0.0;
   dp_xgh3 = 0.0;
   dp_xgh4 = 0.0;
   dp_xh2 = 0.0;
   dp_xh3 = 0.0;
   dp_xqncl = 0.0;
   dp_thgr = 0.0;
   dp_omegaq = 0.0;
   dp_sse = 0.0;
   dp_ssi = 0.0;
   dp_ssl = 0.0;
   dp_ssh = 0.0;
   dp_ssg = 0.0;
   dp_d2201 = 0.0;
   dp_d2211 = 0.0;
   dp_d3210 = 0.0;
   dp_d3222 = 0.0;
   dp_d4410 = 0.0;
   dp_d4422 = 0.0;
   dp_d5220 = 0.0;
   dp_d5232 = 0.0;
   dp_d5421 = 0.0;
   dp_d5433 = 0.0;
   dp_xlamo = 0.0;
   dp_del1 = 0.0;
   dp_del2 = 0.0;
   dp_del3 = 0.0;
   dp_fasx2 = 0.0;
   dp_fasx4 = 0.0;
   dp_fasx6 = 0.0;
   dp_xfact = 0.0;
   dp_xli = 0.0;
   dp_xni = 0.0;
   dp_atime = 0.0;
   dp_stepp = 0.0;
   dp_stepn = 0.0;
   dp_step2 = 0.0;

   dp_iresfl = false;
   dp_isynfl = false;

}


/////////////////////////////////////////////////////////////////////////////
bool cNoradSDP4::DeepInit(double *eosq,  double *sinio,  double *cosio,
                          double *betao, double *aodp,   double *theta2,
                          double *sing,  double *cosg,   double *betao2,
                          double *xmdot, double *omgdot, double *xnodott)
{
   eqsq   = *eosq;
   siniq  = *sinio;   
   cosiq  = *cosio;   
   rteqsq = *betao;
   ao     = *aodp;    
   cosq2  = *theta2;  
   sinomo = *sing;    
   cosomo = *cosg;
   bsq    = *betao2;  
   xlldot = *xmdot;   
   omgdt  = *omgdot;  
   xnodot = *xnodott;

   // Deep space initialization 
   cJulian jd = m_Orbit.Epoch();

   dp_thgr = jd.toGMST();

   double eq   = m_Orbit.Eccentricity();
   double aqnv = 1.0 / ao;

   dp_xqncl = m_Orbit.Inclination();

   double xmao   = m_Orbit.mnAnomaly();
   double xpidot = omgdt + xnodot;
   double sinq   = sin(m_Orbit.RAAN());
   double cosq   = cos(m_Orbit.RAAN());

   dp_omegaq = m_Orbit.ArgPerigee();

   // Initialize lunar solar terms 
   double day = jd.FromJan1_12h_1900();

   if (day != dpi_day)
   {
      dpi_day    = day;
      dpi_xnodce = 4.5236020 - 9.2422029E-4 * day;
      dpi_stem   = sin(dpi_xnodce);
      dpi_ctem   = cos(dpi_xnodce);
      dpi_zcosil = 0.91375164 - 0.03568096 * dpi_ctem;
      dpi_zsinil = sqrt(1.0 - dpi_zcosil * dpi_zcosil);
      dpi_zsinhl = 0.089683511 *dpi_stem / dpi_zsinil;
      dpi_zcoshl = sqrt(1.0 - dpi_zsinhl * dpi_zsinhl);
      dpi_c      = 4.7199672 + 0.22997150 * day;
      dpi_gam    = 5.8351514 + 0.0019443680 * day;
      dp_zmol    = Fmod2p(dpi_c - dpi_gam);
      dpi_zx     = 0.39785416 * dpi_stem / dpi_zsinil;
      dpi_zy     = dpi_zcoshl * dpi_ctem + 0.91744867 * dpi_zsinhl * dpi_stem;
      dpi_zx     = AcTan(dpi_zx,dpi_zy) + dpi_gam - dpi_xnodce;
      dpi_zcosgl = cos(dpi_zx);
      dpi_zsingl = sin(dpi_zx);
      dp_zmos    = 6.2565837 + 0.017201977 * day;
      dp_zmos    = Fmod2p(dp_zmos);
   }

   dp_savtsn = 1.0e20;

   double zcosg = zcosgs;
   double zsing = zsings;
   double zcosi = zcosis;
   double zsini = zsinis;
   double zcosh = cosq;
   double zsinh = sinq;
   double cc  = c1ss;
   double zn  = zns;
   double ze  = zes;
   //   double zmo = dp_zmos;
   double xnoi = 1.0 / m_xnodp;

   double a1;  double a3;  double a7;  double a8;  double a9;  double a10;
   double a2;  double a4;  double a5;  double a6;  double x1;  double x2;
   double x3;  double x4;  double x5;  double x6;  double x7;  double x8;
   double z31; double z32; double z33; double z1;  double z2;  double z3;
   double z11; double z12; double z13; double z21; double z22; double z23;
   double s3;  double s2;  double s4;  double s1;  double s5;  double s6;
   double s7;  
   double se  = 0.0;  double si = 0.0;  double sl = 0.0;  
   double sgh = 0.0;  double sh = 0.0;

   // Apply the solar and lunar terms on the first pass, then re-apply the
   // solar terms again on the second pass.

   for (int pass = 1; pass <= 2; pass++)
   {
      // Do solar terms 
      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 = 12.0 * x1 * x1 - 3.0 * x3 * x3;
      z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4;
      z33 = 12.0 * x2 * x2 - 3.0 * x4 * 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 * xnoi;
      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  = s1 * zn * s5;
      si  = s2 * zn * (z11 + z13);
      sl  = -zn * s3 * (z1 + z3 - 14.0 - 6.0 * eqsq);
      sgh = s4 * zn * (z31 + z33 - 6.0);
      sh  = -zn * s2 * (z21 + z23);

      if (dp_xqncl < 5.2359877E-2)
         sh = 0.0;

      dp_ee2 = 2.0 * s1 * s6;
      dp_e3  = 2.0 * s1 * s7;
      dp_xi2 = 2.0 * s2 * z12;
      dp_xi3 = 2.0 * s2 * (z13 - z11);
      dp_xl2 = -2.0 * s3 * z2;
      dp_xl3 = -2.0 * s3 * (z3 - z1);
      dp_xl4 = -2.0 * s3 * (-21.0 - 9.0 * eqsq) * ze;
      dp_xgh2 = 2.0 * s4 * z32;
      dp_xgh3 = 2.0 * s4 * (z33 - z31);
      dp_xgh4 = -18.0 * s4 * ze;
      dp_xh2 = -2.0 * s2 * z22;
      dp_xh3 = -2.0 * s2 * (z23 - z21);

      if (pass == 1)
      {
         // Do lunar terms 
         dp_sse = se;
         dp_ssi = si;
         dp_ssl = sl;
         dp_ssh = sh / siniq;
         dp_ssg = sgh - cosiq * dp_ssh;
         dp_se2 = dp_ee2;
         dp_si2 = dp_xi2;
         dp_sl2 = dp_xl2;
         dp_sgh2 = dp_xgh2;
         dp_sh2 = dp_xh2;
         dp_se3 = dp_e3;
         dp_si3 = dp_xi3;
         dp_sl3 = dp_xl3;
         dp_sgh3 = dp_xgh3;
         dp_sh3 = dp_xh3;
         dp_sl4 = dp_xl4;
         dp_sgh4 = dp_xgh4;
         zcosg = dpi_zcosgl;
         zsing = dpi_zsingl;
         zcosi = dpi_zcosil;
         zsini = dpi_zsinil;
         zcosh = dpi_zcoshl * cosq + dpi_zsinhl * sinq;
         zsinh = sinq * dpi_zcoshl - cosq * dpi_zsinhl;
         zn = znl;
         cc = c1l;
         ze = zel;
         //         zmo = dp_zmol;
      }
   }

   dp_sse = dp_sse + se;
   dp_ssi = dp_ssi + si;
   dp_ssl = dp_ssl + sl;
   dp_ssg = dp_ssg + sgh - cosiq / siniq * sh;
   dp_ssh = dp_ssh + sh / siniq;

   // Geopotential resonance initialization for 12 hour orbits 
   dp_iresfl = false;
   dp_isynfl = false;

   bool   bInitOnExit = true;
   double g310;
   double f220;
   double bfact = 0.0;

   if ((m_xnodp >= 0.0052359877) || (m_xnodp <= 0.0034906585))
   {
      if ((m_xnodp < 8.26E-3) || (m_xnodp > 9.24E-3) || (eq < 0.5))
      {
         bInitOnExit = false;
      }
      else
      {
         dp_iresfl = true;

         double eoc  = eq * eqsq;
         double g201 = -0.306 - (eq - 0.64) * 0.440;

         double g211;   double g322;

         double g410;   double g422;   
         double g520;

         if (eq <= 0.65)
         {
            g211 = 3.616 - 13.247 * eq + 16.290 * eqsq;
            g310 = -19.302 + 117.390 * eq - 228.419 * eqsq + 156.591 * eoc;
            g322 = -18.9068 + 109.7927 * eq - 214.6334 * eqsq + 146.5816 * eoc;
            g410 = -41.122 + 242.694 * eq - 471.094 * eqsq + 313.953 * eoc;
            g422 = -146.407 + 841.880 * eq - 1629.014 * eqsq + 1083.435 * eoc;
            g520 = -532.114 + 3017.977 * eq - 5740.0 * eqsq + 3708.276 * eoc;
         }
         else
         {
            g211 = -72.099 + 331.819 * eq - 508.738 * eqsq + 266.724 * eoc;
            g310 = -346.844 + 1582.851 * eq - 2415.925 * eqsq + 1246.113 * eoc;
            g322 = -342.585 + 1554.908 * eq - 2366.899 * eqsq + 1215.972 * eoc;
            g410 = -1052.797 + 4758.686 * eq - 7193.992 * eqsq + 3651.957 * eoc;
            g422 = -3581.69 + 16178.11 * eq - 24462.77 * eqsq + 12422.52 * eoc;

            if (eq <= 0.715)
               g520 = 1464.74 - 4664.75 * eq + 3763.64 * eqsq;
            else
               g520 = -5149.66 + 29936.92 * eq - 54087.36 * eqsq + 31324.56 * eoc;
         }

         double g533;   
         double g521;   
         double g532;

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

         double sini2 = siniq * siniq;
         f220 = 0.75*(1.0 + 2.0 * cosiq + cosq2);
         double f221 = 1.5 * sini2;
         double f321 = 1.875 * siniq*(1.0 - 2.0 * cosiq - 3.0 * cosq2);
         double f322 = -1.875 * siniq*(1.0 + 2.0 * cosiq - 3.0 * cosq2);
         double f441 = 35.0 * sini2 * f220;
         double f442 = 39.3750 * sini2 * sini2;
         double f522 = 9.84375 * siniq*(sini2*(1.0 - 2.0 * cosiq - 5.0 * cosq2) +
                       0.33333333*(-2.0 + 4.0 * cosiq + 6.0 * cosq2));
         double f523 = siniq*(4.92187512 * sini2*(-2.0 - 4.0 * cosiq + 10.0 * cosq2) +
                       6.56250012 * (1.0 + 2.0 * cosiq - 3.0 * cosq2));
         double f542 = 29.53125 * siniq*(2.0 - 8.0 * cosiq + cosq2 * (-12.0 + 8.0 * cosiq + 10.0 * cosq2));
         double f543 = 29.53125 * siniq*(-2.0 - 8.0 * cosiq + cosq2 * (12.0 + 8.0 * cosiq - 10.0 * cosq2));
         double xno2 = m_xnodp * m_xnodp;
         double ainv2 = aqnv * aqnv;
         double temp1 = 3.0 * xno2 * ainv2;
         double temp = temp1 * root22;

         dp_d2201 = temp * f220 * g201;
         dp_d2211 = temp * f221 * g211;
         temp1 = temp1 * aqnv;
         temp = temp1 * root32;
         dp_d3210 = temp * f321 * g310;
         dp_d3222 = temp * f322 * g322;
         temp1 = temp1 * aqnv;
         temp = 2.0 * temp1 * root44;
         dp_d4410 = temp * f441 * g410;
         dp_d4422 = temp * f442 * g422;
         temp1 = temp1 * aqnv;
         temp  = temp1 * root52;
         dp_d5220 = temp * f522 * g520;
         dp_d5232 = temp * f523 * g532;
         temp = 2.0 * temp1 * root54;
         dp_d5421 = temp * f542 * g521;
         dp_d5433 = temp * f543 * g533;
         dp_xlamo = xmao + m_Orbit.RAAN() + m_Orbit.RAAN() - dp_thgr - dp_thgr;
         bfact = xlldot + xnodot + xnodot - thdt - thdt;
         bfact = bfact + dp_ssl + dp_ssh + dp_ssh;
      }
   }
   else
   {
      // Synchronous resonance terms initialization 
      dp_iresfl = true;
      dp_isynfl = true;
      double g200 = 1.0 + eqsq * (-2.5 + 0.8125 * eqsq);
      g310 = 1.0 + 2.0 * eqsq;
      double g300 = 1.0 + eqsq * (-6.0 + 6.60937 * eqsq);
      f220 = 0.75 * (1.0 + cosiq) * (1.0 + cosiq);
      double f311 = 0.9375 * siniq * siniq * (1.0 + 3 * cosiq) - 0.75 * (1.0 + cosiq);
      double f330 = 1.0 + cosiq;
      f330 = 1.875 * f330 * f330 * f330;
      dp_del1 = 3.0 * m_xnodp * m_xnodp * aqnv * aqnv;
      dp_del2 = 2.0 * dp_del1 * f220 * g200 * q22;
      dp_del3 = 3.0 * dp_del1 * f330 * g300 * q33 * aqnv;
      dp_del1 = dp_del1 * f311 * g310 * q31 * aqnv;
      dp_fasx2 = 0.13130908;
      dp_fasx4 = 2.8843198;
      dp_fasx6 = 0.37448087;
      dp_xlamo = xmao + m_Orbit.RAAN() + m_Orbit.ArgPerigee() - dp_thgr;
      bfact = xlldot + xpidot - thdt;
      bfact = bfact + dp_ssl + dp_ssg + dp_ssh;
   }

   if (bInitOnExit)
   {
      dp_xfact = bfact - m_xnodp;

      // Initialize integrator 
      dp_xli = dp_xlamo;
      dp_xni = m_xnodp;
      dp_atime = 0.0;
      dp_stepp = 720.0;
      dp_stepn = -720.0;
      dp_step2 = 259200.0;
   }

   *eosq   = eqsq;    
   *sinio  = siniq;   
   *cosio  = cosiq;   
   *betao  = rteqsq;
   *aodp   = ao;      
   *theta2 = cosq2;   
   *sing   = sinomo;  
   *cosg   = cosomo;
   *betao2 = bsq;     
   *xmdot  = xlldot;  
   *omgdot = omgdt;   
   *xnodott = xnodot;

   return true;
}

//////////////////////////////////////////////////////////////////////////////
bool cNoradSDP4::DeepCalcDotTerms(double *pxndot, double *pxnddt, double *pxldot)
{
    // Dot terms calculated 
   if (dp_isynfl)
   {
      *pxndot = dp_del1 * sin(dp_xli - dp_fasx2) + 
                dp_del2 * sin(2.0 * (dp_xli - dp_fasx4)) +
                dp_del3 * sin(3.0 * (dp_xli - dp_fasx6));
      *pxnddt = dp_del1 * cos(dp_xli - dp_fasx2) +
                2.0 * dp_del2 * cos(2.0 * (dp_xli - dp_fasx4)) +
                3.0 * dp_del3 * cos(3.0 * (dp_xli - dp_fasx6));
   }
   else
   {
      double xomi  = dp_omegaq + omgdt * dp_atime;
      double x2omi = xomi + xomi;
      double x2li  = dp_xli + dp_xli;

      *pxndot = dp_d2201 * sin(x2omi + dp_xli - g22) + 
                dp_d2211 * sin(dp_xli - g22)         +
                dp_d3210 * sin(xomi + dp_xli - g32)  +
                dp_d3222 * sin(-xomi + dp_xli - g32) +
                dp_d4410 * sin(x2omi + x2li - g44)   +
                dp_d4422 * sin(x2li - g44)           +
                dp_d5220 * sin(xomi + dp_xli - g52)  +
                dp_d5232 * sin(-xomi + dp_xli - g52) +
                dp_d5421 * sin(xomi + x2li - g54)    +
                dp_d5433 * sin(-xomi + x2li - g54);

      *pxnddt = dp_d2201 * cos(x2omi + dp_xli - g22) +
                dp_d2211 * cos(dp_xli - g22)         +
                dp_d3210 * cos(xomi + dp_xli - g32)  +
                dp_d3222 * cos(-xomi + dp_xli - g32) +
                dp_d5220 * cos(xomi + dp_xli - g52)  +
                dp_d5232 * cos(-xomi + dp_xli - g52) +
                2.0 * (dp_d4410 * cos(x2omi + x2li - g44) +
                dp_d4422 * cos(x2li - g44)         +
                dp_d5421 * cos(xomi + x2li - g54)  +
                dp_d5433 * cos(-xomi + x2li - g54));
   }

   *pxldot = dp_xni + dp_xfact;
   *pxnddt = (*pxnddt) * (*pxldot);

   return true;
}

//////////////////////////////////////////////////////////////////////////////
void cNoradSDP4::DeepCalcIntegrator(double *pxndot, double *pxnddt, 
                                    double *pxldot, const double &delt)
{
   DeepCalcDotTerms(pxndot, pxnddt, pxldot);

   dp_xli = dp_xli + (*pxldot) * delt + (*pxndot) * dp_step2;
   dp_xni = dp_xni + (*pxndot) * delt + (*pxnddt) * dp_step2;
   dp_atime = dp_atime + delt;
}

//////////////////////////////////////////////////////////////////////////////
bool cNoradSDP4::DeepSecular(double *xmdf, double *omgadf, double *xnode,
                             double *emm,  double *xincc,  double *xnn,
                             double *tsince)
{
   xll    = *xmdf;    
   omgasm = *omgadf;  
   xnodes = *xnode;  
   xn     = *xnn;  
   t      = *tsince;

   // Deep space secular effects 
   xll    = xll + dp_ssl * t;
   omgasm = omgasm + dp_ssg * t;
   xnodes = xnodes + dp_ssh * t;
   _em    = m_Orbit.Eccentricity() + dp_sse * t;
   xinc   = m_Orbit.Inclination()  + dp_ssi * t;

   if (xinc < 0.0)
   {
      xinc   = -xinc;
      xnodes = xnodes + PI;
      omgasm = omgasm - PI;
   }

   double xnddt = 0.0;
   double xndot = 0.0;
   double xldot = 0.0;
   double ft    = 0.0;
   double delt  = 0.0;

   bool fDone = false;

   if (dp_iresfl) 
   {
      while (!fDone)
      {
         if ((dp_atime == 0.0)                ||
            ((t >= 0.0) && (dp_atime <  0.0)) ||
            ((t <  0.0) && (dp_atime >= 0.0)))
         {
            if (t < 0)
               delt = dp_stepn;
            else
               delt = dp_stepp;

            // Epoch restart 
            dp_atime = 0.0;
            dp_xni = m_xnodp;
            dp_xli = dp_xlamo;

            fDone = true;
         }
         else
         {
            if (fabs(t) < fabs(dp_atime))
            {
               delt = dp_stepp;

               if (t >= 0.0)
                  delt = dp_stepn;

               DeepCalcIntegrator(&xndot, &xnddt, &xldot, delt);
            }
            else
            {
               delt = dp_stepn;

                           delt = dp_stepp;

               fDone = true;
            }
         }
      }

      while (fabs(t - dp_atime) >= dp_stepp)
      {
         DeepCalcIntegrator(&xndot, &xnddt, &xldot, delt);
      }

      ft = t - dp_atime;

      DeepCalcDotTerms(&xndot, &xnddt, &xldot);

      xn = dp_xni + xndot * ft + xnddt * ft * ft * 0.5;

      double xl   = dp_xli + xldot * ft + xndot * ft * ft * 0.5;
      double temp = -xnodes + dp_thgr + t * thdt;

      xll = xl - omgasm + temp;

      if (!dp_isynfl)
         xll = xl + temp + temp;
   }

   *xmdf   = xll; 
   *omgadf = omgasm;
   *xnode  = xnodes;
   *emm    = _em;
   *xincc  = xinc;
   *xnn    = xn;
   *tsince = t;

   return true;
}

//////////////////////////////////////////////////////////////////////////////
bool cNoradSDP4::DeepPeriodics(double *e,      double *xincc,
                               double *omgadf, double *xnode,
                               double *xmam)
{
   _em    = *e;       
   xinc   = *xincc; 
   omgasm = *omgadf;
   xnodes = *xnode;
   xll    = *xmam;

   // Lunar-solar periodics 
   double sinis = sin(xinc);
   double cosis = cos(xinc);

   double sghs = 0.0;
   double shs  = 0.0;
   double sh1  = 0.0;
   double pe   = 0.0;
   double pinc = 0.0;
   double pl   = 0.0;
   double sghl = 0.0;

   if (fabs(dp_savtsn - t) >= 30.0)
   {
      dp_savtsn = t;

      double zm = dp_zmos + zns * t;
      double zf = zm + 2.0 * zes * sin(zm);
      double sinzf = sin(zf);
      double f2  = 0.5 * sinzf * sinzf - 0.25;
      double f3  = -0.5 * sinzf * cos(zf);
      double ses = dp_se2 * f2 + dp_se3 * f3;
      double sis = dp_si2 * f2 + dp_si3 * f3;
      double sls = dp_sl2 * f2 + dp_sl3 * f3 + dp_sl4 * sinzf;

      sghs = dp_sgh2 * f2 + dp_sgh3 * f3 + dp_sgh4 * sinzf;
      shs = dp_sh2 * f2 + dp_sh3 * f3;
      zm = dp_zmol + znl * t;
      zf = zm + 2.0 * zel * sin(zm);
      sinzf = sin(zf);
      f2 = 0.5 * sinzf * sinzf - 0.25;
      f3 = -0.5 * sinzf * cos(zf);

      double sel  = dp_ee2 * f2 + dp_e3 * f3;
      double sil  = dp_xi2 * f2 + dp_xi3 * f3;
      double sll  = dp_xl2 * f2 + dp_xl3 * f3 + dp_xl4 * sinzf;

      sghl = dp_xgh2 * f2 + dp_xgh3 * f3 + dp_xgh4 * sinzf;
      sh1  = dp_xh2 * f2 + dp_xh3 * f3;
      pe   = ses + sel;
      pinc = sis + sil;
      pl   = sls + sll;
   }

   double pgh  = sghs + sghl;
   double ph   = shs + sh1;
   xinc = xinc + pinc;
   _em  = _em + pe;

   if (dp_xqncl >= 0.2)
   {
      // Apply periodics directly 
      ph  = ph / siniq;
      pgh = pgh - cosiq * ph;
      omgasm = omgasm + pgh;
      xnodes = xnodes + ph;
      xll = xll + pl;
   }
   else
   {
      // Apply periodics with Lyddane modification 
      double sinok = sin(xnodes);
      double cosok = cos(xnodes);
      double alfdp = sinis * sinok;
      double betdp = sinis * cosok;
      double dalf  =  ph * cosok + pinc * cosis * sinok;
      double dbet  = -ph * sinok + pinc * cosis * cosok;

      alfdp = alfdp + dalf;
      betdp = betdp + dbet;

      double xls = xll + omgasm + cosis * xnodes;
      double dls = pl + pgh - pinc * xnodes * sinis;

      xls    = xls + dls;
      xnodes = AcTan(alfdp, betdp);
      xll    = xll + pl;
      omgasm = xls - xll - cos(xinc) * xnodes;
   }

   *e      = _em;      
   *xincc  = xinc;    
   *omgadf = omgasm;  

   *xnode  = xnodes;
   *xmam   = xll;

   return true;
}

//////////////////////////////////////////////////////////////////////////////
// getPosition()
// This procedure returns the ECI position and velocity for the satellite
// in the orbit at the given number of minutes since the TLE epoch time
// using the NORAD Simplified General Perturbation 4, "deep space" orbit
// model.
//
// tsince  - Time in minutes since the TLE epoch (GMT).
// pECI    - pointer to location to store the ECI data.
//           To convert the returned ECI position vector to km,
//           multiply each component by: 
//              (XKMPER_WGS72 / AE).
//           To convert the returned ECI velocity vector to km/sec, 
//           multiply each component by:
//              (XKMPER_WGS72 / AE) * (MIN_PER_DAY / 86400).
bool cNoradSDP4::getPosition(double tsince, cEci &eci)
{
   DeepInit(&m_eosq, &m_sinio, &m_cosio,  &m_betao, &m_aodp,   &m_theta2, 
            &m_sing, &m_cosg,  &m_betao2, &m_xmdot, &m_omgdot, &m_xnodot);

   // Update for secular gravity and atmospheric drag 
   double xmdf   = m_Orbit.mnAnomaly() + m_xmdot * tsince;
   double omgadf = m_Orbit.ArgPerigee() + m_omgdot * tsince;
   double xnoddf = m_Orbit.RAAN() + m_xnodot * tsince;
   double tsq    = tsince * tsince;
   double xnode  = xnoddf + m_xnodcf * tsq;
   double tempa  = 1.0 - m_c1 * tsince;
   double tempe  = m_Orbit.BStar() * m_c4 * tsince;
   double templ  = m_t2cof * tsq;
   double xn     = m_xnodp;
   double em;
   double xinc;

   DeepSecular(&xmdf, &omgadf, &xnode, &em, &xinc, &xn, &tsince);

   double a    = pow(XKE / xn, TWOTHRD) * sqr(tempa);
   double e    = em - tempe;
   double xmam = xmdf + m_xnodp * templ;

   DeepPeriodics(&e, &xinc, &omgadf, &xnode, &xmam);

   double xl = xmam + omgadf + xnode;

   xn = XKE / pow(a, 1.5);

   return FinalPosition(xinc, omgadf, e, a, xl, xnode, xn, tsince, eci);
}


// cOrbit.cpp
//
// Copyright (c) 2002-2003 Michael F. Henry
//
// mfh 11/15/2003
// 
//////////////////////////////////////////////////////////////////////
cOrbit::cOrbit(const cTle &tle) :
  m_tle(tle),
  m_pNoradModel(NULL)
{
  m_tle.Initialize();

  int    epochYear = (int)m_tle.getField(cTle::FLD_EPOCHYEAR);
  double epochDay  =      m_tle.getField(cTle::FLD_EPOCHDAY );

  if (epochYear < 57)
    epochYear += 2000;
  else
    epochYear += 1900;

  m_jdEpoch = cJulian(epochYear, epochDay);

  m_secPeriod = -1.0;

  // Recover the original mean motion and semimajor axis from the
  // input elements.
  double mm     = mnMotion();
  double rpmin  = mm * 2 * PI / MIN_PER_DAY;   // rads per minute

  double a1     = pow(XKE / rpmin, TWOTHRD);
  double e      = Eccentricity();
  double i      = Inclination();
  double temp   = (1.5 * CK2 * (3.0 * sqr(cos(i)) - 1.0) / 
                   pow(1.0 - e * e, 1.5));   
  double delta1 = temp / (a1 * a1);
  double a0     = a1 * 
    (1.0 - delta1 * 
     ((1.0 / 3.0) + delta1 * 
      (1.0 + 134.0 / 81.0 * delta1)));

  double delta0 = temp / (a0 * a0);

  m_mnMotionRec        = rpmin / (1.0 + delta0);
  m_aeAxisSemiMinorRec = a0 / (1.0 - delta0);
  m_aeAxisSemiMajorRec = m_aeAxisSemiMinorRec / sqrt(1.0 - (e * e));
  m_kmPerigeeRec       = XKMPER_WGS72 * (m_aeAxisSemiMajorRec * (1.0 - e) - AE);
  m_kmApogeeRec        = XKMPER_WGS72 * (m_aeAxisSemiMajorRec * (1.0 + e) - AE);

  if (2.0 * PI / m_mnMotionRec >= 225.0)
    {
      // SDP4 - period >= 225 minutes.
      m_pNoradModel = new cNoradSDP4(*this);
    }
  else
    {
      // SGP4 - period < 225 minutes
      m_pNoradModel = new cNoradSGP4(*this);
    }
}

/////////////////////////////////////////////////////////////////////////////
cOrbit::~cOrbit()
{
  delete m_pNoradModel;
}

//////////////////////////////////////////////////////////////////////////////
// Return the period in seconds
double cOrbit::Period() const
{
  if (m_secPeriod < 0.0)
    {
      // Calculate the period using the recovered mean motion.
      if (m_mnMotionRec == 0)
        m_secPeriod = 0.0;
      else
        m_secPeriod = (2 * PI) / m_mnMotionRec * 60.0;
    }

  return m_secPeriod;
}

//////////////////////////////////////////////////////////////////////////////
// Returns elapsed number of seconds from epoch to given time.
// Note: "Predicted" TLEs can have epochs in the future.
double cOrbit::TPlusEpoch(const cJulian &gmt) const
{
  return gmt.spanSec(Epoch());
}

//////////////////////////////////////////////////////////////////////////////
// Returns the mean anomaly in radians at given GMT.
// At epoch, the mean anomaly is given by the elements data.
double cOrbit::mnAnomaly(cJulian gmt) const
{
  double span = TPlusEpoch(gmt);
  double P    = Period();

  assert(P != 0.0);

  return fmod(mnAnomaly() + (TWOPI * (span / P)), TWOPI);
}

//////////////////////////////////////////////////////////////////////////////
// getPosition()
// This procedure returns the ECI position and velocity for the satellite
// at "tsince" minutes from the (GMT) TLE epoch. The vectors returned in
// the ECI object are kilometer-based.
// tsince  - Time in minutes since the TLE epoch (GMT).
bool cOrbit::getPosition(double tsince, cEci *pEci) const
{
  bool rc;

  rc = m_pNoradModel->getPosition(tsince, *pEci);

  pEci->ae2km();

  return rc;
}
   
//////////////////////////////////////////////////////////////////////////////
// SatName()
// Return the name of the satellite. If requested, the NORAD number is
// appended to the end of the name, i.e., "ISS (ZARYA) #25544".
// The name of the satellite with the NORAD number appended is important
// because many satellites, especially debris, have the same name and
// would otherwise appear to be the same satellite in ouput data.
string cOrbit::SatName(bool fAppendId /* = false */) const
{
  string str = m_tle.getName();

  if (fAppendId)
    {
      string strId;

      m_tle.getField(cTle::FLD_NORADNUM, cTle::U_NATIVE, &strId);
      str = str + " #" + strId;
    }

  return str;
}
