//  Copyright (c) CNES  2008
//
//  This software is part of CelestLab, a CNES toolbox for Scilab
//
//  This software is governed by the CeCILL license under French law and
//  abiding by the rules of distribution of free software.  You can  use,
//  modify and/ or redistribute the software under the terms of the CeCILL
//  license as circulated by CEA, CNRS and INRIA at the following URL
//  'http://www.cecill.info'.

#include "sgp4unit.h"
#include "tle_util.h"
#include "tle_interf.h"


// --------------------------------------------------
// Generates PV from TLE data
// IN: cjd, ecc, ... cjd_eph
// OUT: pos, vel, kep, status, err
// ---
// Units are USI (rad, m, s)
// nconst = set of constants: 721, 72, 84
// opsmode: mode of operation afspc (0) or improved (1)
// cjd_eph: vector of wanted dates (cjd) - size = nb
// err = 0 if no error occurred
// status: indicator for each PV (0 if OK)
// opt == 1 => one initial state, nb final times
// opt == 2 => nb initial states, nb final times
// --------------------------------------------------

void CLx_tle_sgp4(int *opt, double *cjd, double *ecc, double *inc,  double *argp, double *raan, double *ma, double *mm,
                  double *bstar, int *nconst, int *nopsmode, double *delta_t, int *nb,
                  double *pos, double *vel, double *kep, int *status, int *err)
{

gravconsttype whichconst;
char opsmode;

// variables for constants used
double tumin, xmu, radiusearthkm, xke, xj2, xj3, xj4, j3oj2;

elsetrec satrec;

*err = 0;  // no error

// initialize whichconst (which set of constants)
switch (*nconst) {
  case 721: whichconst = wgs72old; break;
  case 72:  whichconst = wgs72; break;
  case 84:  whichconst = wgs84; break;
  default: *err = -1; return;
};

// initialize opsmode (mode of operation)
switch (*nopsmode) {
  case 0: opsmode = 'a'; break;
  case 1: opsmode = 'i'; break;
  default: *err = -1; return;
}

// get constants (for mu only)
getgravconst(whichconst, tumin, xmu, radiusearthkm, xke, xj2, xj3, xj4, j3oj2);

// propagate and generate pv
double r[3];
double v[3];
double po[6];
int k;
double tsince; // time from epoch (minutes)
int i;
int ret;

for (k = 0; k < (*nb); k++) {

  if (*opt == 2 || (*opt == 1 && k == 0)) {

    double no = mm[k] * 60;  // rad / mn

    // sgp4 epoch: from 0 jan 1950
    double epoch = cjd[k] + 1;

    // JD epoch (unused in sgp4 functions)
    // double jdsatepoch = epoch + 2433282.5;

    // sat number (arbitrary)
    int satn = 1;

    // init structure
    sgp4init(whichconst, opsmode, satn, epoch,
       bstar[k], ecc[k], argp[k], inc[k], ma[k], no, raan[k],
       satrec);

  }

  tsince = delta_t[k] / 60.0;  // minutes
  sgp4(whichconst, satrec, tsince, r, v);

  // compute keplerian orbital elements
  ret = pv2kep(r, v, xmu, po);

  // conversion to m and m/s
  for (i = 0; i<3; i++) {
    pos[3*k+i] = r[i] * 1000;
    vel[3*k+i] = v[i] * 1000;
  }

  // orbital elements
  kep[6*k] = po[0] * 1000; // sma in meters
  for (i = 1; i<6; i++) {
    kep[6*k+i] = po[i];
  }

  status[k] = satrec.error;
  if (ret != 0) status[k] = -999;
}

}


// --------------------------------------------------
// Returns constants used in SGP4 functions
// IN: nconst, err
// OUT: er, mu, j2, j3, j4
// ---
// Units are USI (rad, m, s)
// nconst: 721, 72, 84
// err = 0 if no error occurred (wrong value of nconst)
// --------------------------------------------------

void CLx_tle_getconst(int *nconst, double *er, double *mu,  double *j2, double *j3, double *j4, int *err)
{

gravconsttype whichconst;
double tumin, xmu, radiusearthkm, xke, xj2, xj3, xj4, j3oj2;

*err = 0; // no error

// initialize whichconst (which set of constants)
switch (*nconst) {
  case 721: whichconst = wgs72old; break;
  case 72:  whichconst = wgs72; break;
  case 84:  whichconst = wgs84; break;
  default: *err = -1; return;
};

// get constants (no error expected)
getgravconst(whichconst, tumin, xmu, radiusearthkm, xke, xj2, xj3, xj4, j3oj2);

// converts to USI (m, s)
*er = radiusearthkm * 1.e3;
*mu = xmu * 1.e9;
*j2 = xj2;
*j3 = xj3;
*j4 = xj4;

}


// --------------------------------------------------
// Computes sma and mean motion (following brouwer
// convention) from the mean motion present in the TLE.
// The code is extracted from Vallado (see function initl).
// IN:
// mm: mean motion (rad/s)
// ecc: eccentricity
// inc: inclination (rad)
// nconst: set of constant used: 72, 721 or 84
// nb: (integer) dimension of the vectors mm, ecc, inc, sma_br, mm_br
// OUT:
// sma_br: semi major axis (m)
// mm_br: mean motion (rad/s)
// err: error indicator (0 = OK)
// --------------------------------------------------

void CLx_tle_unkozai(double *mm, double *ecc, double *inc, int *nconst, int *nb,
                     double *sma_brw, double *mm_brw, int *err)
{
gravconsttype whichconst;

// variables for constants used (see getgravconst)
double tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2;

*err = 0;  // no error

// initialize whichconst (= which set of constants)
switch (*nconst) {
  case 721: whichconst = wgs72old; break;
  case 72:  whichconst = wgs72; break;
  case 84:  whichconst = wgs84; break;
  default: *err = -1; return;
};

getgravconst(whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2);
double x2o3   = 2.0 / 3.0;

/* ------------- calculate auxillary epoch quantities ---------- */
double eccsq, omeosq, rteosq, cosio, cosio2;
double ak, d1, del, adel, no, ao;
int k;

for (k = 0; k < (*nb); k++) {
  eccsq  = ecc[k] * ecc[k];
  omeosq = 1.0 - eccsq;
  rteosq = sqrt(omeosq);
  cosio  = cos(inc[k]);
  cosio2 = cosio * cosio;
  no = mm[k] * 60;  // => rad/mn

  /* ------------------ un-kozai the mean motion ----------------- */
  ak    = pow(xke / no, x2o3);
  d1    = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq);
  del   = d1 / (ak * ak);
  adel  = ak * (1.0 - del * del - del * (1.0 / 3.0 + 134.0 * del * del / 81.0));
  del   = d1/(adel * adel);
  no    = no / (1.0 + del);
  ao    = pow(xke / no, x2o3);

  /* ------------------ final results ----------------- */
  sma_brw[k] = ao * radiusearthkm * 1000.0; // => meters
  mm_brw[k] = no / 60.0; // => rad/s
}

}