// Aero/Plasmabraking Version 1.0
// Ryan Stillwater
// Computational Advanced Propulsion Lab
// Aerospace Engineering Department
// Virginia Polytechnic Institute and State University
// 22 April 2004
//
// This program takes data provided by the user in two (2) txt files.
// The first one is labeled Spacecraft.txt, which contains the neccessary
// information about the spacecraft and its orbit (see example). The 
// second one is labeled Planet.txt, which contains the neccessary 
// information about the body being orbited around and its atmosphere
// (see example).
//
// This program takes the input parameters specified and performs an 
// aerobraking model and outputs the data in a file labled Output.txt.
// The program uses the small angle approximation to determine the 
// position and velocity of the spacecraft while outside of the atmosphere.
// This is done because the no force solution is already defined and for
// speed purposes. The program uses Encke's Method and Runge-Kutta Fourth 
// Order methods to determine the position and velocity of the spacecraft 
// while inside the atmosphere.
// 
// The radius and velocity orbital vectors are output in the x-y plane
// in the file rvector.txt. The file is in the form Rx Ry Vx Vy.


#include <iostream>
#include <fstream>
#include <math.h>
#include <string>
#include <stdlib.h>
#include <iomanip>
#include <time.h>

#define PI 3.141592653589793

using namespace std;

// Define a 3 dimensional vector to make everything smoother
class vector {
private:
	double x,y,z;
public:
	void set( double, double, double );
	double mag();
	double i();
	double j();
	double k();
	vector operator+ (vector);
	vector operator- (vector);
	vector operator* (vector);
	vector operator/ (vector);

friend vector operator* (vector, double);
friend vector operator* (double, vector);
friend vector operator* (vector, int);
friend vector operator* (int, vector);
friend vector operator/ (vector, double);
friend vector operator/ (vector, int);
};

vector vector::operator + (vector alpha) // Defining how to add a vector to a vector
{
	vector out;
	out.x=x+alpha.x;
	out.y=y+alpha.y;
	out.z=z+alpha.z;
	return (out);
}

vector vector::operator - (vector alpha) // Defining how to subtract a vector from a vector
{
	vector out;
	out.x=x-alpha.x;
	out.y=y-alpha.y;
	out.z=z-alpha.z;
	return (out);
}

vector vector::operator * (vector alpha) // Defining how to multiply a vector by a vector
{
	vector out;
	out.x=x*alpha.x;
	out.y=y*alpha.y;
	out.z=z*alpha.z;
	return (out);
}

vector vector::operator / (vector alpha) // Defining how to divide a vector by a vector
{
	vector out;
	out.x=x/alpha.x;
	out.y=y/alpha.y;
	out.z=z/alpha.z;
	return (out);
}

vector operator* (vector alpha, double beta) // Defining how to multiply a vector by a double
{
	vector out;
	out.x=alpha.x*beta;
	out.y=alpha.y*beta;
	out.z=alpha.z*beta;
	return (out);
}

vector operator* (double beta, vector alpha)  // Defining how to multiply a double by a vector
{
	vector out;
	out.x=alpha.x*beta;
	out.y=alpha.y*beta;
	out.z=alpha.z*beta;
	return (out);
}

vector operator* (vector alpha, int beta)  // Defining how to multiply a vector by an int
{
	vector out;
	out.x=alpha.x*(double)beta;
	out.y=alpha.y*(double)beta;
	out.z=alpha.z*(double)beta;
	return (out);
}

vector operator* (int beta, vector alpha) // Defining how to multiply an int by a vector
{
	vector out;
	out.x=alpha.x*(double)beta;
	out.y=alpha.y*(double)beta;
	out.z=alpha.z*(double)beta;
	return (out);
}

vector operator/ (vector alpha, double beta) // Defining how to divide a vector by a double
{
	vector out;
	out.x=alpha.x/beta;
	out.y=alpha.y/beta;
	out.z=alpha.z/beta;
	return (out);
}

