// orbit - Program to compute the orbit of a comet.


/***************************************************************
                -- Code to use gnuplot --
Include these lines of code at the top of your c or c++ program
to use gnuplot for graphical output.  Call open_gnupipe(), then
use gnuplot("command string") to generate plots.  Finally call
close_gnupipe() before exiting the program.  This routine
assumes that your path includes the gnuplot program.
                    (Dr. Mark C. Fair)
***************************************************************/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
FILE *gnu_pipe;
void open_gnupipe(){
    if (( gnu_pipe = popen("gnuplot -persist","w")) == NULL){
        perror("popen failed");
        exit(1); }
}
void gnuplot(char *cmdstr){
    char cmdstr1[strlen(cmdstr)+1];
    sprintf(cmdstr1,"%s\n",cmdstr);
    fputs(cmdstr1,gnu_pipe);
    fflush(gnu_pipe);
}
void close_gnupipe(){
    if (pclose(gnu_pipe) == -1)
        printf("Problem closing communication to Gnuplot\n");
}
/***************************************************************
                -- End of code to use gnuplot --
***************************************************************/


 #include <iostream>
 #include <fstream>
 #include <cmath>
 #include <cassert>


 using std::cout;
 using std::cin;
 using std::endl;
 using std::ofstream;


void gravrk(double x[],double t,double param[],double deriv[]);
void rk4( double x[],int nx,double t,double tau,
    void (*derivsRK)(double x[],double t,double param[],double deriv[]), double param[]);


 int main()

 {

    double r0,theta0;

    cout << "enter in the initial height(cm): ";
    cin >> r0;
    cout << "enter in the initial angle(rads): ";
    cin >> theta0;


    double state[5],r[3],v[3];
    r[1] = r0; r[2] = theta0; v[1] = 0; v[2] = 0;
    state[1] = r[1]; state[2]= r[2]; state[3] = v[1]; state[4] = v[2];
    int nState = 4;

    double pi = 3.14159265;
    double time = 0;
    double mass = .5;
    double I = 1e-4;
    double k = 5;
    double delta = 1e-3;
    double e = 1e-2;

    double param[6];
    param[1] = mass;
    param[2] = I;
    param[3] = k;
    param[4] = delta;
    param[5] = e;



    double tau = .01;
    int nStep = 4000;

    double *tplot,*rplot,*thplot;
    tplot = new double[nStep+1];
    rplot = new double[nStep+1];
    thplot = new double[nStep+1];


    for(int iStep = 1; iStep <= nStep; ++iStep){

            rplot[iStep] = r[1];               // Record position for plotting
            thplot[iStep] = r[2];
        tplot[iStep] = time;

        rk4( state, nState, time, tau, gravrk, param );
        r[1] = state[1]; r[2] = state[2];   // 4th order Runge-Kutta
        v[1] = state[3]; v[2] = state[4];
        time += tau;

    }

    ofstream fout("12.txt");

    for(int i = 1; i <= nStep; ++i)
    {
        fout << 100*rplot[i]<< " " ;
        fout << thplot[i] << " " ;
        fout << tplot[i] << endl;

    }

    fout.close();

    open_gnupipe();
    gnuplot((char *)"set term x11 3 font 'Times,14'; \
            set title 'wilberforce pendulum' font 'Times,18'; \
            set grid; \
            set xlabel 'Time(s)' font 'Times,18'; \
            set ylabel 'z(cm) theta(radians)' font 'Times,18'; \
            plot '12.txt' using 3:1 with lines title 'Z(t)'; \
            replot '12.txt' using 3:2 with lines title 'theta(t)';\
            set term postscript eps color 'Times' 18; \
            set output '12.eps'; \
            replot");
    close_gnupipe();


    delete [] tplot,rplot,thplot;

    return 0;
 }


void gravrk(double x[], double t, double param[], double deriv[]) {
//  Returns right-hand side of Kepler ODE; used by Runge-Kutta routines
//  Inputs
//    x      State vector [r(1) r(2) v(1) v(2)]
//    t      Time (not used)
//    param     mass, I, k, delta, e
//  Output
//    deriv  Derivatives [dr(1)/dt dr(2)/dt dv(1)/dt dv(2)/dt]

  //* Compute acceleration
  double mass = param[0], I = param[1], k = param[2], delta = param[3], e = param[4];
  double r1 = x[1], r2 = x[2];     // Unravel the vector s into
  double v1 = x[3], v2 = x[4];     // position and velocity

  double accel1 = (-k*r1/mass) - (e*r2*.5/mass);  // Gravitational acceleration
  double accel2 = (-delta*r2/I) - (e*r1*.5/I);


  //* Return derivatives [dr[1]/dt dr[2]/dt dv[1]/dt dv[2]/dt]
  deriv[1] = v1;       deriv[2] = v2;
  deriv[3] = accel1;   deriv[4] = accel2;
}

void rk4(double x[], int nX, double t, double tau,
  void (*derivsRK)(double x[], double t, double param[], double deriv[]),
  double param[]) {
// Runge-Kutta integrator (4th order)
// Inputs
//   x          Current value of dependent variable
//   nX         Number of elements in dependent variable x
//   t          Independent variable (usually time)
//   tau        Step size (usually time step)
//   derivsRK   Right hand side of the ODE; derivsRK is the
//              name of the function which returns dx/dt
//              Calling format derivsRK(x,t,param,dxdt).
//   param      Extra parameters passed to derivsRK
// Output
//   x          New value of x after a step of size tau

  double *F1, *F2, *F3, *F4, *xtemp;
  F1 = new double [nX+1];  F2 = new double [nX+1];
  F3 = new double [nX+1];  F4 = new double [nX+1];
  xtemp = new double [nX+1];

  //* Evaluate F1 = f(x,t).
  (*derivsRK)( x, t, param, F1 );

  //* Evaluate F2 = f( x+tau*F1/2, t+tau/2 ).
  double half_tau = 0.5*tau;
  double t_half = t + half_tau;

  for(int i=1; i<= nX; ++i)
    xtemp[i] = x[i] + half_tau*F1[i];
  (*derivsRK)( xtemp, t_half, param, F2 );

  //* Evaluate F3 = f( x+tau*F2/2, t+tau/2 ).
  for(int i=1; i<= nX; ++i)
    xtemp[i] = x[i] + half_tau*F2[i];
  (*derivsRK)( xtemp, t_half, param, F3 );

  //* Evaluate F4 = f( x+tau*F3, t+tau ).
  double t_full = t + tau;
  for(int i=1; i<= nX; ++i)
    xtemp[i] = x[i] + tau*F3[i];
  (*derivsRK)( xtemp, t_full, param, F4 );

  //* Return x(t+tau) computed from fourth-order R-K.
  for(int i=1; i<= nX; ++i){
    x[i] += tau/6.*(F1[i] + F4[i] + 2.*(F2[i]+F3[i]));
    //cout << "x[i]: " << x[i] << endl;
    }

  delete [] F1, F2, F3, F4, xtemp;
}