/** MOPS Library to calculate arbitrary ephemeris given orbital parameters

    $Id: mopsephem.c 1875 2007-09-05 04:20:49Z denneau $
    Calculate ephemerides using specified orbital parameters and JPL libraries.
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <slalib.h>

#include "mopsephem.h"


#define DEG2RAD(d) (M_PI / 180.0 * (d))
#define RAD2DEG(r) (180.0 / M_PI * (r))

static int check_inputs(double q_au,
                        double e,
                        double i,
                        double node,
                        double arg_peri,
                        double time_peri,
                        double h_v) {

  /** Perform zero-order checking of inputs. */
  if (q_au < 0)
    return EBADQ;
  if (e < 0)
    return EBADE;
  if (i > 180 || i < -90)
    return EBADI;

  return 0;
}


int mopsephem_init(void) {
  ssd_init_f();                 ///< initialize JPL stuff
  return 0;
}


int mopsephem_compute_ephemerides(
    ORBIT_BLK *orbit,       ///< input orbit block
    EPHEM_BLK *ephems,      ///< output ephem block
    double *t_ephem_ut_mjds,///< epochs of prediction
	int num_t,              ///< length of t_ephem_ut_mjds
    char *station           ///< Earth station ID (Mauna Kea = "568"
  ) {
  int n;

  if (NULL == orbit || NULL == ephems || NULL == station)
    return EBADINPUT;   ///< problem with inputs

  /* Do some MOPS filtering of data.  
     Generally we want do not want to calculate ephemerides for anything close 
     to the sun or otherwise unobservable.  Initially we have noticed that 
     comets with small q and epoch near time_peri cause problems with JPL code.
     So we're going to skip objects with 
                   q < 0.05 and fabs(time_peri - epoch) < 5. */
  int errcod;
  errcod = check_inputs(orbit->q_au, orbit->e, orbit->i_deg, orbit->node_deg, 
                        orbit->arg_peri_deg, orbit->time_peri_mjd, orbit->h_v);

  if (errcod) {
    return errcod;
  } else {
    /* Some convenience vars */
    double cnv[6] = {1., 1., RAD2DEG(1.), RAD2DEG(1.), RAD2DEG(1.), 1.};
    double cov_matrix[6][6];
    int i, k, ik;
    int hasCovar = 1;
    
    
    /* Check for sun grazers. */
    for(i=0; i<num_t; i++) {
      if(orbit->q_au < 0.05 && 
	 fabs(orbit->time_peri_mjd - t_ephem_ut_mjds[i]) < 5)
	return ESUNGRAZER;
    }

    /* JPL F95 code wants elems in this order. */
    double elems[6];
    elems[0] = orbit->e;
    elems[1] = orbit->q_au;
    elems[2] = orbit->time_peri_mjd;
    elems[3] = orbit->node_deg;
    elems[4] = orbit->arg_peri_deg;
    elems[5] = orbit->i_deg;

    /* If we do not have a g param, set it to 0.15 */
    if(!orbit->g)
      orbit->g = 0.15;
    
    /* Do we have a sqrt covariance matrix? */
    if(!orbit->covar || orbit->covar == NULL) {
      orbit->covar = NULL;
      hasCovar = 0;
    }
    
    /* More setup for many_eph. */
    double sorted_t_ephem_ut_mjds[num_t];
    double pred_ra[num_t];
    double pred_dec[num_t];
    double pred_ra_err[num_t];
    double pred_dec_err[num_t];
    double pred_smaa[num_t];
    double pred_smia[num_t];
    double pred_pa[num_t];
    double pred_v_minus_h[num_t];


    /* Sort times in reverse; many_eph wants this. */
    for (i = 0; i < num_t; i++) {
      sorted_t_ephem_ut_mjds[num_t - 1 - i] = t_ephem_ut_mjds[i];
    }

    ssd_compute_ephemerides_f(&orbit->t_epoch_mjd, 
		     elems, 
		     orbit->covar,
		     station, 
		     &orbit->g,
		     sorted_t_ephem_ut_mjds, 
		     &hasCovar,
		     &num_t,
		     pred_ra,
		     pred_dec,
		     pred_ra_err,
		     pred_dec_err,
		     pred_smaa,
		     pred_smia,
		     pred_pa,
		     pred_v_minus_h,
		     &errcod);

    /* Morph the data in predictions into our ORBIT_BLK structure. */
    /* predictions=[RA,Dec,mag,t,RAErr,DecErr,SMaxis,Smaxis,PA] */
    for(i = 0; i < num_t; i++) {
      n = num_t - i - 1;
      ephems[i].ra_deg = RAD2DEG(pred_ra[n]);
      ephems[i].dec_deg = RAD2DEG(pred_dec[n]);
      ephems[i].mag = orbit->h_v + pred_v_minus_h[n];
      ephems[i].err_ra_sec = pred_ra_err[n];
      ephems[i].err_dec_sec = pred_dec_err[n];
      ephems[i].smaa_sec = pred_smaa[n];
      ephems[i].smia_sec = pred_smia[n];
      ephems[i].pa_deg = pred_pa[n];
      ephems[i].orient_deg = 0;          ///< set these to something
      ephems[i].length_deg = 0;
    }
    
    if (errcod > 0)
      return EJPLEPHEM + errcod;
  }
  return 0;       ///< happy
}