vector operator/ (vector alpha, int beta) // Defining how to divide a vector by an int
{
	vector out;
	out.x=alpha.x/(double)beta;
	out.y=alpha.y/(double)beta;
	out.z=alpha.z/(double)beta;
	return (out);
}

void vector :: set(double a, double b, double c) // Defining how to input values into the vector
{
	x=a;
	y=b;
	z=c;
}

double vector :: i() // Defining how to get the (1) component
{
	return x;
}

double vector :: j() // Defining how to get the (2) component
{
	return y;
}

double vector :: k() // Defining how to get the (3) component
{
	return z;
}

double vector :: mag() // Defining how to get the magnitude of the vector
{
	return sqrt(x*x+y*y+z*z);
}

double dot(vector, vector);
vector cross(vector,vector);
void orbitdet(vector, vector, double, double&, double&, double&);
void rvfoft(vector&, vector&, double, double, double&);
void rvdet(vector&, vector&, double, double, double, double);
vector drdt_t(vector, vector, double, vector, vector, double, double);
vector dr_t(vector, vector, double, vector, vector, double, double);

int main()
{
	string test;
	const string check;
	double rp,ra; // initial periapsis and apoapsis altitudes
	double rpmin,rafinal; // lowest allowable periapsis and apoapsis to end program at
	double Mcraft,Acraft; // mass and cross-sectional area of spacecraft
	double Cd; // drag coefficient of spacecraft
	double phi; // electrical potential of spacecraft
	double dt; // time step for inside atmosphere calculations

	cout.precision(8); // To prevent me from jumping to conclusions
	ifstream planet("planet.txt"); // Load the planetary data file 
	ifstream spacecraft("spacecraft.txt"); // Load the spacecraft data file

	// Loading Spacecraft Data
	while(!spacecraft.eof())
	{
		spacecraft >> test;
		if(test=="Mass")
			spacecraft >> test >> Mcraft;
		if(test=="Area")
			spacecraft >> test >> Acraft;
		if(test=="Drag,")
			spacecraft >> test >> Cd;
		if(test=="Phi")
			spacecraft >> test >> phi;
		if(test=="Time")
			spacecraft >> test >> test >> dt;
		if(test=="Apoapsis")
			spacecraft >> test >> test >> ra;
		if(test=="Periapsis")
			spacecraft >> test >> test >> rp;
		if(test=="Minimum")
			spacecraft >> test >> test >> test >> rpmin;
		if(test=="Final")
			spacecraft >> test >> test >> test >> rafinal;
	}

	double RPlanet, muPlanet; // radius and gravitational parameter of the planet
	int numions=0; // number of ion species
	int numneut=0; // number of neutral species
	double mions[40], mneut[40]; // masses of ion and neutral species
	int ionaltlev, neutaltlev; // number of altitude levels for ions and neutrals
	double ndions[100][40], ndneut[100][40]; // number densities of ions and neutrals
	double altitude[100]; // altitude 

	// Load Atmospheric Data
	while(!planet.eof())
	{
		planet >> test;
		if(test=="Radius")
			planet >> test >> RPlanet;
		if(test=="mu")
			planet >> test >> muPlanet;
		if(test=="Ions")
		{
			planet >> test;
			while(test !="Mass") // determine the number of ion species listed
			{
				planet >> test;
				numions++;
			}
			numions--;
			for(int i=0; i < numions; i++) // load ion masses
			{
				planet >> mions[i];
			}
			planet >> test >> test >> ionaltlev;
			planet >> test >> test >> test >> test >> test;
			for(int i=0; i < ionaltlev; i++) // load ion number density data
			{
				planet >> altitude[i];
				for(int j=0; j < numions; j++)
					planet >> ndions[i][j];
			}
		}
		if(test=="Neutrals")
		{
			planet >> test;
			while(test !="Mass")// determine the number of neutrals species listed
			{
				planet >> test;
				numneut++;
			}
			numneut--;
			for(int i=0; i < numneut; i++) // load neutral masses
				planet >> mneut[i];
			planet >> test >> test >> neutaltlev;
			planet >> test >> test >> test >> test >> test;
			for(int i=0; i < neutaltlev; i++) // load neutrals number density data
			{
				planet >> altitude[i];
				for(int j=0; j < numneut; j++)
					planet >> ndneut[i][j];
			}
			if(neutaltlev < ionaltlev) // fill in zeros as place holders
			{
				for(int i=neutaltlev-1; i < ionaltlev; i++)
				{
					for(int j=0; j < numneut; j++)
						ndneut[i][j]=0.0;
				}
			}
		}
	}
	
	double avgmions[100],avgmneut[100]; // average mass of the ions and neutrals
	double totndions[100],totndneut[100]; // total number density of the ions and neutrals
	double densityions[100],densityneut[100]; // density of the ions and neutrals
	double NA=6.022e23; // Avagadro's Number

	// Calculate the ion density versus altitude
	for(int i=0; i < ionaltlev; i++)
	{
		densityions[i]=0;
		totndions[i]=0;
		for(int j=0; j < numions; j++)
		{
			densityions[i]=densityions[i]+ndions[i][j]*mions[j];
			totndions[i]=totndions[i]+ndions[i][j];
		}
		avgmions[i]=densityions[i]/totndions[i]/1000/NA;
		densityions[i]=densityions[i]*100*100*100/1000/NA;
		totndions[i]=totndions[i]*100*100*100;
	}
	// Calculate the neutral density versus altitude
	for(int i=0; i < ionaltlev; i++)
	{
		densityneut[i]=0;
		totndneut[i]=0;
		for(int j=0; j < numneut; j++)
		{
			densityneut[i]=densityneut[i]+ndneut[i][j]*mneut[j];
			totndneut[i]=totndneut[i]+ndneut[i][j];
		}
		avgmneut[i]=densityneut[i]/totndneut[i]/1000/NA;
		densityneut[i]=densityneut[i]*100*100*100/1000/NA;
		totndneut[i]=totndneut[i]*100*100*100;
	}

	double ecc=1.6022e-19; // defining the charge on an alectron in Coulombs
	double dnu=PI/180.0; // defining 1 degree angle changes outside of atmosphere
	
	RPlanet=RPlanet*1000; // converting planet radius km to m
	ra=ra*1000+RPlanet; // converting altitude km to radius m
	rp=rp*1000+RPlanet; // converting altitude km to radius m
	double rpreset=rp; // periapsis radius to reset to incase it gets too low
	rafinal=rafinal*1000+RPlanet; // converting altitude km to radius m
	rpmin=rpmin*1000+RPlanet; // converting altitude km to radius m
	double brakingalt=altitude[ionaltlev-1]*1000+RPlanet; // converting ionosphere edge altitude km to radius m
	muPlanet=muPlanet*1e9; // converting km^3/s^2 to m^3/s^2
	
	vector R,V; // true radius and velocity vectors
	vector P,Pdot; // osculation radius and velocity vectors
	vector drnew,drdotnew,drold,drdotold; // difference between true and asculation orbits
	vector dv; // delta-v burn just in case the periapsis drops too low 
	double e, p, nu, a, Energy; // Orbital parameters (eccentricity, parameter, true anomaly, semi-major axis, energy)
	double E1,E2; // eccentric anomalies
	double vp, va; // periapsis and apoapsis velocities
	double vanew; // helps with the delta-v burn calculation
	double nu2,nu2old, nu2output; // keeps track of true anomaly when in atmosphere
	int orbit=0; // number of orbits completed
	int lower; // number of the highest altitude that is still less than the radius
	double fraction; // fraction representing how close to lower is the radius 

	R.set(ra,0.0,0.0); // initiating radius vector
	a=0.5*(ra+rp); // calculating semi-major axis
	Energy=-muPlanet/2/a; // calculating orbital energy
	va=sqrt(2*(Energy+muPlanet/ra)); // calculating initial apoapsis velocity
	V.set(0,va,0); // initializing velocity vector
	orbitdet(R,V,muPlanet,p,e,nu); // determine remaining orbital parameters
	double accelions, accelnuet, acceltot; // accelerations resulting from atmosphere
	double densityi, densityn; // local density of the neutrals and ions

	ofstream rvout("rvector.txt"); // initialize file for radius and velocity output in x-y plane
	ofstream output("output.txt"); // initialize main output file
	output << setw(6) << "Orbit" << setw(15) << "Time (days)";
	output << setw(15) << "Apoapsis (km)" << setw(15) << "Velocity (m/s)";
	output << setw(15) << "Periapsis (km)" << setw(15) << "Velocity (m/s)" << endl;
	output << fixed << setprecision(3);
	rvout << fixed << setprecision(8);
	cout << "Apoapsis Altitude (km)" << endl;

	drold.set(0.0,0.0,0.0); // initialize vector
	drdotold.set(0.0,0.0,0.0); // initialize vector
	double time=0.0; // Orbit time accumulated
	double dragseconds; // count seconds inside atmosphere
	double dvburn=0.0; // accumulated dv burns
	int dvburncount=0; // count number of dv burns
	double rpcheck;

	// Begin main program
	while (ra > rafinal)
	{
		while(R.mag()>brakingalt && ra > rafinal)
		{
			nu=nu+dnu; // step the true anomaly 
			rvdet(R,V,muPlanet,p,e,nu); // get new radius and velocity vectors
			rvout << R.i() << " " << R.j() << " " << V.i() << " " << V.j() << endl; // output to file
			if (nu < PI+dnu/2.0 && nu > PI-dnu/2.0) // check to see if orbit is at apoapsis
			{
				ra=R.mag();
				va=V.mag();
				Energy=va*va/2-muPlanet/ra; // calculate current orbital energy
				rpcheck=-1.0*muPlanet/Energy-ra; // calculate approximate periapsis radius on next pass
				orbit++;
				a=p/(1-e*e);
				time=time+sqrt(a*a*a/muPlanet)*(PI-(E1-e*sin(E1))); // adding time since leaving atmosphere

				output << setw(6) <<  orbit << setw(15) <<  time/3600/24;
				output << setw(15) <<  (ra-RPlanet)/1000 << setw(15) <<  va;
				output << setw(15) <<  (rp-RPlanet)/1000 << setw(15) <<  vp << endl;

				if(rpcheck < rpmin) // if periapsis radius is too low perform dv burn
				{
					Energy=-1.0*muPlanet/(ra+rpreset); // calculate energy with higher periapsis radius
					vanew=sqrt(2.0*(Energy+muPlanet/ra)); // calculate resulting apoapsis velocity
					dv.set(0,va-vanew,0); // calculate delta v needed
					V=V+dv; // correct velocity
					orbitdet(R,V,muPlanet,p,e,nu); // calculate new orbital parameters
					dvburn=dvburn+va-vanew; // accumulate delta v used
					dvburncount++; // count delta v burns
				}
			}
		}

		E2=2.0*PI-acos((e+cos(nu))/(1+e*cos(nu)));
		a=p/(1-e*e);
		time=time+sqrt(a*a*a/muPlanet)*(E2-e*sin(E2)-PI); // Adding time since periapsis

		// Reset values to begin aerobraking 
		P=R;
		Pdot=V;
		drold.set(0,0,0);
		drdotold.set(0,0,0);
		dragseconds=0.0;
		nu2=nu;
		nu2output=nu+dnu;

		// Begin aerobraking program
		while(R.mag()<=brakingalt && ra > rafinal)
		{
			nu2old=nu2;
			dragseconds=dragseconds+dt; // calculate the seconds spent inside the atmosphere
			rvfoft(P,Pdot,dt,muPlanet, nu2); // calculate the no force change of radius and velocity over dt
			
			for(int i=0; i < ionaltlev; i++) // calculate the altitude step right below the radius for 1-D PIC 
				if((R.mag()-RPlanet)/1000 > altitude[i])
					lower=i;
            fraction=(R.mag()-(RPlanet+altitude[lower]*1000))/(altitude[lower+1]-altitude[lower])/1000; // calculate how close radius is to lower altitude
			
			densityn=fraction*(densityneut[lower+1])+(1-fraction)*(densityneut[lower]); // calculate local neutral density 
			densityi=fraction*(densityions[lower+1])+(1-fraction)*(densityions[lower]); // calculate local ion density 
			
			accelnuet=-0.5*V.mag()*V.mag()*Cd*Acraft*densityn/Mcraft; // calculate acceleration from neutrals
			accelions=-0.5*Cd*Acraft*densityi*(V.mag()*V.mag()+2*ecc*phi/(fraction*avgmions[lower+1]+(1-fraction)*avgmions[lower])); // calculate acceleration from ions
			acceltot=accelnuet+accelions; // calculate total acceleration

			drdotnew=drdt_t(R,V,acceltot,drdotold,drold,dt,muPlanet); // calculate velocity change
			drnew=dr_t(R,V,acceltot,drdotold,drold,dt,muPlanet); // calculate radius change

            R=P+drnew; // calculate new radius
			V=Pdot+drdotnew; // calculate new velocity
			drold=drnew;
			drdotold=drdotnew;

			if(fabs(drnew.mag()/P.mag()) > 0.00005 && nu2 > PI/360.0) // if radius change is too large reset the osculation orbit
			{
				P=R;
				Pdot=V;
				drold.set(0,0,0);
				drdotold.set(0,0,0);
			}

			if (fabs(nu2old-nu2) > PI) // save periapsis information
			{
				rp=R.mag();
				vp=V.mag();
			}

			if (nu2 >= PI && nu2old < PI) // save apoapsis information
			{
				ra=R.mag();
				va=V.mag();
				orbit++;
				time=time+dragseconds;
				dragseconds=0.0;

				output << setw(6) <<  orbit << setw(15) << time/3600/24;
				output << setw(15) <<  (ra-RPlanet)/1000 << setw(15) << va;
				output << setw(15) <<  (rp-RPlanet)/1000 << setw(15) << vp << endl;
			}
			if(nu2 >= nu2output && nu2old < nu2output)
			{
				nu2output=nu2output+dnu;
                rvout << R.i() << " " << R.j() << " " << V.i() << " " << V.j() << endl; // output to file
			}
		}
		orbitdet(R,V,muPlanet,p,e,nu); // calculate new orbit parameters
		time=time+dragseconds; // adding time inside atmosphere
		E1=acos((e+cos(nu))/(1+e*cos(nu)));
		cout << (ra-RPlanet)/1000.0 << endl; // output apoapsis radius so I know its working
	}
	cout << "Total number of orbits: " << orbit <<endl;
	cout << "Total number of delta v burns: " << dvburncount << endl;
	cout << "Total delta v required (m/s): " << dvburn << endl;
	cout << "The program required " << clock()/CLOCKS_PER_SEC/60 << " minutes to run." << endl;

	// Close output files
	rvout.close();
	output.close();

	return 0;
}

