//  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'.

// Position and velocity => keplerian parameters
// (elliptical orbits) 
// inspired from CelestLab: CL__oe_car2kep_ell
// Author: AL

#include "math.h"
#include "tle_util.h"

// Euclidian norm
static double norm(double x[3])
{
  return sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);
}  

// Cross product
static void cross(double x[3], double y[3], double z[3])
{
  z[0]= x[1]*y[2] - x[2]*y[1];
  z[1]= x[2]*y[0] - x[0]*y[2];
  z[2]= x[0]*y[1] - x[1]*y[0];
}  

// Dot product
static double dot(double x[3], double y[3])
{
  return x[0]*y[0] + x[1]*y[1] + x[2]*y[2];
}  

// Eccentric anomaly -> true anomaly (ellipse)
static double E2v(double ecc, double E)
{
  double beta = ecc / (1+sqrt(1-ecc*ecc));
  return E + 2 * atan2(beta * sin(E), 1 - beta * cos(E));
}


// Position and velocity => Keplerian orbital elements
// (elliptical orbit only)
// returns 0 if OK.  
int pv2kep(double pos[3], double vel[3], double mu, double kep[6])
{
  double r = norm(pos); 
  double V = norm(vel); 
  double w[3];
  cross(pos, vel, w);
  double W = norm(w); 
  double pv = dot(pos, vel); 

  if (r == 0 || V == 0 || W == 0) return(-1); 

  // Semi major axis (any orbit type)  
  double sma = r / (2 - r*V*V/mu);

  // Eccentricity (any orbit type)
  double energy = V*V / 2 - mu / r;  
  double ecc = sqrt(fabs(1 + 2 * W*W * energy / (mu*mu)));
  if (ecc > 0.99) return(-1); 

  // Inclination
  double inc = atan2(sqrt(w[0]*w[0]+w[1]*w[1]), w[2]);

  // Raan
  double node[3]; 
  static double z[3] = {0,0,1}; 
  cross(z, w, node);
  // nearly equatorial
  if (sin(inc) < 1.e-10) {
    node[0] = 1; 
    node[1] = 0;
    node[2] = 0;
  }
  double raan = atan2(node[1], node[0]);


  // Eccentricity (already computed)
  
  // Mean anomaly
  double esinE = pv / sqrt(mu * sma);
  double ecosE = r * (V*V) / mu - 1;

  double E = atan2(esinE, ecosE);
  double M = E - esinE; 

  // Argument of perigee
  double cos_alphav = dot(pos, node);
  double wnode[3]; 
  cross(w, node, wnode);
  double sin_alphav = dot(pos, wnode) / W;
  double alphav = atan2(sin_alphav, cos_alphav);
  double v = E2v(ecc,E);
  double argp = alphav - v;

  // nearly circular
  if (ecc < 1.e-10) {
    M = M + argp;
    argp = 0; 
  }

  kep[0] = sma; 
  kep[1] = ecc; 
  kep[2] = inc; 
  kep[3] = argp; 
  kep[4] = raan; 
  kep[5] = M; 

  return(0); 
}