/*              ----- basics_cel_mech.c -----
   The present file contains some basic functions that are useful
   for implementing computational methods commonly used in Celestial
   Mechanics.
   The functions included in the present file have been written
   following the indications reported in the file
                      orb_el_informal_notes.pdf
   (from now on, PDF), so the comments would refer to some contents of
   that file.
*/


#define EPSTOLERANCE 2.e-15   //  The value of EPSTOLERANCE gives the maximal
                              //  error that is tolered, in the determination
                              //  of some quantities during the calculation
                              //  (1) of the keplerian motion,
                              //  (2) of the orbital elements from position
                              //      and velocity
                              //  and (3) vice versa

    /* REMARK: It is convenient to fix the value of EPSTOLERANCE so
               that it is about one order of magnitude greater than
               the "machine epsilon" for the floating point arithmetics
	       in double precision.
	       Let us recall that such a value of machine epsilon is about
	                        2.2e-16
	       On one hand, a too large value of EPSTOLERANCE would
	       produce a loss of precision; on the other hand, a too
	       small value of EPSTOLERANCE could prevent the code to
	       end its execution, because some functions could
	       indefinitely loop, due to the fact that the wanted
	       precision is too demanding. */


/* Prototypes of the functions */
double bisec_newton(double mean, double ecc);
void cross(double x[], double y[], double z[]);
void kepl_motion(double x[], double p[],
		 double gamma, double fic_mass, double dt);
void matrix_vector(double M[][3], double v[], double x[]);
double mod(double x[]);
void orbel_el2xv(double a, double ecc, double cos_i, double sin_i,
		 double cos_om, double sin_om, double cos_w, double sin_w,
		 double mean, double gamma,
		 double x[], double xdot[]);
void orbel_xv2el(double x[], double xdot[], double gamma,
                 double *a, double *ecc, double *cos_i, double *sin_i,
                 double *cos_om, double *sin_om, double *cos_w,
                 double *sin_w, double *mean);
double scalar(double x[], double y[]);
int sign(double x);


/* ### */
/* bisec_newton(M,ecc): it computes the solution of M = E - ecc*sin(E)
                        by combining the bisection and the Newton
                        numerical method, as explained in pages 22-23
                        of PDF. */
double bisec_newton(mean, ecc)
   double mean, ecc;
{
  double Emin, Eplus, Emed, half_diff, delta_Emed, f_Emin, f_Eplus, f_Emed;

  // printf("Entry bisec_newton\n");
  Emin = mean - ecc;
  Eplus = mean + ecc;
  f_Emin = Emin - ecc * sin(Emin) - mean;
  f_Eplus = Eplus - ecc * sin(Eplus) - mean;
  
  // Check of the necessary condition for the bisection method
  if ((f_Emin * f_Eplus)>=0) {
    if (fabs(f_Emin)<EPSTOLERANCE || fabs(f_Eplus)<(EPSTOLERANCE)){
      if(fabs(f_Emin)<fabs(f_Eplus))
	Emed = Emin;
      else
	Emed = Eplus;
    }
    else{
      printf("The necessary condition for the bisection method is not satisfied.\n");
      exit(1);
    }
  }
  else {
    // Execution of the mixed method
    half_diff = (Eplus - Emin) * 0.5;
    do {
      if (half_diff < EPSTOLERANCE) {
	Emed = (Emin + Eplus) * 0.5;
        // printf("Exit bisec_newton\n");
	return Emed;
	break;
      }
      Emed = (Emin + Eplus) * 0.5;
      f_Emed = Emed - ecc * sin(Emed) - mean;
      if(f_Emed == 0.)
	return Emed;
      if ((f_Emed * f_Emin)<0)
	Eplus = Emed;
      else
	Emin = Emed;
      half_diff = (Eplus - Emin) * 0.5;
    } while((ecc * half_diff/(1-ecc))>=1);
    
    Emed = (Eplus + Emin) * 0.5;
  }

  // Execution of the Newton method
  do {
    delta_Emed = - (Emed - ecc * sin(Emed) - mean)/(1 - ecc * cos(Emed));
    Emed += delta_Emed;
  } while(fabs(delta_Emed) >= EPSTOLERANCE &&
	  (Emed == 0. || fabs(delta_Emed/Emed) >= EPSTOLERANCE) );

  // printf("Exit bisec_newton\n");
  return Emed;
}


/* ### */
/* cross(x,y,z): it assigns to z the result of the cross product between
                 x and y */
void cross(x,y,z)
   double x[3], y[3], 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];
}