// Determine the orbital parameters given a radius and velocity vector 
void orbitdet(vector R, vector V, double mu, double& p, double& e, double& nu)
{
	vector H,E;
	double h,r,v,rdotv;
	H=cross(R, V); // determine angular momentum vector
	rdotv=dot(R,V);
	r=R.mag();
	v=V.mag();
	h=H.mag();
	p=h*h/mu; // determine parameter
	E=1/mu*((v*v-mu/r)*R-rdotv*V); // determine eccentricity vector
	e=E.mag();
	if(R.j() < 0)
		nu=2.0*acos(-1)-acos(dot(E,R)/(e*r)); // true anomaly if it is greater than 180 degrees
	else
		nu=acos(dot(E,R)/(e*r)); // true anomaly if it is less than 180 degrees
}

// Determine the change in radius and velocity vectors given a timestep
void rvfoft(vector& R, vector& V, double dt, double mu, double& nuout)
{
	double p, e, nu, E, Eo, a, error1, error2, E1, E2, nu1, nu2, M, Mo, n, nuold;
	double maxerror=1e-10;
	
	orbitdet(R, V, mu, p, e, nu); // get orbital parameters
	nuold=nu;
	if (R.j() < 0)
		dt=-1.0*dt; // if Eo is really greater than pi it is easier to solve this way

	Eo=acos((e+cos(nu))/(1+e*cos(nu))); // calculate initial eccentric anomaly
	a=p/(1-e*e); // calculate semi-major axis
	n=sqrt(mu/(a*a*a)); // calculate mean motion
	Mo=Eo-e*sin(Eo); // calculate initial mean anomaly
	M=dt*n+Mo; // calculate new mean anomaly

	E2=Eo+PI/180.0; // set initial guess for new eccentric anomaly
	E1=Eo;// set initial guess for new eccentric anomaly

	// Calculate new eccentric anomaly using the secant method
	error2=1.0; // reset error
	while(fabs(error2) > maxerror)
	{
		error1=M-(E1-e*sin(E1));
		error2=M-(E2-e*sin(E2));
		E=E1-error1*(E2-E1)/(error2-error1);
		E1=E2;
		E2=E;
	}
	if(E > 2.0*PI)  // error check
		E=E-2*PI; 
	
	// Calculate new true anomally
	error2=1.0; // reset error
	// Using bisection method if close to 0 or 180 degrees
	if(nuold >=359.0*PI/180.0 || fabs(nuold-PI) <= PI/180.0 )
	{
		if(E > 0 && fabs(E-Eo) > maxerror) // initial conditions adding dt doesn't cross zero degrees
		{
			nu1=2.0*PI-PI/180.0;
			nu2=2.0*PI;
		}
		if(E <= 0 && fabs(E-Eo) > maxerror) // initial conditions adding dt does cross zero degrees
		{
			nu2=PI/180.0;
			nu1=0.0;
		}
		if(fabs(E-Eo) <= maxerror) // removing encountered anomaly
		{
			error2=maxerror;
			nu=nuold;
		}
		if(E >= 179.0*PI/180.0 && E <=181.0*PI/180.0) // initial conditions if close to 180 degrees
		{
			nu1=179.0*PI/180.0;
			nu2=181.0*PI/180.0;
		}
		// Performing bisection method
		while(fabs(error2) > maxerror)
		{
			error1=(e+cos(nu1))/(1+e*cos(nu1))-cos(E);
			error2=(e+cos((nu2+nu1)/2.0))/(1+e*cos((nu2+nu1)/2.0))-cos(E);
			if(error1*error2 < 0)
				nu2=(nu2+nu1)/2.0;
			else
				nu1=(nu2+nu1)/2.0;
			nu=nu2;
			if(fabs(error1-error2) < maxerror) // removing encountered anomaly
			{
				error2=maxerror;
			}

		}
	} 
	else // every other angle uses the secant method
	{

		nu2=nuold+PI/360.0;	// standard initial conditions
		nu1=nuold;
		
		// Secant method
		while(fabs(error2) > maxerror)
		{
			error1=(e+cos(nu1))/(1+e*cos(nu1))-cos(E);
			error2=(e+cos(nu2))/(1+e*cos(nu2))-cos(E);
			nu=nu2-error2*(nu2-nu1)/(error2-error1);
			nu1=nu2;
			nu2=nu;
		}
		nu=nu2;
		if(nu > nuold+PI/45.0) // to make sure it doesn't diverge
		{
			nu1=nuold;
			nu2=nuold+PI/360.0;
		}
	}
    if (nu > 2.0*PI) // error check
		nu=nu-2.0*PI;
	if(nuold < PI/45.0 && nu > 2.0*PI-PI/45.0)
		nu=2.0*PI-nu;
	
	rvdet(R, V, mu, p, e, nu); // get new radius and velocity vectors for new nu
	nuout=nu;
}

