/*******************************************************************************
*$ Component_name:
*	TKep_Locate
*$ Abstract:
*	Test program for Kepler Library routine Kep_Locate().
*$ Keywords:
*	KEPLER, ORBITAL_MOTION
*	C, TEST_PROGRAM
*$ Declarations:
*	none
*$ Inputs:
*	none
*$ Outputs:
*	none
*$ Returns:
*	none
*$ Detailed_description:
*	This program tests Kepler Library routine Kep_Locate(), by confirming
*	that positions and velocities are compatible.  It also checks that
*	angular momentum is conserved.
*
*	The user is prompted for the number of gravitational moments to use in
*	the Saturn gravity model, and for the orbital elements of a hypothetical
*	satellite.  Then, the user is prompted repeatedly for a time (in units
*	of orbital period, and prints the following information:
*		(1) The vector position (km).
*		(2) The velocity (km/sec).
*		(3) An approximate velocity, using a numerical derivative of the
*		    position.
*		(4) The satellite's specific angular momentum.
*$ External_references:
*	Kep_SetPlanet(), Kep_SetOrbit(), Kep_MeanAnom(), Kep_Omega(),
*	Kep_Locate()
*$ Examples:
*	In general, (2) and (3) should be equal to roughly one part in 10^6.
*	When the number of gravitational moments is zero, both the vector and
*	scalar values for (4) should be conserved to high precision.  When
*	higher moments are present, only the scalar value should be conserved.
*$ Error_handling:
*	none
*$ Limitations:
*	none
*$ Author_and_institution:
*	Mark R. Showalter
*	NASA/Ames Research Center
*$ Version_and_date:
*	1991 June 18
*$ Change_history:
*	none
*******************************************************************************/
#include <stdio.h>
#include <math.h>
#include "kepler.h"
#include "pi.h"

main()
{
KEP_PLANET	saturn;
static double	rSaturn = 60330.,
		gm = 3.7931200e7,
		Js[] = {16297.e-6, -910.e-6, 107.e-6, -10.e-6, 2.e-6, -0.5e-6};
static int	nJs_max=6;
KEP_ORBIT	orbit;
double		a, e, i, peri, node, trueLon, meanLon, period, t1, t2, dt;
double		loc1[3], loc2[3], vel1[3], vel2[3];
double		cross[3], mag;
int		nJs, status;

/*******************************************************************************
* Establish saturn gravity field
*******************************************************************************/

	printf( "Enter number of gravitational moments: " );
	scanf( "%d", &nJs );
	if (nJs > nJs_max) nJs = nJs_max;

	Kep_SetPlanet( rSaturn, gm, Js, nJs, &saturn );

/*******************************************************************************
* Establish satellite orbit
*******************************************************************************/

	printf( "Enter a, e, i, peri, node, trueLon: " );
	scanf( "%lf,%lf,%lf,%lf,%lf,%lf", &a, &e, &i, &peri, &node, &trueLon );

/* Convert angles to radians */
	i    *= PI/180.;
	peri *= PI/180.;
	node *= PI/180.;
	trueLon *= PI/180.;
	
/* Calculate mean longitude and orbital period */
	meanLon = Kep_MeanAnom(e, trueLon-peri) + peri;
	period = 2.*PI / Kep_Omega(&saturn, a);

	Kep_SetOrbit( a, e, i, peri, node, meanLon, 0., &saturn, &orbit );

while (1) {

/*******************************************************************************
* Get time and step
*******************************************************************************/

	printf( "Enter time in orbits: " );
	status = scanf( "%lf", &t1 );
	if (status == EOF) break;

	t1 = period * t1;
	t2 = t1 + period / 1.e6;
	dt = t2 - t1;

/*******************************************************************************
* Print position and velocity
*******************************************************************************/

	Kep_Locate( &orbit, t1, loc1, vel1 );
	Kep_Locate( &orbit, t2, loc2, vel2 );

	printf( "Location: % .15e  % .15e  % .15e\n", loc1[0],loc1[1],loc1[2] );
	printf( "Velocity: % .15e  % .15e  % .15e\n", vel1[0],vel1[1],vel1[2] );

	vel2[0] = (loc2[0] - loc1[0]) / dt;
	vel2[1] = (loc2[1] - loc1[1]) / dt;
	vel2[2] = (loc2[2] - loc1[2]) / dt;
	printf( "Test vel: % .15e  % .15e  % .15e\n", vel2[0],vel2[1],vel2[2] );

/*******************************************************************************
* Evaluate specific angular momentum
*******************************************************************************/

	cross[0] = loc1[1] * vel1[2] - loc1[2] * vel1[1];
	cross[1] = loc1[2] * vel1[0] - loc1[0] * vel1[2];
	cross[2] = loc1[0] * vel1[1] - loc1[1] * vel1[0];
	printf( "L vector: % .15e  % .15e  % .15e\n",
			cross[0], cross[1], cross[2] );

	mag = sqrt(cross[0]*cross[0] + cross[1]*cross[1] + cross[2]*cross[2]);
	printf( "L length: % .15e\n", mag );
}

	printf( "\n" );
}

/******************************************************************************/