/* ### */
/*
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                 KEPL_MOTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     PURPOSE:  Given the cartesian coordinates and the related momenta 
       compute the same quantities after a given interval of time.
       The trajectory is calculated for a Keplerian orbit corresponding
       to a body having a given mass fic_mass (that can be fictitious,
       according to the considered problem) and attracted toward the
       center by a force equal to gamma*fic_mass/r^2, where r is the
       euclidean norm of the coordinates and gamma is a given constant

       input:
            x[0],x[1],x[2]    ==>  position of the object
            p[0],p[1],p[2]    ==>  linear momentum of the object
            gamma             ==>  constant appearing in the expression
                                   of the gravitational force
            fic_mass          ==>  mass of the object
            dt                ==>  interval of time of the integration
            EPSTOLERANCE      ==>  error that is tolered, in the determination
	                           of some quantities during the calculation
				   of the keplerian motion
				   REMARK: EPSTOLERANCE does not explicitly
				   appear among the arguments of the function
				   because its value is fixed (by a "#define"
				   statement) at the very beginning of the
				   present file

       Output:
            x[0],x[1],x[2]    ==>  position of the object
                                   (after dt time)
            p[0],p[1],p[2]    ==>  linear momentum of the object
                                   (after dt time)

     GENARAL REMARKS: the present computational algorithm is complete
                      for elliptic motions only.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*/
void kepl_motion(x, p, gamma, fic_mass, dt)
   double x[3], p[3];
   double gamma, fic_mass, dt;
{
   int i=0;
   double a, ecc, cos_i, sin_i;
   double cos_om, sin_om, cos_w, sin_w;
   double mean_0, mean_dt;
   double xdot[3];

   // printf("Entry kepl_motion\n");

   // NOTE: orbel_xv2el and its sibling orbel_el2xv have been written
   //       considering velocities:
   //       we would so need to perform a change p-->v before and v-->p
   //       after calling them.

   // Change p --> v
   for(i=0;i<3;i++)
     xdot[i] = p[i] / fic_mass;
   // Change (x,p)(0) ---> orbital elements at time t=0
   orbel_xv2el(x, xdot, gamma, &a, &ecc, &cos_i, &sin_i,
	       &cos_om, &sin_om, &cos_w, &sin_w, &mean_0);
   // Evolution of M according to formula (19) of PDF
   mean_dt = sqrt(gamma / (a*a*a)) * dt + mean_0;
   //Change orbital elements at time t=dt ---> (x,p)(dt)
   orbel_el2xv(a, ecc, cos_i, sin_i, cos_om, sin_om, cos_w, sin_w,
	       mean_dt, gamma, x, xdot);
   // Change v --> p
   for(i=0;i<3;i++)
      p[i] = xdot[i] * fic_mass;
   // printf("Exit kepl_motion\n");
}


/* ### */
/* matrix_vector(M,v,x): it assigns to x the product between the
                         matrix M and the vector v.*/
void matrix_vector(M, v, x)
   double M[3][3], v[3], x[3];
{
   int i, j;
   i=0;
   j=0;

   for(i=0;i<3;i++) {
      x[i] = 0.;
      for(j=0;j<3;j++)
      x[i] += M[i][j] * v[j];
  }
}


/* ### */
/* mod: it evaluates the module of a three-dimensional vector */
double mod(x)
   double x[3];
{
   return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
}


/* ### */
/* orbel_el2xv: function whose aim is to perform the change of coordinates
                (a,e,i,Omega,omega,M) ---> (x,v).
   In order to avoid possible misunderstandings, the velocity v
   (recall that vector v is also commonly used to denote Jacobi
   coordinates) is replaced by xdot.
*/
void orbel_el2xv(a, ecc, cos_i, sin_i, cos_om, sin_om,
		 cos_w, sin_w, mean, gamma, x, xdot)
   double a, ecc, cos_i, sin_i, cos_om, sin_om;
   double cos_w, sin_w, mean, gamma;
   double x[3], xdot[3];
{
   double E, cos_E, sin_E, X, Y, rho, beta, Xdot, Ydot;
   double R[3][3], coord[3], velocities[3];
   int i;
   i = 0;

   // Computation of E solving the implicit equation mean = E - sin(E)
   E = bisec_newton(mean,ecc);

   // I will use formulas (21)-(25) of PDF
   X = a * (cos(E) - ecc);
   Y = a * sin(E) * sqrt(1 - ecc*ecc);
   rho = a * (1 - ecc*cos(E));
   beta = sqrt(gamma/pow(a,3));
   Xdot = - beta * a * a * sin(E) / rho;
   Ydot = beta * a * a * cos(E) * sqrt(1 - ecc *ecc) / rho;

   coord[0] = X;
   coord[1] = Y;
   coord[2] = 0;
   velocities[0] = Xdot;
   velocities[1] = Ydot;
   velocities[2] = 0;

   // Computation of the rotation matrix (30)

   R[0][0] = cos_om * cos_w - sin_om * sin_w * cos_i;
   R[1][0] = sin_om * cos_w + cos_om * cos_i * sin_w;
   R[2][0] = sin_i * sin_w;
   R[0][1] = - cos_om * sin_w - sin_om * cos_i * cos_w;
   R[1][1]= - sin_om * sin_w + cos_om * cos_w * cos_i;
   R[2][1] = sin_i * cos_w;
   for(i=0;i<3;i++)
      R[i][2] = 0;

   matrix_vector(R, coord, x);
   matrix_vector(R, velocities, xdot);
}