// Determine the radius and velocity vectors in the x-y plane given orbital parameters
void rvdet(vector& R, vector& V, double mu, double p, double e, double nu)
{
	double r=p/(1+e*cos(nu));
	R.set(r*cos(nu),r*sin(nu),0);
	V.set(sqrt(mu/p)*-1*sin(nu),sqrt(mu/p)*(e+cos(nu)),0);
}

// Calculate the change in velocity over a time step using a Runge-Kutta fourth order method 
vector drdt_t(vector R, vector V, double A, vector drdot, vector dr, double dt, double mu)
{
	vector drdotnew,P,Pdot,Pnew,Pdotnew,Rnew,k1,k2,k3,k4,Ap;
	double p,r,nu;
	Ap=V/V.mag()*A; // turn acceleration magnitude into a vector
	P=R-dr; // determine old osculation radius
	Pdot=V-drdot; // determine old osculation velocity
	p=P.mag();
	r=R.mag();
	k1=dt*(Ap-mu/(p*p*p)*((1-p*p*p/(r*r*r))*R-dr)); // calculate k1
	Pnew=P;
	Pdotnew=Pdot;
	rvfoft(Pnew,Pdotnew,dt/2.0,mu,nu); // determine new osculation radius and velocity at half timestep
	Rnew=Pnew+dr+k1/2.0; // determine new radius using k1
	p=Pnew.mag();
	r=Rnew.mag();
	k2=dt*(Ap-mu/(p*p*p)*((1-p*p*p/(r*r*r))*Rnew-dr-k1/2.0)); // calculate k2
	Rnew=Pnew+dr+k2/2.0; // determine new radius using k2
	r=Rnew.mag();
	k3=dt*(Ap-mu/(p*p*p)*((1-p*p*p/(r*r*r))*Rnew-dr-k2/2.0)); // calculate k3
	Pnew=P;
	Pdotnew=Pdot;
	rvfoft(Pnew,Pdotnew,dt,mu,nu); // determine new osculation radius and velocity at whole timestep
	Rnew=Pnew+dr+k3; // determine new radius using k3
	p=Pnew.mag();
	r=Rnew.mag();
	k4=dt*(Ap-mu/(p*p*p)*((1-p*p*p/(r*r*r))*Rnew-dr-k3)); // calculate k4
	drdotnew=drdot+(k1+2.0*k2+2.0*k3+k4)/6.0; // calculate total velocity change over timestep
	return drdotnew;
}

