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

#define G 1

/* prototypes of the functions */
void orbel_xv2el(double x[], double xdot[], double gamma, double eps,
                 double *a, double *ecc, double *cos_i, double *sin_i,
                 double *cos_om, double *sin_om, double *cos_w,
                 double *sin_w, double *mean);
void orbel_el2xv(double a, double ecc, double eps, 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 matrix_vector(double M[][3], double v[], double x[]);
double bisec_newton(double mean, double ecc, double eps);
double mod(double x[]);
void cross(double x[], double y[], double z[]);
double scalar(double x[], double y[]);
int sign(double x);

int main() {

   /* DEFINITION OF ALL THE ENTITIES */

   /* General ones */
   double eps;
   eps = 4.e-16;

   /* Motion ones */
   double x_init[6], p_init[6];
   double x_t0[6], p_t0[6];

   /* Sun ones */
   double m_0;

   /* Jupiter ones */
   double m_1, gamma_1;
   // Orbital elements
   double a_1, ecc_1, cos_i_1, sin_i_1;
   double cos_om_1, sin_om_1, cos_w_1, sin_w_1;
   double mean_1;

   /* Saturn ones */
   double m_2, gamma_2;
   // Orbital elements
   double a_2, ecc_2, cos_i_2, sin_i_2;
   double cos_om_2, sin_om_2, cos_w_2, sin_w_2;
   double mean_2;

   /* Operative ones */
   // Counters
   int i, j;
   // Reading file related
   FILE *ifp;
   char line[300];

   /* INITIAL CONDITIONS */

   m_0 = 4. * M_PI * M_PI;
   m_1 = m_0 / 1047.355;
   m_2 = m_0 / 3498.5;

   gamma_1 = G * (m_0 + m_1);
   gamma_2 = G * (m_0 + m_2);

   /* Input of initial data */
   ifp = fopen("init_cond_SJS_xv.txt","r");

   for(i=0;i<12;i++)
      fgets(line,300,ifp);

   // Reading of Jupiter initial data
   printf("Jupiter initial data:\n");
   for(i=0;i<3;i++){
      fgets(line,300,ifp);
      sscanf(line,"%le",&x_t0[i]);
      printf("x_t0[%d] = %24.16le\n", i, x_t0[i]);
   }
   for(i=0;i<3;i++){
      fgets(line,300,ifp);
      sscanf(line,"%le",&p_t0[i]);
      printf("xdot_t0[%d] = %24.16le\n", i, p_t0[i]);
   }
   printf("\n");

   // Reading of Saturn initial data
   printf("Saturn initial data:\n");
   for(i=3;i<6;i++){
      fgets(line,300,ifp);
      sscanf(line,"%le",&x_t0[i]);
      printf("x_t0[%d] = %24.16le\n", i, x_t0[i]);
   }
   for(i=3;i<6;i++){
      fgets(line,300,ifp);
      sscanf(line,"%le",&p_t0[i]);
      printf("xdot_t0[%d] = %24.16le\n", i, p_t0[i]);
   }
   printf("\n");

   /* Computation of the initial orbital elements of Jupiter */
   orbel_xv2el(x_t0,p_t0,gamma_1,eps,&a_1,&ecc_1,&cos_i_1,
      &sin_i_1,&cos_om_1,&sin_om_1,&cos_w_1,&sin_w_1,&mean_1);
   printf("The orbital elements of Jupiter at the initial time are:\n");
   printf("a = %lf, ecc = %lf, i = %lf,\nOmega = %lf, omega = %lf, M = %lf\n\n",
          a_1,ecc_1,atan2(sin_i_1,cos_i_1),atan2(sin_om_1,cos_om_1),
          atan2(sin_w_1,cos_w_1),mean_1);
   /* Computation of the initial orbital elements of Saturn */
   orbel_xv2el(x_t0+3,p_t0+3,gamma_2,eps,&a_2,&ecc_2,&cos_i_2,&sin_i_2,
      &cos_om_2,&sin_om_2,&cos_w_2,&sin_w_2,&mean_2);
   printf("The orbital elements of Saturn at the initial time are:\n");
   printf("a = %lf, ecc = %lf, i = %lf,\nOmega = %lf, omega = %lf, M = %lf\n\n",
          a_2,ecc_2,atan2(sin_i_2,cos_i_2),atan2(sin_om_2,cos_om_2),
          atan2(sin_w_2,cos_w_2),mean_2);

   printf("Just as a check, let us come back to the Jupiter initial data!\n");
   orbel_el2xv(a_1, ecc_1, eps, cos_i_1, sin_i_1, cos_om_1, sin_om_1,
	       cos_w_1, sin_w_1, mean_1, gamma_1, x_t0, p_t0);
   printf("Jupiter initial data:\n");
   for(i=0;i<3;i++)
      printf("x_t0[%d] = %24.16le\n", i, x_t0[i]);
   for(i=0;i<3;i++)
      printf("xdot_t0[%d] = %24.16le\n", i, p_t0[i]);
   printf("Just as a check, let us come back to the Saturn initial data!\n");
   orbel_el2xv(a_2, ecc_2, eps, cos_i_2, sin_i_2, cos_om_2, sin_om_2,
	       cos_w_2, sin_w_2, mean_2, gamma_2, x_t0+3, p_t0+3);
   printf("Saturn initial data:\n");
   for(i=3;i<6;i++)
      printf("x_t0[%d] = %24.16le\n", i, x_t0[i]);
   for(i=3;i<6;i++)
      printf("xdot_t0[%d] = %24.16le\n", i, p_t0[i]);
}




/* orbel_xv2el: function whose aim is to perform the change of
   coordinates (x,v) --->(a,e,i,Omega,omega,M).
   In order to avoid possible misunderstandings, the velocity v
   (recall that vector v is also used to denote Jacobi coordinates)
   is replaced by xdot.
   This function has been written following the indications in
   20141003_meeting.pdf (from now on, PDF), so the comments would
   refer to some contents of that file.*/

void orbel_xv2el(x, xdot, gamma, eps, a, ecc, cos_i, sin_i,
       cos_om, sin_om, cos_w, sin_w, mean)
   double x[3], xdot[3];
   double gamma, eps, *a, *ecc, *cos_i, *sin_i;
   double *cos_om, *sin_om, *sin_w, *cos_w, *mean;
{
   int i;
   double rho, nrg, modJ, modn, modu;
   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);

   /* 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)>eps){
      // 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>eps) {
      // I will use (12)-(18)
      for(i=0;i<3;i++)
         ex[i] = A[i] / (gamma * (*ecc));
      // 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 = scalar(u,ex);
      // printf("cos_w=%le  sin_w=%le\n", *cos_w, *sin_w);


/* NB: having defined cross as a void function requires the use of a
       support vector, as in this case.  Might be not the best
       choice
   ANS: This actually is the best choice! */

      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);
   }
}

/* orbel_el2xv: function whose aim is to perform the change of coordinates
                (a,e,i,Omega,omega,M) ---> (x,v).
      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_el2xv(a, ecc, eps, cos_i, sin_i, cos_om, sin_om,
       cos_w, sin_w, mean, gamma, x, xdot)
   double a, ecc, eps, 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,eps);

   // (21)-(25)
   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);
}


/* 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];
  }
}


/* bisec_newton(M,ecc,eps): it computes the solution of mean = 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, eps)
   double mean, ecc, eps;
{
   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
   // that must hold apart the roundoff errors
   if ((f_Emin * f_Eplus)>=0) {
     if(fabs(f_Emin)<fabs(f_Eplus))
       Emed = Emin;
     else
       Emed = Eplus;
   }
   else {
     // Execution of the bisection method until the Newton method
     // can be safely started
     half_diff = (Eplus - Emin) * 0.5;
     do {
       if (half_diff < eps) {
         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) >= eps &&
	   (Emed == 0. || fabs(delta_Emed/Emed) >= eps) );
   
   // printf("Exit bisec_newton\n");
   return Emed;
}

/* 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]);
}

/* 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];
}

/* 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;
}