/* ### */
/* orbel_xv2el: function whose aim is to perform the change of
                coordinates (x,v) --->(a,e,i,Omega,omega,M).
   Also here, in order to avoid possible misunderstandings, the
   velocity v (recall that vector v is also used to denote Jacobi
   coordinates) is replaced by xdot.
*/
void orbel_xv2el(x, xdot, gamma, a, ecc, cos_i, sin_i,
       cos_om, sin_om, cos_w, sin_w, mean)
   double x[3], xdot[3];
   double gamma, *a, *ecc, *cos_i, *sin_i;
   double *cos_om, *sin_om, *sin_w, *cos_w, *mean;
{
   int i;
   double rho, nrg, modJ, modn, modu, modA;
   double J[3], k[3], n[3], u[3], nxu[3], A[3], ex[3], support[3];
   double E, cos_E, sin_E;

   k[0]=0;
   k[1]=0;
   k[2]=1;

   /* Computation of rho, nrg and J using formulas (1)-(3) of PDF */
   rho = mod(x);
   nrg = (0.5 * scalar(xdot,xdot)) - (gamma/rho);
   cross(x,xdot,J);
   modJ = mod(J);

   // printf("cos_w=%24.16le\n", nrg);
   
   /* Check of the consistency of the initial conditions */
   if (nrg>=0) {
      printf("The energy of the current Keplerian subsystem is greater\n");
      printf("or equal to zero   --   It is not possible to proceed!!!\n");
      exit(1);
   }
   if (modJ==0) {
      printf("The angular momentum (of the current Keplerian subsystem\n");
      printf("is equal to zero   --   It is not possible to proceed!!!\n");
      exit(1);
   }

   /* Computation of a, ecc, cos_i and sin_i using formulas
      (4)-(7) of PDF */
   *a = - gamma/(2.*nrg);
   *ecc = sqrt(1 - scalar(J,J)/(gamma*(*a)));
   *cos_i = J[2]/modJ;
   *sin_i = sqrt(J[0]*J[0] + J[1]*J[1])/modJ;

   /* Computation of the remaining orbital elements */
   if (fabs(*sin_i)>EPSTOLERANCE){
      // I will use (8)-(10)
      cross(k,J,n);
      modn = mod(n);
      for(i=0;i<3;i++)
         n[i] = n[i]/modn;
      // Computation of a vector u such that scalar(n,u)=0
      // scalar(u,u)=1 and cross(n,u,nxu) with nxu parallel
      // with J and scalar(nxu,J)>0
      cross(J,n,u);
      modu = mod(u);
      for(i=0;i<3;i++)
	 u[i] = u[i]/modu;
      cross(n,u,nxu);
      if(scalar(nxu,J)<0)
	 for(i=0;i<3;i++)
	    u[i] *= -1;
   }
   else {
      // I will use (8a)-(10a)
      n[0] = 1.;
      n[1] = 0.;
      n[2] = 0.;
      u[0] = 0.;
      u[1] = 1.;
      u[2] = 0.;
   }
   *cos_om = n[0];
   *sin_om = n[1];

   // Computation of the Laplace-Runge-Lenz vector A
   cross(xdot,J,A);
   for(i=0;i<3;i++)
      A[i] -= (gamma/rho)*x[i];

   if (*ecc>EPSTOLERANCE) {
      modA = mod(A);
      // I will use (12)-(18)
      for(i=0;i<3;i++)
         ex[i] = A[i] / modA;
      // printf("n[0]=%le  n[1]=%le  n[2]=%le\n", n[0], n[1], n[2]);
      // printf("ex[0]=%le  ex[1]=%le  ex[2]=%le\n", ex[0], ex[1], ex[2]);
      *cos_w = scalar(n,ex);
      *sin_w = sqrt(1 - pow(*cos_w,2) );
      if(scalar(u,ex) < 0)
	*sin_w *= -1; 
      // printf("cos_w=%24.16le  sin_w=%24.16le\n", *cos_w, *sin_w);

      cos_E = (1. - (rho/(*a)))/(*ecc);
      if (fabs(cos_E)>1) {
         if(cos_E>0)
            cos_E = 1;
         else
            cos_E = -1;
      }
      cross(ex,x,support);
      E = sign(scalar(x,xdot)) * acos(cos_E);
      *mean = E - (*ecc) * sin(E);
   }
   else {
      // I will use (12a)-(18a)
      for(i=0;i<3;i++)
         ex[i] = n[i];
      *cos_w = 1;
      *sin_w = 0;
      cos_E = scalar(ex,x)/rho;
      if (fabs(cos_E)>1) {
         if(cos_E>0)
            cos_E = 1;
         else
            cos_E = -1;
      }
      cross(ex,x,support);
      *mean = sign(scalar(support,J)) * acos(cos_E);
   }
}


/* ### */
/* scalar(x,y): it returns the scalar product between x and y */
double scalar(x,y)
   double x[3], y[3];
{
   int i;
   double result;

   result = 0.;
   i = 0;
   for(i=0;i<=2;i++)
      result += x[i]*y[i];
   return result;
}


/* ### */
/* sign(x): it returns the sign of x */
int sign(x)
   double x;
{
   if(x>0)
      return 1;
   else if(x==0)
      return 1;
   else
      return -1;
}