int mopsephem_propagate_elements(
    ORBIT_BLK *in_orbit,        ///< input orbit block
    ORBIT_BLK *out_orbit,       ///< input orbit block
    double epoch_mjd            ///< epoch of propagation
  ) {

  if (NULL == in_orbit || NULL == out_orbit)
    return EBADINPUT;   ///< problem with inputs

  /* Do some MOPS filtering of data.  
     Generally we want do not want to calculate ephemerides for anything close 
     to the sun or otherwise unobservable.  Initially we have noticed that 
     comets with small q and epoch near time_peri cause problems with JPL code.
     So we're going to skip objects with 
                   q < 0.05 and fabs(time_peri - epoch) < 5. */
  int errcod;
  errcod = check_inputs(
    in_orbit->q_au, 
    in_orbit->e, 
    in_orbit->i_deg, 
    in_orbit->node_deg, 
    in_orbit->arg_peri_deg, 
    in_orbit->time_peri_mjd, 
    in_orbit->h_v
  );

  if (errcod) {
    return errcod;
  } 
  else {
    /* JPL F95 code wants elems in this order. */
    double in_elems[6];
    double out_elems[6];
    in_elems[0] = in_orbit->e;
    in_elems[1] = in_orbit->q_au;
    in_elems[2] = in_orbit->time_peri_mjd;
    in_elems[3] = in_orbit->node_deg;
    in_elems[4] = in_orbit->arg_peri_deg;
    in_elems[5] = in_orbit->i_deg;

    /* If we do not have a g param, set it to 0.15 */
    if(!in_orbit->g)
      in_orbit->g = 0.15;
    in_orbit->covar = NULL;
    
    /* Boldly enter fortran land! */
    ssd_propagate_elements_f(
        &in_orbit->t_epoch_mjd, 
		in_elems, 
        out_elems,
		&epoch_mjd, 
		&errcod
    );

    /* Shred out_elems back into out_orbit. */
    *out_orbit = *in_orbit;             // copy stuff, then replace
    out_orbit->e = out_elems[0];
    out_orbit->q_au = out_elems[1];
    out_orbit->time_peri_mjd = out_elems[2];
    out_orbit->node_deg = out_elems[3];
    out_orbit->arg_peri_deg = out_elems[4];
    out_orbit->i_deg = out_elems[5];

    if (errcod > 0)
      return EJPLEPHEM + errcod;
  }
  return 0;       ///< happy
}