// Calculate the change in radius over a time step using a Runge-Kutta fourth order method 
vector dr_t(vector R, vector V, double A, vector drdot, vector dr, double dt, double mu)
{
	vector drnew,k1,k2,k3,k4;
	k1=dt*(drdt_t(R,V,A,drdot,dr,0.0,mu)); // calculate k1
	k2=dt*(drdt_t(R,V,A,drdot,dr+k1/2.0,dt/2.0,mu)); // calculate k2
	k3=dt*(drdt_t(R,V,A,drdot,dr+k2/2.0,dt/2.0,mu)); // calculate k3
	k4=dt*(drdt_t(R,V,A,drdot,dr+k3,dt,mu)); // calculate k4
	drnew=dr+(k1+2.0*k2+2.0*k3+k4)/6.0; // calculate total radius change over timestep
	return drnew;
}

// Define the dot product of two vectors
double dot(vector a, vector b)
{
	return (a.i()*b.i()+a.j()*b.j()+a.k()*b.k());
}

// Define the cross product of two vectors
vector cross(vector a, vector b)
{
	vector out;
	double x,y,z;
	x=a.j()*b.k()-a.k()*b.j();
	y=-1*(a.i()*b.k()-a.k()*b.i());
	z=a.i()*b.j()-a.j()*b.i();
	out.set(x,y,z);
	return (out);
}