/* solves a PAIR of simultaneous 2nd-order ODEs
via 4th-order runge-kutta method */

#include <iostream>
#include <cmath>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>

using namespace std;

// modify f[0], f[1], f[2], f[3] in function func()
// GM is a constant number

int
     func (double t, const double y[], double f[],
           void *params)
     {
       double GM = *(double *)params;
       f[0] = y[1];                    
       f[1] = -GM*y[0]/(pow(y[0]*y[0] + y[2]*y[2],1.5));
       f[2] = y[3];
       f[3] = -GM*y[2]/(pow(y[0]*y[0] + y[2]*y[2],1.5));
       return GSL_SUCCESS;
     }
     
     int
     jac (double t, const double y[], double *dfdy, 
          double dfdt[], void *params)
     {
       double GM = *(double *)params;
       gsl_matrix_view dfdy_mat 
         = gsl_matrix_view_array (dfdy, 4, 4);
       gsl_matrix * m = &dfdy_mat.matrix; 
       gsl_matrix_set (m, 0, 0, 0.0);
       gsl_matrix_set (m, 0, 1, 1.0);
       gsl_matrix_set (m, 0, 2, 0.0);
       gsl_matrix_set (m, 0, 3, 0.0);


       gsl_matrix_set (m, 1, 0, -GM*(-2.0*pow(y[0],2) + pow(y[2],2))/
		       (pow(y[0]*y[0] + y[2]+y[2],2.5)));
       gsl_matrix_set (m, 1, 1, 0.0);
       gsl_matrix_set (m, 1, 2, GM*(3.0*y[0]*y[2])/
		       (pow(y[0]*y[0] + y[2]+y[2],2.5)));
       gsl_matrix_set (m, 1, 3, 0.0);


       gsl_matrix_set (m, 2, 0, 0.0); 
       gsl_matrix_set (m, 2, 1, 0.0); 
       gsl_matrix_set (m, 2, 2, 0.0);
       gsl_matrix_set (m, 2, 3, 1.0);


       gsl_matrix_set (m, 3, 0, GM*(3.0*y[0]*y[2])/
		       (pow(y[0]*y[0] + y[2]+y[2],2.5)));
       gsl_matrix_set (m, 3, 1, 0.0);
       gsl_matrix_set (m, 3, 2, -GM*(-2.0*pow(y[2],2) + pow(y[0],2))/
		       (pow(y[0]*y[0] + y[2]+y[2],2.5)));
       gsl_matrix_set (m, 3, 3, 0.0);
 
       // since df/dt = 0 for all choices of f, these are zero:
       
       dfdt[0] = 0.0;
       dfdt[1] = 0.0;
       dfdt[2] = 0.0;
       dfdt[3] = 0.0;
       return GSL_SUCCESS;
     }
     

int main ()
     {
       const gsl_odeiv_step_type * T 
         = gsl_odeiv_step_rk4;
     
       gsl_odeiv_step * s 
         = gsl_odeiv_step_alloc (T, 4);
     
       double GM = 1.00;               //CHANGE ME. value of G*M_sun
       gsl_odeiv_system sys = {func, jac, 4, &GM}; // GM is ODE dependent
     
       double t = 0.0, t1 = 100.0;   // CHANGE ME. bounds.
       double h = 0.05;              // CHANGE ME. step size

       // initial conditions: y[0], y[2] = position, y[1],y[3] = velocity
       double y[4] = { 0.5, 0.0, 0.0, 1.25 }, y_err[4];
       double dydt_in[4], dydt_out[4];
     
       /* initialise dydt_in from system parameters */
       GSL_ODEIV_FN_EVAL(&sys, t, y, dydt_in);
     
       while (t < t1)
         {
           int status = gsl_odeiv_step_apply (s, t, h, 
                                              y, y_err, 
                                              dydt_in, 
                                              dydt_out, 
                                              &sys);
     
           if (status != GSL_SUCCESS)
               break;
     
           dydt_in[0] = dydt_out[0];
           dydt_in[1] = dydt_out[1];
           dydt_in[2] = dydt_out[2];
           dydt_in[3] = dydt_out[3];
     
           t += h;
	   cout << t << " " << y[0] << " " << y[1] << 
	     " " << y[2] << " " << y[3] << endl; 

	   //           printf ("%.5e %.5e %.5e\n", t, y[0], y[1]);
         }
     
       gsl_odeiv_step_free (s);
       return 0;
     }