/* Main program for numerical integration of the solar system.
 * See the readme file for discussion.
 *
 * Before compiling, set type of computer in mconf.h,
 * arithmetic precision in prec.h, and model parameters
 * in ssystem.h.
 *
 * If DE200 compatible outputs are desired, include the
 * rotation r118_200() in the output routine.
 */

#include "mconf.h"
#include "prec.h"
#include "int.h"
#include "ssystem.h"

/* Include data for the initial states and model parameters.
 */
#if DE102
#include "ini102.h"
#endif
#if DE118
#include "ini118.h"
#endif
#if DE200
#include "ini200.h"
#endif

static DOUBLE vnewt[6*NTOTAL];
DOUBLE *awork; /* Adams integrator work array */
DOUBLE *rwork; /* Runge-Kutta work array */
DOUBLE Rij[NTOTAL][NTOTAL];

DOUBLE SQRT();

static int fd;
static char fname[80];

#if IBMPC
#include <stdio.h>
#include <sys\types.h>
#include <sys\stat.h>
#include <fcntl.h>
#endif

main()
{
DOUBLE t, e0, e, err, h, ccor;
static DOUBLE xa, ya, za, xb, yb, zb; /* for display */
DOUBLE *pdv;
int i, ii, j, ord, outsteps, iout;
long isamps, nsamps, nsteps;
DOUBLE adstep();

orlup:
printf( "Adams order ? " );
scanf( "%d", &ord );
if( (ord > MAXORD) || (ord < 1) )
	{
	printf( "order must be between 1 and %d\n", MAXORD );
	goto orlup;
	}

printf( "step size, days ? " );
#if LDOUBLE
scanf( "%Lf", &h );
#else
scanf( "%lf", &h );
#endif

/* Each output file record contains the Julian date
 * followed by the current velocity and position states.
 * See ini118.h for the data structure of the state array.
 * The state variables for a test object such as a comet
 * or asteroid should be placed in the array immediately after
 * the Sun state.
 */
printf( "Output file name ? " );
scanf( "%s", fname );
#if IBMPC
fd = open( fname, O_BINARY | O_CREAT | O_RDWR, S_IREAD | S_IWRITE );
#else
fd = creat( fname );
#endif
if( fd <= 0 )
	{
	printf( "cant create \"%s\"\n", fname );
	exit(1);
	}
printf( "Interval between output samples, in steps ? " );
scanf( "%d", &outsteps );
printf( "Number of output samples ? " );
scanf( "%ld", &nsamps );

/* Note, it is not necessary to clear out the work arrays. */
i = ((NEQ+1)*(MAXORD+2)+(6*NEQ))*sizeof(DOUBLE);
awork = (DOUBLE *)malloc(i);
if( awork == 0 )
	{
	printf( "malloc(awork) failed\n" );
	exit(0);
	}
printf( "Allocated %d bytes to awork\n", i );

i = (MAXRUNG+2)*NEQ*sizeof(DOUBLE);
rwork = (DOUBLE *)malloc(i);
if( rwork == 0 )
	{
	printf( "malloc(rwork) failed\n" );
	exit(0);
	}
printf( "Allocated %d bytes to rwork\n", i );

printf( "NEQ %d, NEQC %d, 6*NTOTAL %d, IAROIDS %d, NBODY %d, NTOTAL %d\n",
   NEQ, NEQC, 6*NTOTAL, IAROIDS, NBODY, NTOTAL );

printf( "initializing...\n" );

/* Ensure that the relativistic center of the N bodies is at 0.0.
 * This condition will already be satisfied by states taken from the
 * JPL ephemeris tapes.
 */
/*
findcenter( JD0, yn );
findcenter( JD1, yn1 );
*/

t = JD0;
err = 0.0;
rkstart( NEQ, rwork );
adstart( h, &yn[0], awork, NEQ, ord, t );
printf( "initialized.\n" );

wfile( t, yn ); /* see below */

nsteps = 0;
for( isamps=0; isamps<nsamps; isamps++ )
	{
	for( iout=0; iout<outsteps; iout++ )
		{
		err += adstep( &t, &yn[0], NEQC );
		nsteps += 1;
		}
	wfile( t, yn );
#if LDOUBLE
	printf( "%5ld %11.2Lf %.6Le\n", nsteps, t, err );
#else
	printf( "%5ld %11.2lf %.6e\n", nsteps, t, err );
#endif
	}
close( fd );

/* Test if the ending time is equal to the time of
 * the compiled-in test state vector. If so, print out
 * the errors.
 */
xa = t - JD1;
if( xa < 0 )
	xa = -xa;
 /* allow for roundoff in the time variable */
/*
if( xa > 1e-6 )
	exit(0);
*/
printf( "Final x, y, z, positions and errors:\n" );
/*
ii = 6*FMASS;
for(i=FMASS; i<IAROIDS; i++)
*/

ii = 0;
for(i=0; i<=ISUN; i++)

	{
/* Compare Heliocentric positions
	xa = yn[ii+1] - yn[6*ISUN+1];
	ya = yn[ii+3] - yn[6*ISUN+3];
	za = yn[ii+5] - yn[6*ISUN+5];
	xb = yn1[ii+1] - yn1[6*ISUN+1];
	yb = yn1[ii+3] - yn1[6*ISUN+3];
	zb = yn1[ii+5] - yn1[6*ISUN+5];
*/
	xa = yn[ii+1];
	ya = yn[ii+3];
	za = yn[ii+5];
	xb = yn1[ii+1];
	yb = yn1[ii+3];
	zb = yn1[ii+5];

if( i == 0 )
	{
#if LDOUBLE
	printf("%19.15Lf%19.15Lf%19.15Lf\n",
#else
	printf("%19.15lf%19.15lf%19.15lf\n",
#endif
		yn[0], yn[2], yn[4] );
	}
#if LDOUBLE
	printf("%19.15Lf%19.15Lf%19.15Lf",
#else
	printf("%19.15lf%19.15lf%19.15lf",
#endif
		xa, ya, za );

	xa = xa - xb;
	ya = ya - yb;
	za = za - zb;
	err = SQRT( xa*xa + ya*ya + za*za );
#if LDOUBLE
	printf(" %.3Le\n",
#else
	printf(" %.3e\n",
#endif
		err );

	ii += 6;
	}
} /* end of main program */



/* Subroutine func() calculates the forces and accelerations.
 * Data in the output vector v[] are in the order
 * d^2x/dt^2, dx/dt, d^2y/dt^2, dy/dt, d^2z/dt^2, dz/dt
 * for each object.  For this problem the velocities dx/dt, ...
 * are simply copied over from the input array y[].
 */

#if MOON
static DOUBLE y[6*NTOTAL] = {0.0};
static DOUBLE ymoon[6];
#endif

func( t, yin, v )
DOUBLE t; /* time */
DOUBLE yin[]; /* input state: velocity and position */
DOUBLE v[]; /* output: calculated acceleration, copy of input velocity */
{
DOUBLE xs, ys, zs, xd, yd, zd, xv, yv, zv, r, temp;
int i, j, k, ii, jj;


#if MOON
/* Locally replace input variable EMB by barycentric earth
 * and input variable Moon-Earth by barycentric Moon.
 */
fromemb( yin, y );
#endif

/* asteroid positions */
#if AROIDS
aroids( t, y );
#endif


/* Table of distances between objects i and j */
distances(y);

fixsun( y, v );
#if MOON
for( j=0; j<6; j++ )
	yin[(6*ISUN)+j] = y[(6*ISUN)+j];
#endif

/* Compute Newtonian acceleration. */
ii = 6*FMASS;
for( i=FMASS; i<NTOTAL; i++ )
	{
	xs = 0.0;
	ys = 0.0;
	zs = 0.0;
	xv = y[ii+1];
	yv = y[ii+3];
	zv = y[ii+5];
	jj = 6*FMASS;
	for( j=FMASS; j<NTOTAL; j++ )
		{
		if( j != i )
			{
			xd = y[jj+1] - xv;
			yd = y[jj+3] - yv;
			zd = y[jj+5] - zv;
			r = Rij[i][j];
			temp = GMs[j]/(r * r * r);
			xs += xd * temp;
			ys += yd * temp;
			zs += zd * temp;
			}
		jj += 6;
		}
	vnewt[ii] = xs; /* acceleration */
	vnewt[ii+2] = ys;
	vnewt[ii+4] = zs;
	vnewt[ii+1] = y[ii]; /* velocity */
	vnewt[ii+3] = y[ii+2];
	vnewt[ii+5] = y[ii+4];
	ii += 6;
	}

#if DOREL
reltiv( y, v );
#else
/* No relativity theory. Return the Newtonian accelerations. */
ii = 6*FMASS;
for( i=FMASS; i<IAROIDS; i++ )
	{
	v[ii] = vnewt[ii];
	v[ii+2] = vnewt[ii+2];
	v[ii+4] = vnewt[ii+4];
	v[ii+1] = y[ii];
	v[ii+3] = y[ii+2];
	v[ii+5] = y[ii+4];
	ii += 6;
	}
#endif


#if MOON
oblate( t, y, v, ymoon );
/* Convert barycentric Earth and Moon to output EMB and M variables. */
ii = 6*IEARTH;
jj = 6*IMOON;
for( i=0; i<6; i += 2 )
	{
	xd = v[ii+i]; /* Earth */
	yd = v[jj+i]; /* Moon */
	v[ii+i] = (EMRAT * xd + yd)/(EMRAT+1.0); /* EMB */
	v[jj+i] = yd - xd; /* M = Moon - Earth */
	v[ii+i+1] = yin[ii+i]; /* copy original velocity */
	v[jj+i+1] = yin[jj+i];

/*
#if LDOUBLE
	printf( "%.3Le %.3Le %.3Le\n",
#else
	printf( "%.3e %.3e %.3e\n",
#endif
 v[ii+i]-aearth[i/2], v[jj+i]-amoon[i/2], v[i]-alibr[i/2] );
*/
	}
/*
exit(0);
*/
#endif /* MOON */
}

#if DOREL
/* Relativistic corrections.  Reference:
 *
 * Newhall, XX, E. M. Standish, and J. G. Williams, "DE102: a
 * numerically integrated ephemeris of the Moon and planets
 * spanning forty-four centuries," Astronomy and Astrophysics
 * 125, 150-167 (1983).
 */
	
reltiv( y, v )
DOUBLE y[], v[];
{
DOUBLE xd, yd, zd, xs, ys, zs, xv, yv, zv, vi, csqi;
DOUBLE rc, r, s, temp;

int i, j, k, ii, jj;

csqi = 1.0/(C*C);
ii = 6*FMASS;
for(i=FMASS; i<IAROIDS; i++)
	{
	v[ii] = 0.0;
	v[ii+2] = 0.0;
	v[ii+4] = 0.0;

	xv = y[ii]; /* velocity */
	yv = y[ii+2];
	zv = y[ii+4];
	vi = xv*xv + yv*yv + zv*zv;

	xs = 0.0;
	ys = 0.0;
	zs = 0.0;
	jj = 6*FMASS;
	for(j=FMASS; j<NTOTAL; j++)
		{
		if( j == i )
			{
			jj += 6;
			continue; /* skip to next j if i = j */
			}

		xd = y[jj+1] - y[ii+1];
		yd = y[jj+3] - y[ii+3];
		zd = y[jj+5] - y[ii+5];

		s = 0.0;
		for(k=FMASS; k<NTOTAL; k++)
			{
			if( k == i )
				continue;
			s += GMs[k]/Rij[i][k];
			}
		rc = -4.0 * csqi * s;

		s = 0.0;
		for(k=FMASS; k<NTOTAL; k++)
			{
			if(k ==j )
				continue;
			s += GMs[k]/Rij[j][k];
			}
		rc -= csqi * s;

		rc += vi * csqi;

		xv = y[jj];
		yv = y[jj+2];
		zv = y[jj+4];

		r = xv * xv + yv * yv + zv * zv;
		rc += 2.0 * csqi * r;

		r = y[ii] * y[jj] + y[ii+2] * y[jj+2] + y[ii+4] * y[jj+4];
		rc -= 4.0 * csqi * r;

		s = -xd * y[jj] - yd * y[jj+2] - zd * y[jj+4];
		s /= Rij[i][j];
		rc -= 1.5 * csqi * s * s;

		s = xd * vnewt[jj] + yd * vnewt[jj+2] + zd * vnewt[jj+4];
		rc += 0.5 * csqi * s;

		r = Rij[i][j];
		temp = GMs[j]/(r * r * r);

		rc = temp * (1.0 + rc );
		xs += xd * rc;
		ys += yd * rc;
		zs += zd * rc;

		s = -xd * (4.0*y[ii] - 3.0*y[jj])
			- yd * (4.0*y[ii+2] - 3.0*y[jj+2])
			- zd * (4.0*y[ii+4] - 3.0*y[jj+4]);
		s = s * temp * csqi;
		xs += s * (y[ii] - y[jj]);
		ys += s * (y[ii+2] - y[jj+2]);
		zs += s * (y[ii+4] - y[jj+4]);

		temp = 3.5 * csqi * GMs[j] / r;
		xs += temp * vnewt[jj];
		ys += temp * vnewt[jj+2];
		zs += temp * vnewt[jj+4];
		jj += 6;
		}
	v[ii] = xs;
	v[ii+2] = ys;
	v[ii+4] = zs;
	v[ii+1] = y[ii];
	v[ii+3] = y[ii+2];
	v[ii+5] = y[ii+4];
	ii += 6;
	}
}
#endif

/* Program to calculate positions of the asteroids:
 */
#if AROIDS
#include "jplmp.c"
#endif

/* Constrain the barycenter to stay at the origin.
 * This is done by adjusting the Sun.
 */

fixsun( y, v )
DOUBLE y[], v[];
{
DOUBLE xx, yy, zz, vx, vy, vz;
DOUBLE csqi, s;
static DOUBLE mustar[NTOTAL];
int i, j, k, ii, jj;


csqi = 0.5 / (C*C);
for( k=0; k<2; k++ )
 { /* Iterate to find solution of implicit expressions. */


/* Relativistic GM of each body */
ii = 6*FMASS;
for( i=FMASS; i<NTOTAL; i++ )
	{
	vx = y[ii]; /* velocity */
	vy = y[ii+2];
	vz = y[ii+4];
	s = vx * vx + vy * vy + vz * vz; /* square of velocity */
	for( j=FMASS; j<NTOTAL; j++ )
		{
		if( j == i )
			continue;
		s -= GMs[j]/Rij[i][j];
		}
	mustar[i] = GMs[i] * (1.0 + csqi * s);
	ii += 6;
	}
/* Compute center of mass with Sun omitted. */
xx = 0.0;
yy = 0.0;
zz = 0.0;
vx = 0.0;
vy = 0.0;
vz = 0.0;
ii = 6*FMASS;
for( i=FMASS; i<NTOTAL; i++ )
	{
	if( i != ISUN )
		{
		s = mustar[i];
		xx += y[ii+1] * s; /* position */
		yy += y[ii+3] * s;
		zz += y[ii+5] * s;
		vx += y[ii] * s; /* velocity */
		vy += y[ii+2] * s;
		vz += y[ii+4] * s;
		}
	ii += 6;
	}
/* Evaluate the Sun so that the center = 0. */
s = mustar[ISUN];
xx = -xx/s;
yy = -yy/s;
zz = -zz/s;
vx = -vx/s;
vy = -vy/s;
vz = -vz/s;
jj = 6*ISUN;
y[jj+1] = xx;
y[jj+3] = yy;
y[jj+5] = zz;
y[jj] = vx;
y[jj+2] = vy;
y[jj+4] = vz;
v[jj+1] = vx;
v[jj+3] = vy;
v[jj+5] = vz;
/* Adjust the table of distances between bodies i and j.
 * Note, most of this work was already done by func().
 * Only the entries involving the Sun need to be changed.
 */
ii = 6*FMASS;
for( j=FMASS; j<NTOTAL; j++ )
	{
	if( j != ISUN )
		{
		vx = xx - y[ii+1];
		vy = yy - y[ii+3];
		vz = zz - y[ii+5];
		vx = SQRT( vx*vx + vy*vy + vz*vz );
		Rij[ISUN][j] = vx;
		Rij[j][ISUN] = vx;
		}
	ii += 6;
	}
 } /* iteration */

}



/* This routine adds a constant vector to move the
 * barycenter to the origin.
 */
findcenter( t, yin )
DOUBLE t;
DOUBLE yin[];
{
int i, j, k, ii, jj;
static DOUBLE xx, yy, zz, vx, vy, vz, ax, ay, az, csqi, s, mu;
static DOUBLE mustar[NTOTAL];

#if MOON
fromemb( yin, y );
#endif

#if AROIDS
aroids( t, y );
#endif

csqi = 0.5 / (C*C);
for( k=0; k<2; k++ )
{ /* Iterate to find solution of implicit expressions. */

distances(y);

#if DOREL
/* Relativistic GM of each body */
ii = 6*FMASS;
for( i=FMASS; i<NTOTAL; i++ )
	{
	vx = y[ii]; /* velocity */
	vy = y[ii+2];
	vz = y[ii+4];
	s = vx * vx + vy * vy + vz * vz; /* square of velocity */
	for( j=FMASS; j<NTOTAL; j++ )
		{
		if( j == i )
			continue;
		s -= GMs[j]/Rij[i][j];
		}
	mustar[i] = GMs[i] * (1.0 + csqi * s);
	mu += s;
	ii += 6;
	}
#endif

/* Compute relativistic (or Newtonian) center of mass. */
mu = 0.0; /* total mass of the system */
xx = 0.0;
yy = 0.0;
zz = 0.0;
vx = 0.0;
vy = 0.0;
vz = 0.0;
ii = 6*FMASS;
for( i=FMASS; i<NTOTAL; i++ )
	{
#if DOREL
	s = mustar[i];
#else
	s = GMs[i];
#endif
	mu += s;
	xx += y[ii+1] * s; /* position */
	yy += y[ii+3] * s;
	zz += y[ii+5] * s;
	vx += y[ii] * s; /* velocity */
	vy += y[ii+2] * s;
	vz += y[ii+4] * s;
	ii += 6;
	}
/* Offset the coordinates of all the objects equally. */
xx /= mu;
yy /= mu;
zz /= mu;
vx /= mu;
vy /= mu;
vz /= mu;
jj = 6*FMASS;
for( i=FMASS; i<NTOTAL; i++ )
	{
	y[jj+1] -= xx;
	y[jj+3] -= yy;
	y[jj+5] -= zz;
	y[jj] -= vx;
	y[jj+2] -= vy;
	y[jj+4] -= vz;
	jj += 6;
	}
if( k == 0 )
	{
	xx = SQRT( xx*xx + yy*yy + zz*zz );
	vx = SQRT( vx*vx + vy*vy + vz*vz );
#if LDOUBLE
	printf( "Barycenter offset = %.5Le au, %.5Le au/d\n", xx, vx );
#else
	printf( "Barycenter offset = %.5e au, %.5e au/d\n", xx, vx );
#endif
	}
}

#if MOON
jj = 6*FMASS;
for( i=FMASS; i<NTOTAL; i++ )
	{
	if( i == IMOON )
		jj += 6;
	else
		{
		for( j=0; j<6; j++ )
			{
			yin[jj] = y[jj];
			jj += 1;
			}
		}
	}
/* Convert barycentric Earth and Moon to output EMB variables. */
ii = 6*IEARTH;
jj = 6*IMOON;
for( i=0; i<6; i++ )
	{
	xx = y[ii+i]; /* SS barycentric Earth */
	yy = y[jj+i]; /* SS barycentric Moon */
	yin[ii+i] = (EMRAT * xx + yy)/(EMRAT+1.0); /* EMB */
/*	yin[jj+i] = yy - xx; */ /* M = Moon - Earth */
	yin[jj+i] = ymoon[i];
	}
#endif
}



#if MOON
/* Convert EMB, M to barycentric Earth and Moon vectors.
 * ymoon[] declared externally.
 */
fromemb( yinp, y )
DOUBLE yinp[], y[];
{
DOUBLE zd, yd;
int i, ii, jj;

for( i=0; i<(6*NTOTAL); i++ )
	y[i] = yinp[i];

for( i=0; i<6; i++ )
	ymoon[i] = yinp[6*IMOON+i]; /* M */

ii = 6*IEARTH;
jj = 6*IMOON;
for( i=0; i<6; i++ )
	{
	zd = yinp[ii+i];		/* EMB */
	yd = yinp[jj+i]/(1.0+EMRAT);	/* M */
	y[ii+i] = zd -  yd;		/* r_e */
	y[jj+i] = zd + EMRAT * yd;	/* r_m */
	}
}
#endif




/* Make table of distances between ith and jth objects
 */
distances(y)
DOUBLE y[];
{
DOUBLE r, xv, yv, zv, xd, yd, zd;
int j, i, jj, ii;

jj = 6*(FMASS+1);
for(j=(FMASS+1); j<NTOTAL; j++)
	{
	ii = 6*FMASS;
	xv = y[jj+1];
	yv = y[jj+3];
	zv = y[jj+5];
	for(i=FMASS; i<j; i++)
		{
		xd = xv - y[ii+1];
		yd = yv - y[ii+3];
		zd = zv - y[ii+5];
		r = SQRT(xd*xd + yd*yd + zd*zd);
		Rij[j][i] = r;
		Rij[i][j]  = r;
		ii += 6;
		}
	jj += 6;
	}
#if MOON
/* Take the input M vector for distance from Earth to Moon.
 * ymoon[] set by previous call to fromemb().
 */
xd = ymoon[1];
yd = ymoon[3];
zd = ymoon[5];
r = SQRT( xd*xd + yd*yd + zd*zd );
Rij[IEARTH][IMOON] = r;
Rij[IMOON][IEARTH] = r;
#endif
}


/* Write date and solution vector to output file.
 */
static DOUBLE yout[6*(ISUN+1)]; /* Output array for disc file. */

wfile( t, y )
DOUBLE t;
DOUBLE y[];
{
DOUBLE dt;
DOUBLE p[3], v[3];
int i, j, k;

dt = t;
for( i=0; i<6*(ISUN+1); i++ )
	yout[i] = y[i];

/* Rotate to DE200 coordinate system
 */
for( i=FMASS; i<ISUN+1; i++ )
	{
	k = 6 * i;
	for( j=0; j<3; j++ )
		{
		v[j] = yout[k+j+j];
		p[j] = yout[k+j+j+1];
		}
#if DE118
	r118_200(v);
	r118_200(p);
#endif
	for( j=0; j<3; j++ )
		{
		yout[k+j+j] = v[j];
		yout[k+j+j+1] = p[j];
		}
	}

write( fd, &dt, sizeof(DOUBLE) );
write( fd, yout, 6*(ISUN+1)*sizeof(DOUBLE) );
}