/* -*- linux-c -*- */
/* triple.c

   Copyright (C) 2002-2004 John M. Fregeau

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <getopt.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include "fewbody.h"
#include "triple.h"

/* print the usage */
void print_usage(FILE *stream)
{
	fprintf(stream, "USAGE:\n");
	fprintf(stream, "  triple [options...]\n");
	fprintf(stream, "\n");
	fprintf(stream, "OPTIONS:\n");
	fprintf(stream, "  -m --m000 <m000/MSUN>        : set mass of star 0 of inner binary of triple [%.6g]\n", FB_M000/FB_CONST_MSUN);
	fprintf(stream, "  -n --m001 <m001/MSUN>        : set mass of star 1 of inner binary of triple [%.6g]\n", FB_M001/FB_CONST_MSUN);
	fprintf(stream, "  -o --m01 <m01/MSUN>          : set mass of outer star of triple [%.6g]\n", FB_M01/FB_CONST_MSUN);
	fprintf(stream, "  -r --r000 <r000/RSUN>        : set radius of star 0 of inner binary of triple [%.6g]\n", FB_R000/FB_CONST_RSUN);
	fprintf(stream, "  -g --r001 <r001/RSUN>        : set radius of star 1 of inner binary of triple [%.6g]\n", FB_R001/FB_CONST_RSUN);
	fprintf(stream, "  -i --r01 <r01/RSUN>          : set radius of outer star of triple [%.6g]\n", FB_R01/FB_CONST_RSUN);
	fprintf(stream, "  -a --a00 <a00/AU>            : set inner semimajor axis of triple [%.6g]\n", FB_A00/FB_CONST_AU);
	fprintf(stream, "  -Q --a0 <a0/AU>              : set outer semimajor axis of triple [%.6g]\n", FB_A0/FB_CONST_AU);
	fprintf(stream, "  -e --e00 <e00>               : set inner eccentricity of triple [%.6g]\n", FB_E00);
	fprintf(stream, "  -F --e0 <e0>                 : set outer eccentricity of triple [%.6g]\n", FB_E0);
	fprintf(stream, "  -t --tstop <tstop/t_dyn>     : set stopping time [%.6g]\n", FB_TSTOP);
	fprintf(stream, "  -D --dt <dt/t_dyn>           : set approximate output dt [%.6g]\n", FB_DT);
	fprintf(stream, "  -c --tcpustop <tcpustop/sec> : set cpu stopping time [%.6g]\n", FB_TCPUSTOP);
	fprintf(stream, "  -A --absacc <absacc>         : set integrator's absolute accuracy [%.6g]\n", FB_ABSACC);
	fprintf(stream, "  -R --relacc <relacc>         : set integrator's relative accuracy [%.6g]\n", FB_RELACC);
	fprintf(stream, "  -N --ncount <ncount>         : set number of integration steps between calls\n");
	fprintf(stream, "                                 to fb_classify() [%d]\n", FB_NCOUNT);
	fprintf(stream, "  -z --tidaltol <tidaltol>     : set tidal tolerance [%.6g]\n", FB_TIDALTOL);
	fprintf(stream, "  -x --fexp <f_exp>            : set expansion factor of merger product [%.6g]\n", FB_FEXP);
	fprintf(stream, "  -k --ks                      : turn K-S regularization on or off [%d]\n", FB_KS);
	fprintf(stream, "  -s --seed                    : set random seed [%ld]\n", FB_SEED);
	fprintf(stream, "  -d --debug                   : turn on debugging\n");
	fprintf(stream, "  -V --version                 : print version info\n");
	fprintf(stream, "  -h --help                    : display this help text\n");
}

/* calculate the units used */
int calc_units(fb_obj_t *obj[1], fb_units_t *units)
{
	double m0, m00, m01, m000, m001, a00, a0;

	m0 = obj[0]->m;
	m00 = obj[0]->obj[0]->m;
	m01 = obj[0]->obj[1]->m;
	m000 = obj[0]->obj[0]->obj[0]->m;
	m001 = obj[0]->obj[0]->obj[1]->m;

	a0 = obj[0]->a;
	a00 = obj[0]->obj[0]->a;

	/* Unit of velocity is approximate relative orbital speed of inner binary,
	   unit of length is semimajor axis of inner binary;
	   therefore, unit of time is approximately 1 inner orbital period. */
	units->v = sqrt(FB_CONST_G*(m000+m001)/a00);
	units->l = a00;
	units->t = units->l / units->v;
	units->m = units->l * fb_sqr(units->v) / FB_CONST_G;
	units->E = units->m * fb_sqr(units->v);

	return(0);
}

/* the main attraction */
int main(int argc, char *argv[])
{
	int i, j;
	unsigned long int seed;
	double m000, m001, m01, r000, r001, r01, a00, a0, e00, e0;
	double Ei, Lint[3], Li[3], t;
	fb_hier_t hier;
	fb_input_t input;
	fb_ret_t retval;
	fb_units_t units;
	char string1[FB_MAX_STRING_LENGTH], string2[FB_MAX_STRING_LENGTH];
	gsl_rng *rng;
	const gsl_rng_type *rng_type=gsl_rng_mt19937;
	const char *short_opts = "m:n:o:r:g:i:a:Q:e:F:t:D:c:A:R:N:z:x:k:s:dVh";
	const struct option long_opts[] = {
		{"m000", required_argument, NULL, 'm'},
		{"m001", required_argument, NULL, 'n'},
		{"m01", required_argument, NULL, 'o'},
		{"r000", required_argument, NULL, 'r'},
		{"r001", required_argument, NULL, 'g'},
		{"r01", required_argument, NULL, 'i'},
		{"a00", required_argument, NULL, 'a'},
		{"a0", required_argument, NULL, 'Q'},
		{"e00", required_argument, NULL, 'e'},
		{"e0", required_argument, NULL, 'F'},
		{"tstop", required_argument, NULL, 't'},
		{"dt", required_argument, NULL, 'D'},
		{"tcpustop", required_argument, NULL, 'c'},
		{"absacc", required_argument, NULL, 'A'},
		{"relacc", required_argument, NULL, 'R'},
		{"ncount", required_argument, NULL, 'N'},
		{"tidaltol", required_argument, NULL, 'z'},
		{"fexp", required_argument, NULL, 'x'},
		{"ks", required_argument, NULL, 'k'},
		{"seed", required_argument, NULL, 's'},
		{"debug", no_argument, NULL, 'd'},
		{"version", no_argument, NULL, 'V'},
		{"help", no_argument, NULL, 'h'},
		{NULL, 0, NULL, 0}
	};

	/* set parameters to default values */
	m000 = FB_M000;
	m001 = FB_M001;
	m01 = FB_M01;
	r000 = FB_R000;
	r001 = FB_R001;
	r01 = FB_R01;
	a00 = FB_A00;
	a0 = FB_A0;
	e00 = FB_E00;
	e0 = FB_E0;
	input.ks = FB_KS;
	input.tstop = FB_TSTOP;
	input.Dflag = 0;
	input.dt = FB_DT;
	input.tcpustop = FB_TCPUSTOP;
	input.absacc = FB_ABSACC;
	input.relacc = FB_RELACC;
	input.ncount = FB_NCOUNT;
	input.tidaltol = FB_TIDALTOL;
	input.fexp = FB_FEXP;
	seed = FB_SEED;
	fb_debug = FB_DEBUG;

	while ((i = getopt_long(argc, argv, short_opts, long_opts, NULL)) != -1) {
		switch (i) {
		case 'm':
			m000 = atof(optarg) * FB_CONST_MSUN;
			break;
		case 'n':
			m001 = atof(optarg) * FB_CONST_MSUN;
			break;
		case 'o':
			m01 = atof(optarg) * FB_CONST_MSUN;
			break;
		case 'r':
			r000 = atof(optarg) * FB_CONST_RSUN;
			break;
		case 'g':
			r001 = atof(optarg) * FB_CONST_RSUN;
			break;
		case 'i':
			r01 = atof(optarg) * FB_CONST_RSUN;
			break;
		case 'a':
			a00 = atof(optarg) * FB_CONST_AU;
			break;
		case 'Q':
			a0 = atof(optarg) * FB_CONST_AU;
			break;
		case 'e':
			e00 = atof(optarg);
			if (e00 >= 1.0) {
				fprintf(stderr, "e00 must be less than 1\n");
				return(1);
			}
			break;
		case 'F':
			e0 = atof(optarg);
			if (e0 >= 1.0) {
				fprintf(stderr, "e0 must be less than 1\n");
				return(1);
			}
			break;
		case 't':
			input.tstop = atof(optarg);
			break;
		case 'D':
			input.Dflag = 1;
			input.dt = atof(optarg);
			break;
		case 'c':
			input.tcpustop = atof(optarg);
			break;
		case 'A':
			input.absacc = atof(optarg);
			break;
		case 'R':
			input.relacc = atof(optarg);
			break;
		case 'N':
			input.ncount = atoi(optarg);
			break;
		case 'z':
			input.tidaltol = atof(optarg);
			break;
		case 'x':
			input.fexp = atof(optarg);
			break;
		case 'k':
			input.ks = atoi(optarg);
			break;
		case 's':
			seed = atol(optarg);
			break;
		case 'd':
			fb_debug = 1;
			break;
		case 'V':
			fb_print_version(stdout);
			return(0);
		case 'h':
			fb_print_version(stdout);
			fprintf(stdout, "\n");
			print_usage(stdout);
			return(0);
		default:
			break;
		}
	}

	/* check to make sure there was nothing crazy on the command line */
	if (optind < argc) {
		print_usage(stdout);
		return(1);
	}

	/* initialize a few things for integrator */
	t = 0.0;
	hier.nstarinit = 3;
	hier.nstar = 3;
	fb_malloc_hier(&hier);
	fb_init_hier(&hier);

	/* put stuff in log entry */
	snprintf(input.firstlogentry, FB_MAX_LOGENTRY_LENGTH, "  command line:");
	for (i=0; i<argc; i++) {
		snprintf(&(input.firstlogentry[strlen(input.firstlogentry)]),
			 FB_MAX_LOGENTRY_LENGTH-strlen(input.firstlogentry), " %s", argv[i]);
	}
	snprintf(&(input.firstlogentry[strlen(input.firstlogentry)]),
		 FB_MAX_LOGENTRY_LENGTH-strlen(input.firstlogentry), "\n");

	/* print out values of paramaters */
	fprintf(stderr, "PARAMETERS:\n");
	fprintf(stderr, "  ks=%d  seed=%ld\n", input.ks, seed);
	fprintf(stderr, "  a00=%.6g AU  e00=%.6g  m000=%.6g MSUN  m001=%.6g MSUN  r000=%.6g RSUN  r001=%.6g RSUN\n", \
		a00/FB_CONST_AU, e00, m000/FB_CONST_MSUN, m001/FB_CONST_MSUN, r000/FB_CONST_RSUN, r001/FB_CONST_RSUN);
	fprintf(stderr, "  a0=%.6g AU  e0=%.6g  m01=%.6g MSUN  r01=%.6g RSUN\n", \
		a0/FB_CONST_AU, e0, m01/FB_CONST_MSUN, r01/FB_CONST_RSUN);
	fprintf(stderr, "  tstop=%.6g  tcpustop=%.6g\n", \
		input.tstop, input.tcpustop);
	fprintf(stderr, "  tidaltol=%.6g  abs_acc=%.6g  rel_acc=%.6g  ncount=%d  fexp=%.6g\n\n", \
		input.tidaltol, input.absacc, input.relacc, input.ncount, input.fexp);

	/* initialize GSL rng */
	gsl_rng_env_setup();
	rng = gsl_rng_alloc(rng_type);
	gsl_rng_set(rng, seed);

	/* create hierarchies */
	hier.narr[2] = 1;
	hier.narr[3] = 1;
	/* inner binary of triple */
	hier.hier[hier.hi[2]+0].obj[0] = &(hier.hier[hier.hi[1]+0]);
	hier.hier[hier.hi[2]+0].obj[1] = &(hier.hier[hier.hi[1]+1]);
	hier.hier[hier.hi[2]+0].t = t;
	/* outer binary of triple */
	hier.hier[hier.hi[3]+0].obj[0] = &(hier.hier[hier.hi[2]+0]);
	hier.hier[hier.hi[3]+0].obj[1] = &(hier.hier[hier.hi[1]+2]);
	hier.hier[hier.hi[3]+0].t = t;

	/* give the objects some properties */
	for (j=0; j<hier.nstar; j++) {
		hier.hier[hier.hi[1]+j].ncoll = 1;
		hier.hier[hier.hi[1]+j].id[0] = j;
		snprintf(hier.hier[hier.hi[1]+j].idstring, FB_MAX_STRING_LENGTH, "%d", j);
		hier.hier[hier.hi[1]+j].n = 1;
		hier.hier[hier.hi[1]+j].obj[0] = NULL;
		hier.hier[hier.hi[1]+j].obj[1] = NULL;
		hier.hier[hier.hi[1]+j].Eint = 0.0;
		hier.hier[hier.hi[1]+j].Lint[0] = 0.0;
		hier.hier[hier.hi[1]+j].Lint[1] = 0.0;
		hier.hier[hier.hi[1]+j].Lint[2] = 0.0;
	}

	hier.hier[hier.hi[1]+0].R = r000;
	hier.hier[hier.hi[1]+1].R = r001;
	hier.hier[hier.hi[1]+2].R = r01;

	hier.hier[hier.hi[1]+0].m = m000;
	hier.hier[hier.hi[1]+1].m = m001;
	hier.hier[hier.hi[1]+2].m = m01;

	hier.hier[hier.hi[2]+0].m = m000 + m001;
	hier.hier[hier.hi[3]+0].m = m000 + m001 + m01;

	hier.hier[hier.hi[2]+0].a = a00;
	hier.hier[hier.hi[3]+0].a = a0;

	hier.hier[hier.hi[2]+0].e = e00;
	hier.hier[hier.hi[3]+0].e = e0;

	hier.nobj = 1;
	hier.obj[0] = &(hier.hier[hier.hi[3]+0]);
	hier.obj[1] = NULL;
	hier.obj[2] = NULL;

	/* get the units and normalize */
	calc_units(hier.obj, &units);
	fb_normalize(&hier, units);

	/* place triple at origin */
	for (j=0; j<3; j++) {
		hier.obj[0]->x[j] = 0.0;
		hier.obj[0]->v[j] = 0.0;
	}

	/* randomize binary orientations and downsync */
	fb_randorient(&(hier.hier[hier.hi[3]+0]), rng);
	fb_downsync(&(hier.hier[hier.hi[3]+0]), t);
	fb_randorient(&(hier.hier[hier.hi[2]+0]), rng);
	fb_downsync(&(hier.hier[hier.hi[2]+0]), t);

	fprintf(stderr, "UNITS:\n");
	fprintf(stderr, "  v=%.6g km/s  l=%.6g AU  t=t_dyn=%.6g yr\n", \
		units.v/1.0e5, units.l/FB_CONST_AU, units.t/FB_CONST_YR);
	fprintf(stderr, "  M=%.6g M_sun  E=%.6g erg\n\n", units.m/FB_CONST_MSUN, units.E);

	/* trickle down properties (not sure if this is actually needed here, but it doesn't harm anything) */
	fb_trickle(&hier, t);

	/* store the initial energy and angular momentum*/
	Ei = fb_petot(&(hier.hier[hier.hi[1]]), hier.nstar) + fb_ketot(&(hier.hier[hier.hi[1]]), hier.nstar) +
		fb_einttot(&(hier.hier[hier.hi[1]]), hier.nstar);
	fb_angmom(&(hier.hier[hier.hi[1]]), hier.nstar, Li);
	fb_angmomint(&(hier.hier[hier.hi[1]]), hier.nstar, Lint);
	for (j=0; j<3; j++) {
		Li[j] += Lint[j];
	}

	/* integrate along */
	fb_dprintf("calling fewbody()...\n");

	/* call fewbody! */
	retval = fewbody(input, &hier, &t);

	/* print information to screen */
	fprintf(stderr, "OUTCOME:\n");
	if (retval.retval == 1) {
		fprintf(stderr, "  encounter complete:  t=%.6g (%.6g yr)  %s  (%s)\n\n",
			t, t * units.t/FB_CONST_YR,
			fb_sprint_hier(hier, string1),
			fb_sprint_hier_hr(hier, string2));
	} else {
		fprintf(stderr, "  encounter NOT complete:  t=%.6g (%.6g yr)  %s  (%s)\n\n",
			t, t * units.t/FB_CONST_YR,
			fb_sprint_hier(hier, string1),
			fb_sprint_hier_hr(hier, string2));
	}

	fb_dprintf("there were %ld integration steps\n", retval.count);
	fb_dprintf("fb_classify() was called %ld times\n", retval.iclassify);

	fprintf(stderr, "FINAL:\n");
	fprintf(stderr, "  t_final=%.6g (%.6g yr)  t_cpu=%.6g s\n", \
		t, t*units.t/FB_CONST_YR, retval.tcpu);

	fprintf(stderr, "  L0=%.6g  DeltaL/L0=%.6g  DeltaL=%.6g\n", fb_mod(Li), retval.DeltaLfrac, retval.DeltaL);
	fprintf(stderr, "  E0=%.6g  DeltaE/E0=%.6g  DeltaE=%.6g\n", Ei, retval.DeltaEfrac, retval.DeltaE);
	fprintf(stderr, "  Rmin=%.6g (%.6g RSUN)  Rmin_i=%d  Rmin_j=%d\n", \
		retval.Rmin, retval.Rmin*units.l/FB_CONST_RSUN, retval.Rmin_i, retval.Rmin_j);
	fprintf(stderr, "  Nosc=%d (%s)\n", retval.Nosc, (retval.Nosc>=1?"resonance":"non-resonance"));

	/* free GSL stuff */
	gsl_rng_free(rng);

	/* free our own stuff */
	fb_free_hier(hier);

	/* done! */
	return(0);
}


/* -*- linux-c -*- */
/* fewbody.h

   Copyright (C) 2002-2004 John M. Fregeau

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef _FEWBODY_H
#define _FEWBODY_H 1

#include <stdio.h>
#include <gsl/gsl_nan.h>
#include <gsl/gsl_rng.h>

/* version information */
#define FB_VERSION "0.26"
#define FB_NICK "Oblivion"
#define FB_DATE "Mon Sep  6 09:05:47 EDT 2010"

/* dimensionless constants */
#define FB_CONST_PI 3.141592653589793238462643

/* constants, in cgs units */
#define FB_CONST_MSUN 1.989e+33
#define FB_CONST_RSUN 6.9599e+10
#define FB_CONST_C 2.99792458e+10
#define FB_CONST_G 6.67259e-8
#define FB_CONST_AU 1.496e+13
#define FB_CONST_PARSEC 3.0857e+18
#define FB_CONST_YR 3.155693e+7

/* these usually shouldn't need to be changed */
#define FB_H 1.0e-2
#define FB_SSTOP GSL_POSINF
#define FB_AMIN GSL_POSINF
#define FB_RMIN GSL_POSINF
#define FB_ROOTSOLVER_MAX_ITER 100
#define FB_ROOTSOLVER_ABS_ACC 1.0e-11
#define FB_ROOTSOLVER_REL_ACC 1.0e-11
#define FB_MAX_STRING_LENGTH 2048
#define FB_MAX_LOGENTRY_LENGTH (32 * FB_MAX_STRING_LENGTH)

/* a struct containing the units used */
typedef struct{
	double v; /* velocity */
	double l; /* length */
	double t; /* time */
	double m; /* mass */
	double E; /* energy */
} fb_units_t;

/* the fundamental object */
typedef struct fb_obj{
	int ncoll; /* total number of stars collided together in this star */
	long *id; /* numeric id array */
	char idstring[FB_MAX_STRING_LENGTH]; /* string id */
	double m; /* mass */
	double R; /* radius */
	double Eint; /* internal energy (used to check energy conservation) */
	double Lint[3]; /* internal ang mom (used to check ang mom conservation) */
	double x[3]; /* position */
	double v[3]; /* velocity */
	int n; /* total number of stars in hierarchy */
	struct fb_obj *obj[2]; /* pointers to children */
	double a; /* semimajor axis */
	double e; /* eccentricity */
	double Lhat[3]; /* angular momentum vector */
	double Ahat[3]; /* Runge-Lenz vector */
	double t; /* time at which node was upsynced */
	double mean_anom; /* mean anomaly when node was upsynced */
} fb_obj_t;

/* parameters for the K-S integrator */
typedef struct{
	int nstar; /* number of actual stars */
	int kstar; /* nstar*(nstar-1)/2, number of separations */
	double *m; /* m[nstar] */
	double *M; /* M[kstar] */
	double **amat; /* amat[nstar][kstar] */
	double **Tmat; /* Tmat[kstar][kstar] */
	double Einit; /* initial energy used in integration scheme */
} fb_ks_params_t;

/* parameters for the non-regularized integrator */
typedef struct{
	int nstar; /* number of actual stars */
	double *m; /* m[nstar] */
} fb_nonks_params_t;

/* the hierarchy data structure */
typedef struct{
	int nstarinit; /* initial number of stars (may not equal nstar if there are collisions) */
	int nstar; /* number of stars */
	int nobj; /* number of binary trees */
	int *hi; /* hierarchical index array */
	int *narr; /* narr[i] = number of hierarchical objects with i elements */
	fb_obj_t *hier; /* memory location of hierarchy information */
	fb_obj_t **obj; /* array of pointers to top nodes of binary trees */
} fb_hier_t;

/* input parameters */
typedef struct{
	int ks; /* 0=no regularization, 1=K-S regularization */
	double tstop; /* stopping time, in units of t_dyn */
	int Dflag; /* 0=don't print to stdout, 1=print to stdout */
	double dt; /* time interval between printouts will always be greater than this value */
	double tcpustop; /* cpu stopping time, in units of seconds */
	double absacc; /* absolute accuracy of the integrator */
	double relacc; /* relative accuracy of the integrator */
	int ncount; /* number of integration steps between each call to fb_classify() */
	double tidaltol; /* tidal tolerance */
	char firstlogentry[FB_MAX_LOGENTRY_LENGTH]; /* first entry to put in printout log */
	double fexp; /* expansion factor for a merger product: R = f_exp (R_1+R_2) */
} fb_input_t;

/* return parameters */
typedef struct{
	long count; /* number of integration steps */
	int retval; /* return value; 1=success, 0=failure */
	long iclassify; /* number of times classify was called */
	double tcpu; /* cpu time taken */
	double DeltaE; /* change in energy */
	double DeltaEfrac; /* change in energy, as a fraction of initial energy */
	double DeltaL; /* change in ang. mom. */
	double DeltaLfrac; /* change in ang. mom., as a fraction of initial ang. mom. */
	double Rmin; /* minimum distance of close approach during interaction */
	int Rmin_i; /* index of star i participating in minimum close approach */
	int Rmin_j; /* index of star j participating in minimum close approach */
	int Nosc; /* number of oscillations of the quantity s^2 (McMillan & Hut 1996) (Nosc=Nmin-1, so resonance if Nosc>=1) */
} fb_ret_t;

/* fewbody.c */
fb_ret_t fewbody(fb_input_t input, fb_hier_t *hier, double *t);

/* fewbody_classify.c */
int fb_classify(fb_hier_t *hier, double t, double tidaltol);
int fb_is_stable(fb_obj_t *obj);
int fb_is_stable_binary(fb_obj_t *obj);
int fb_is_stable_triple(fb_obj_t *obj);
int fb_is_stable_quad(fb_obj_t *obj);
int fb_mardling(fb_obj_t *obj, int ib, int is);

/* fewbody_coll.c */
int fb_is_collision(double r, double R1, double R2);
int fb_collide(fb_hier_t *hier, double f_exp);
void fb_merge(fb_obj_t *obj1, fb_obj_t *obj2, int nstarinit, double f_exp);

/* fewbody_hier.c */
void fb_malloc_hier(fb_hier_t *hier);
void fb_init_hier(fb_hier_t *hier);
void fb_free_hier(fb_hier_t hier);
void fb_trickle(fb_hier_t *hier, double t);
void fb_elkcirt(fb_hier_t *hier, double t);
int fb_create_indices(int *hi, int nstar);
int fb_n_hier(fb_obj_t *obj);
char *fb_sprint_hier(fb_hier_t hier, char string[FB_MAX_STRING_LENGTH]);
char *fb_sprint_hier_hr(fb_hier_t hier, char string[FB_MAX_STRING_LENGTH]);
void fb_upsync(fb_obj_t *obj, double t);
void fb_randorient(fb_obj_t *obj, gsl_rng *rng);
void fb_downsync(fb_obj_t *obj, double t);
void fb_objcpy(fb_obj_t *obj1, fb_obj_t *obj2);

/* fewbody_int.c */
void fb_malloc_ks_params(fb_ks_params_t *ks_params);
void fb_init_ks_params(fb_ks_params_t *ks_params, fb_hier_t hier);
void fb_free_ks_params(fb_ks_params_t ks_params);
void fb_malloc_nonks_params(fb_nonks_params_t *nonks_params);
void fb_init_nonks_params(fb_nonks_params_t *nonks_params, fb_hier_t hier);
void fb_free_nonks_params(fb_nonks_params_t nonks_params);

/* fewbody_io.c */
void fb_print_version(FILE *stream);
void fb_print_story(fb_obj_t *star, int nstar, double t, char *logentry);

/* fewbody_isolate.c */
int fb_collapse(fb_hier_t *hier, double t, double tidaltol);
int fb_expand(fb_hier_t *hier, double t, double tidaltol);

/* fewbody_ks.c */
double fb_ks_dot(double x[4], double y[4]);
double fb_ks_mod(double x[4]);
void fb_calc_Q(double q[4], double Q[4]);
void fb_calc_ksmat(double Q[4], double Qmat[4][4]);
void fb_calc_amat(double **a, int nstar, int kstar);
void fb_calc_Tmat(double **a, double *m, double **T, int nstar, int kstar);
int fb_ks_func(double s, const double *y, double *f, void *params);
double fb_ks_Einit(const double *y, fb_ks_params_t params);
void fb_euclidean_to_ks(fb_obj_t **star, double *y, int nstar, int kstar);
void fb_ks_to_euclidean(double *y, fb_obj_t **star, int nstar, int kstar);

/* fewbody_nonks.c */
int fb_nonks_func(double t, const double *y, double *f, void *params);
int fb_nonks_jac(double t, const double *y, double *dfdy, double *dfdt, void *params);
void fb_euclidean_to_nonks(fb_obj_t **star, double *y, int nstar);
void fb_nonks_to_euclidean(double *y, fb_obj_t **star, int nstar);

/* fewbody_scat.c */
void fb_init_scattering(fb_obj_t *obj0, fb_obj_t *obj1, double vinf, double b, double rtid);
void fb_normalize(fb_hier_t *hier, fb_units_t units);

/* fewbody_utils.c */
double *fb_malloc_vector(int n);
double **fb_malloc_matrix(int nr, int nc);
void fb_free_vector(double *v);
void fb_free_matrix(double **m);
double fb_sqr(double x);
double fb_cub(double x);
double fb_dot(double x[3], double y[3]);
double fb_mod(double x[3]);
int fb_cross(double x[3], double y[3], double z[3]);
int fb_angmom(fb_obj_t *star, int nstar, double L[3]);
void fb_angmomint(fb_obj_t *star, int nstar, double L[3]);
double fb_einttot(fb_obj_t *star, int nstar);
double fb_petot(fb_obj_t *star, int nstar);
double fb_ketot(fb_obj_t *star, int nstar);
double fb_outerpetot(fb_obj_t **obj, int nobj);
double fb_outerketot(fb_obj_t **obj, int nobj);
double fb_kepler(double e, double mean_anom);
double fb_keplerfunc(double mean_anom, void *params);
double fb_reltide(fb_obj_t *bin, fb_obj_t *single, double r);

/* fewbody_ui.c */
int fbui_new_hier(fb_hier_t *hier, int n);
int fbui_delete_hier(fb_hier_t *hier);
fb_obj_t *fbui_hierarchy_element(fb_hier_t *hier, int n, int m);
fb_obj_t *fbui_hierarchy_single(fb_hier_t *hier, int m);
fb_obj_t *fbui_hierarchy_binary(fb_hier_t *hier, int m);
fb_obj_t *fbui_hierarchy_triple(fb_hier_t *hier, int m);
fb_obj_t *fbui_hierarchy_quadruple(fb_hier_t *hier, int m);
fb_obj_t *fbui_hierarchy_quintuple(fb_hier_t *hier, int m);
fb_obj_t *fbui_hierarchy_sextuple(fb_hier_t *hier, int m);
fb_obj_t *fbui_hierarchy_septuple(fb_hier_t *hier, int m);
fb_obj_t *fbui_hierarchy_octuple(fb_hier_t *hier, int m);
fb_obj_t *fbui_hierarchy_nonuple(fb_hier_t *hier, int m);
fb_obj_t *fbui_hierarchy_decuple(fb_hier_t *hier, int m);
int fbui_make_pair(fb_obj_t *parentobj, fb_obj_t *obj1, fb_obj_t *obj2);
int fbui_initialize_single(fb_obj_t *single, long id, char idstring[FB_MAX_STRING_LENGTH]);
fb_obj_t *fbui_tree(fb_hier_t *hier, int n);
int fbui_obj_ncoll_set(fb_obj_t *obj, int ncoll);
int fbui_obj_ncoll_get(fb_obj_t *obj);
int fbui_obj_id_set(fb_obj_t *obj, int index, long id);
long fbui_obj_id_get(fb_obj_t *obj, int index);
int fbui_obj_idstring_set(fb_obj_t *obj, char idstring[FB_MAX_STRING_LENGTH]);
char *fbui_obj_idstring_get(fb_obj_t *obj);
int fbui_obj_mass_set(fb_obj_t *obj, double mass);
double fbui_obj_mass_get(fb_obj_t *obj);
int fbui_obj_radius_set(fb_obj_t *obj, double radius);
double fbui_obj_radius_get(fb_obj_t *obj);
int fbui_obj_Eint_set(fb_obj_t *obj, double Eint);
double fbui_obj_Eint_get(fb_obj_t *obj);
int fbui_obj_Lint_set(fb_obj_t *obj, double Lint[3]);
int fbui_obj_Linti_set(fb_obj_t *obj, double Lint0, double Lint1, double Lint2);
double *fbui_obj_Lint_get(fb_obj_t *obj);
int fbui_obj_x_set(fb_obj_t *obj, double x[3]);
int fbui_obj_xi_set(fb_obj_t *obj, double x0, double x1, double x2);
double *fbui_obj_x_get(fb_obj_t *obj);
int fbui_obj_v_set(fb_obj_t *obj, double v[3]);
int fbui_obj_vi_set(fb_obj_t *obj, double v0, double v1, double v2);
double *fbui_obj_v_get(fb_obj_t *obj);
int fbui_obj_n_set(fb_obj_t *obj, int n);
int fbui_obj_n_get(fb_obj_t *obj);
int fbui_obj_obj_set(fb_obj_t *obj, int i, fb_obj_t *objtopointto);
int fbui_obj_left_child_set(fb_obj_t *obj, fb_obj_t *objtopointto);
int fbui_obj_right_child_set(fb_obj_t *obj, fb_obj_t *objtopointto);
fb_obj_t *fbui_obj_obj_get(fb_obj_t *obj, int i);
fb_obj_t *fbui_obj_left_child_get(fb_obj_t *obj);
fb_obj_t *fbui_obj_right_child_get(fb_obj_t *obj);
int fbui_obj_a_set(fb_obj_t *obj, double a);
double fbui_obj_a_get(fb_obj_t *obj);
int fbui_obj_e_set(fb_obj_t *obj, double e);
double fbui_obj_e_get(fb_obj_t *obj);
int fbui_obj_Lhat_set(fb_obj_t *obj, double Lhat[3]);
int fbui_obj_Lhati_set(fb_obj_t *obj, double Lhat0, double Lhat1, double Lhat2);
double *fbui_obj_Lhat_get(fb_obj_t *obj);
int fbui_obj_Ahat_set(fb_obj_t *obj, double Ahat[3]);
int fbui_obj_Ahati_set(fb_obj_t *obj, double Ahat0, double Ahat1, double Ahat2);
double *fbui_obj_Ahat_get(fb_obj_t *obj);
int fbui_obj_t_set(fb_obj_t *obj, double t);
double fbui_obj_t_get(fb_obj_t *obj);
int fbui_obj_mean_anom_set(fb_obj_t *obj, double mean_anom);
double fbui_obj_mean_anom_get(fb_obj_t *obj);

/* macros */
/* The variadic macro syntax here conforms to the C99 standard, but for some
   reason won't compile on Mac OSX with gcc. */
/* #define fb_dprintf(...) if (fb_debug) fprintf(stderr, __VA_ARGS__) */
/* The variadic macro syntax here is the old gcc standard, and compiles on
   Mac OSX with gcc. */
#define fb_dprintf(args...) if (fb_debug) fprintf(stderr, args)
#define FB_MIN(a, b) ((a)<=(b)?(a):(b))
#define FB_MAX(a, b) ((a)>=(b)?(a):(b))
#define FB_DELTA(i, j) ((i)==(j)?1:0)
#define FB_KS_K(i, j, nstar) ((i)*(nstar)-((i)+1)*((i)+2)/2+(j))

/* there is just one global variable */
extern int fb_debug;

#endif /* fewbody.h */


/* -*- linux-c -*- */
/* cluster.c

   Copyright (C) 2002-2004 John M. Fregeau

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <getopt.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_roots.h>
#include "fewbody.h"
#include "cluster.h"

/* print the usage */
void print_usage(FILE *stream)
{
	fprintf(stream, "USAGE:\n");
	fprintf(stream, "  cluster [options...]\n");
	fprintf(stream, "\n");
	fprintf(stream, "OPTIONS:\n");
	fprintf(stream, "  -n --n <n>                    : set number of stars in cluster [%d]\n", FB_N);
	fprintf(stream, "  -m --m <m/MSUN>               : set mass of each star [%.6g]\n", FB_M/FB_CONST_MSUN);
	fprintf(stream, "  -r --r <r/RSUN>               : set radius of each star [%.6g]\n", FB_R/FB_CONST_RSUN);
	fprintf(stream, "  -S --sigma <sigma/(km/s)>     : set central velocity dispersion [%.6g]\n", FB_SIGMA/1.0e5);
	fprintf(stream, "  -T --rmax <rmax/PARSEC>       : set truncation radius [%.6g]\n", FB_RMAX/FB_CONST_PARSEC);
	fprintf(stream, "  -t --tstop <tstop/t_dyn>      : set stopping time [%.6g]\n", FB_TSTOP);
	fprintf(stream, "  -P --tphysstop <tphysstop/yr> : set physical stopping time [%.6g]\n", FB_TPHYSSTOP/FB_CONST_YR);
	fprintf(stream, "  -D --dt <dt/t_dyn>            : set approximate output dt [%.6g]\n", FB_DT);
	fprintf(stream, "  -c --tcpustop <tcpustop/sec>  : set cpu stopping time [%.6g]\n", FB_TCPUSTOP);
	fprintf(stream, "  -A --absacc <absacc>          : set integrator's absolute accuracy [%.6g]\n", FB_ABSACC);
	fprintf(stream, "  -R --relacc <relacc>          : set integrator's relative accuracy [%.6g]\n", FB_RELACC);
	fprintf(stream, "  -N --ncount <ncount>          : set number of integration steps between calls\n");
	fprintf(stream, "                                  to fb_classify() [%d]\n", FB_NCOUNT);
	fprintf(stream, "  -z --tidaltol <tidaltol>      : set tidal tolerance [%.6g]\n", FB_TIDALTOL);
	fprintf(stream, "  -x --fexp <f_exp>             : set expansion factor of merger product [%.6g]\n", FB_FEXP);
	fprintf(stream, "  -k --ks                       : turn K-S regularization on or off [%d]\n", FB_KS);
	fprintf(stream, "  -s --seed                     : set random seed [%ld]\n", FB_SEED);
	fprintf(stream, "  -d --debug                    : turn on debugging\n");
	fprintf(stream, "  -V --version                  : print version info\n");
	fprintf(stream, "  -h --help                     : display this help text\n");
}

/* calculate the units used (N-body units) */
void calc_units(fb_hier_t hier, fb_units_t *units)
{
	int i, j, k;
	double petot, ketot, r[3];

	/* the unit of mass is defined so that M_tot=1 */
	units->m = 0.0;
	for (i=0; i<hier.nstar; i++) {
		units->m += hier.hier[hier.hi[1]+i].m;
	}

	/* calculate total potential and kinetic energy */
	petot = 0.0;
	for (i=0; i<hier.nstar-1; i++) {
		for (j=i+1; j<hier.nstar; j++) {
			for (k=0; k<3; k++) {
				r[k] = hier.hier[hier.hi[1]+j].x[k] - hier.hier[hier.hi[1]+i].x[k];
			}
			petot += - FB_CONST_G * hier.hier[hier.hi[1]+i].m * hier.hier[hier.hi[1]+j].m / fb_mod(r);
		}
	}

	ketot = 0.0;
	for (i=0; i<hier.nstar; i++) {
		ketot += 0.5 * hier.hier[hier.hi[1]+i].m * fb_dot(hier.hier[hier.hi[1]+i].v, hier.hier[hier.hi[1]+i].v);
	}

	/* the unit of energy is defined so that E=-1/4 */
	units->E = -4.0 * (petot + ketot);

	/* with G=1, all other units are derived */
	units->t = FB_CONST_G * pow(units->m, 2.5) * pow(units->E, -1.5);
	units->l = FB_CONST_G * fb_sqr(units->m) / units->E;
	units->v = units->l / units->t;
}

/* f_0-f(v/v_esc) for the Plummer model */
double fv(double v, void *params)
{
	double f;

	f = ((double *)params)[0];
	return(f - 512.0/(7.0*FB_CONST_PI) * (sqrt(1.0-fb_sqr(v))*(-7.0*v/256.0 + 121.0*fb_cub(v)/384.0 - 263.0*fb_sqr(v)*fb_cub(v)/480.0 + 31.0*v*fb_sqr(fb_cub(v))/80.0 - fb_cub(fb_cub(v))/10.0) + 7.0*asin(v)/256.0));
}

/* get speed from Plummer distribution */
double vf(double f)
{
	int status, iter;
	double v, params[1];
	gsl_function F;
	const gsl_root_fsolver_type *T;
	gsl_root_fsolver *s;

	/* set up the root solver */
	F.function = &fv;
	F.params = &params;

	/* set the parameters */
	params[0] = f;

	T = gsl_root_fsolver_brent;
	s = gsl_root_fsolver_alloc(T);
	gsl_root_fsolver_set(s, &F, 0.0, 1.0);

	/* get v/v_esc by root-finding */
	iter = 0;
	do {
		iter++;
		gsl_root_fsolver_iterate(s);
		status = gsl_root_test_interval(gsl_root_fsolver_x_lower(s), gsl_root_fsolver_x_upper(s), \
						FB_ROOTSOLVER_ABS_ACC, FB_ROOTSOLVER_REL_ACC);
	} while (status == GSL_CONTINUE && iter < FB_ROOTSOLVER_MAX_ITER);

	if (iter >= FB_ROOTSOLVER_MAX_ITER) {
		fprintf(stderr, "Root finder failed to converge.\n");
		exit(1);
	}

	/* we've got the root */
	v = gsl_root_fsolver_root(s);

	/* free memory associated with root solver */
	gsl_root_fsolver_free(s);

	return(v);
}

/* the main attraction */
int main(int argc, char *argv[])
{
	int i, j, n;
	unsigned long int seed;
	double tphysstop, m, r, Ei, Li[3], Lint[3], t, M, a, sigma, rmax, Xmax, radius, v, theta, phi, xcm[3], vcm[3];
	fb_hier_t hier;
	fb_input_t input;
	fb_ret_t retval;
	fb_units_t units;
	char string1[FB_MAX_STRING_LENGTH], string2[FB_MAX_STRING_LENGTH];
	gsl_rng *rng;
	const gsl_rng_type *rng_type=gsl_rng_mt19937;
	const char *short_opts = "n:m:r:S:T:t:P:D:c:A:R:N:z:x:k:s:dVh";
	const struct option long_opts[] = {
		{"n", required_argument, NULL, 'n'},
		{"m", required_argument, NULL, 'm'},
		{"r", required_argument, NULL, 'r'},
		{"sigma", required_argument, NULL, 'S'},
		{"rmax", required_argument, NULL, 'T'},
		{"tstop", required_argument, NULL, 't'},
		{"tphysstop", required_argument, NULL, 'P'},
		{"dt", required_argument, NULL, 'D'},
		{"tcpustop", required_argument, NULL, 'c'},
		{"absacc", required_argument, NULL, 'A'},
		{"relacc", required_argument, NULL, 'R'},
		{"ncount", required_argument, NULL, 'N'},
		{"tidaltol", required_argument, NULL, 'z'},
		{"fexp", required_argument, NULL, 'x'},
		{"ks", required_argument, NULL, 'k'},
		{"seed", required_argument, NULL, 's'},
		{"debug", no_argument, NULL, 'd'},
		{"version", no_argument, NULL, 'V'},
		{"help", no_argument, NULL, 'h'},
		{NULL, 0, NULL, 0}
	};

	/* set parameters to default values */
	n = FB_N;
	m = FB_M;
	r = FB_R;
	sigma = FB_SIGMA;
	rmax = FB_RMAX;
	input.ks = FB_KS;
	input.tstop = FB_TSTOP;
	tphysstop = FB_TPHYSSTOP;
	input.Dflag = 0;
	input.dt = FB_DT;
	input.tcpustop = FB_TCPUSTOP;
	input.absacc = FB_ABSACC;
	input.relacc = FB_RELACC;
	input.ncount = FB_NCOUNT;
	input.tidaltol = FB_TIDALTOL;
	input.fexp = FB_FEXP;
	seed = FB_SEED;
	fb_debug = FB_DEBUG;

	while ((i = getopt_long(argc, argv, short_opts, long_opts, NULL)) != -1) {
		switch (i) {
		case 'n':
			n = atoi(optarg);
			break;
		case 'm':
			m = atof(optarg) * FB_CONST_MSUN;
			break;
		case 'r':
			r = atof(optarg) * FB_CONST_RSUN;
			break;
		case 'S':
			sigma = atof(optarg) * 1.0e5;
			break;
		case 'T':
			rmax = atof(optarg) * FB_CONST_PARSEC;
			break;
		case 't':
			input.tstop = atof(optarg);
			break;
		case 'P':
			tphysstop = atof(optarg) * FB_CONST_YR;
			break;
		case 'D':
			input.Dflag = 1;
			input.dt = atof(optarg);
			break;
		case 'c':
			input.tcpustop = atof(optarg);
			break;
		case 'A':
			input.absacc = atof(optarg);
			break;
		case 'R':
			input.relacc = atof(optarg);
			break;
		case 'N':
			input.ncount = atoi(optarg);
			break;
		case 'z':
			input.tidaltol = atof(optarg);
			break;
		case 'x':
			input.fexp = atof(optarg);
			break;
		case 'k':
			input.ks = atoi(optarg);
			break;
		case 's':
			seed = atol(optarg);
			break;
		case 'd':
			fb_debug = 1;
			break;
		case 'V':
			fb_print_version(stdout);
			return(0);
		case 'h':
			fb_print_version(stdout);
			fprintf(stdout, "\n");
			print_usage(stdout);
			return(0);
		default:
			break;
		}
	}

	/* check to make sure there was nothing crazy on the command line */
	if (optind < argc) {
		print_usage(stdout);
		return(1);
	}

	/* set up parameters for Plummer model */
	M = ((double) n) * m;
	a = FB_CONST_G * M / (6.0*fb_sqr(sigma));
	Xmax = pow(1.0+fb_sqr(a/rmax), -1.5);

	/* initialize a few things for integrator */
	t = 0.0;
	hier.nstarinit = n;
	hier.nstar = n;
	fb_malloc_hier(&hier);
	fb_init_hier(&hier);

	/* put stuff in log entry */
	snprintf(input.firstlogentry, FB_MAX_LOGENTRY_LENGTH, "  command line:");
	for (i=0; i<argc; i++) {
		snprintf(&(input.firstlogentry[strlen(input.firstlogentry)]),
			 FB_MAX_LOGENTRY_LENGTH-strlen(input.firstlogentry), " %s", argv[i]);
	}
	snprintf(&(input.firstlogentry[strlen(input.firstlogentry)]),
		 FB_MAX_LOGENTRY_LENGTH-strlen(input.firstlogentry), "\n");

	/* print out values of paramaters */
	fprintf(stderr, "PARAMETERS:\n");
	fprintf(stderr, "  ks=%d  seed=%ld\n", input.ks, seed);
	fprintf(stderr, "  M=%.6g MSUN  sigma=%.6g km/s  a=%.6g PARSEC  rmax=%.6g PARSEC  rmax/a=%.6g\n",
		M/FB_CONST_MSUN, sigma/1.0e5, a/FB_CONST_PARSEC, rmax/FB_CONST_PARSEC, rmax/a);
	fprintf(stderr, "  n=%d  m=%.6g MSUN  r=%.6g RSUN  tstop=%.6g  tphysstop=%.6g yr  tcpustop=%.6g\n", \
		n, m/FB_CONST_MSUN, r/FB_CONST_RSUN, input.tstop, tphysstop/FB_CONST_YR, input.tcpustop);
	fprintf(stderr, "  tidaltol=%.6g  abs_acc=%.6g  rel_acc=%.6g  ncount=%d  fexp=%.6g\n\n", \
		input.tidaltol, input.absacc, input.relacc, input.ncount, input.fexp);

	/* initialize GSL rng */
	gsl_rng_env_setup();
	rng = gsl_rng_alloc(rng_type);
	gsl_rng_set(rng, seed);

	/* prepare to calculate the center of mass position and velocity */
	for (i=0; i<3; i++) {
		xcm[i] = 0.0;
		vcm[i] = 0.0;
	}

	/* give the objects some properties */
	for (j=0; j<hier.nstar; j++) {
		hier.hier[hier.hi[1]+j].ncoll = 1;
		hier.hier[hier.hi[1]+j].id[0] = j;
		snprintf(hier.hier[hier.hi[1]+j].idstring, FB_MAX_STRING_LENGTH, "%d", j);
		hier.hier[hier.hi[1]+j].n = 1;
		hier.hier[hier.hi[1]+j].obj[0] = NULL;
		hier.hier[hier.hi[1]+j].obj[1] = NULL;
		hier.hier[hier.hi[1]+j].Eint = 0.0;
		hier.hier[hier.hi[1]+j].Lint[0] = 0.0;
		hier.hier[hier.hi[1]+j].Lint[1] = 0.0;
		hier.hier[hier.hi[1]+j].Lint[2] = 0.0;
		hier.hier[hier.hi[1]+j].m = m;
		hier.hier[hier.hi[1]+j].R = r;

		/* draw radius from Plummer distribution */
		radius = a / sqrt(pow(Xmax*gsl_rng_uniform(rng), -2.0/3.0)-1.0);
		theta = acos(2.0 * gsl_rng_uniform(rng) - 1.0);
		phi = 2.0 * FB_CONST_PI * gsl_rng_uniform(rng);
		hier.hier[hier.hi[1]+j].x[0] = radius * sin(theta) * cos(phi);
		hier.hier[hier.hi[1]+j].x[1] = radius * sin(theta) * sin(phi);
		hier.hier[hier.hi[1]+j].x[2] = radius * cos(theta);

		/* draw velocity from Plummer distribution */
		v = vf(gsl_rng_uniform(rng)) * sqrt(2.0*FB_CONST_G*M/(a*sqrt(1.0+fb_sqr(radius/a))));
		theta = acos(2.0 * gsl_rng_uniform(rng) - 1.0);
		phi = 2.0 * FB_CONST_PI * gsl_rng_uniform(rng);
		hier.hier[hier.hi[1]+j].v[0] = v * sin(theta) * cos(phi);
		hier.hier[hier.hi[1]+j].v[1] = v * sin(theta) * sin(phi);
		hier.hier[hier.hi[1]+j].v[2] = v * cos(theta);

		/* calculate center of mass position and velocity */
		for (i=0; i<3; i++) {
			xcm[i] += hier.hier[hier.hi[1]+j].x[i] / ((double) n);
			vcm[i] += hier.hier[hier.hi[1]+j].v[i] / ((double) n);
		}
	}

	/* transform to center of mass frame */
	for (j=0; j<hier.nstar; j++) {
		for (i=0; i<3; i++) {
			hier.hier[hier.hi[1]+j].x[i] -= xcm[i];
			hier.hier[hier.hi[1]+j].v[i] -= vcm[i];
		}
	}

	/* get the units and normalize */
	calc_units(hier, &units);
	fb_normalize(&hier, units);

	/* reset input.tstop if necessary, based on the value of tphysstop */
	if (tphysstop/units.t < input.tstop) {
		input.tstop = tphysstop/units.t;
		fb_dprintf("decreasing tstop in accord with tphysstop: tstop=%.6g\n", input.tstop);
	}

	fb_dprintf("virial ratio: q=%.6g\n", -fb_ketot(&(hier.hier[hier.hi[1]]), hier.nstar)/fb_petot(&(hier.hier[hier.hi[1]]), hier.nstar));

	fprintf(stderr, "UNITS:\n");
	fprintf(stderr, "  v=%.6g km/s  l=%.6g AU  t=t_dyn=%.6g yr\n", \
		units.v/1.0e5, units.l/FB_CONST_AU, units.t/FB_CONST_YR);
	fprintf(stderr, "  M=%.6g M_sun  E=%.6g erg\n\n", units.m/FB_CONST_MSUN, units.E);

	/* calculate the initial energy and angular momentum, for bookkeeping */
	Ei = fb_petot(&(hier.hier[hier.hi[1]]), hier.nstar) + fb_ketot(&(hier.hier[hier.hi[1]]), hier.nstar) +
		fb_einttot(&(hier.hier[hier.hi[1]]), hier.nstar);
	fb_angmom(&(hier.hier[hier.hi[1]]), hier.nstar, Li);
	fb_angmomint(&(hier.hier[hier.hi[1]]), hier.nstar, Lint);
	for (j=0; j<3; j++) {
		Li[j] += Lint[j];
	}

	/* integrate along */
	fb_dprintf("calling fewbody()...\n");

	/* call fewbody! */
	retval = fewbody(input, &hier, &t);

	/* print information to screen */
	fprintf(stderr, "OUTCOME:\n");
	if (retval.retval == 1) {
		fprintf(stderr, "  encounter complete:  t=%.6g (%.6g yr)  %s  (%s)\n\n",
			t, t * units.t/FB_CONST_YR,
			fb_sprint_hier(hier, string1),
			fb_sprint_hier_hr(hier, string2));
	} else {
		fprintf(stderr, "  encounter NOT complete:  t=%.6g (%.6g yr)  %s  (%s)\n\n",
			t, t * units.t/FB_CONST_YR,
			fb_sprint_hier(hier, string1),
			fb_sprint_hier_hr(hier, string2));
	}

	fb_dprintf("there were %ld integration steps\n", retval.count);
	fb_dprintf("fb_classify() was called %ld times\n", retval.iclassify);

	fprintf(stderr, "FINAL:\n");
	fprintf(stderr, "  t_final=%.6g (%.6g yr)  t_cpu=%.6g s\n", \
		t, t*units.t/FB_CONST_YR, retval.tcpu);

	fprintf(stderr, "  L0=%.6g  DeltaL/L0=%.6g  DeltaL=%.6g\n", fb_mod(Li), retval.DeltaLfrac, retval.DeltaL);
	fprintf(stderr, "  E0=%.6g  DeltaE/E0=%.6g  DeltaE=%.6g\n", Ei, retval.DeltaEfrac, retval.DeltaE);
	fprintf(stderr, "  Rmin=%.6g (%.6g RSUN)  Rmin_i=%d  Rmin_j=%d\n", \
		retval.Rmin, retval.Rmin*units.l/FB_CONST_RSUN, retval.Rmin_i, retval.Rmin_j);
	fprintf(stderr, "  Nosc=%d (%s)\n", retval.Nosc, (retval.Nosc>=1?"resonance":"non-resonance"));

	/* free GSL stuff */
	gsl_rng_free(rng);

	/* free our own stuff */
	fb_free_hier(hier);

	/* done! */
	return(0);
}


#ifndef REACHOBJMULTIDEMO_H
#define REACHOBJMULTIDEMO_H

#include <cstdlib>
#include <ctime>
#include <vector>

#include <yarp/sig/all.h>
#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include <yarp/math/Math.h>

#include <iCub/iKin/iKinFwd.h>
#include <iCub/ctrl/math.h>

#include <gsl/gsl_math.h>

#include "reachObjMultiDemo_IDL.h"

class reachObjMultiDemo : public yarp::os::RFModule, public reachObjMultiDemo_IDL
{
protected:
    yarp::os::RpcServer             rpcPort;

    yarp::dev::IPositionControl*    posArm;
    yarp::dev::IVelocityControl*    velArm;
    yarp::dev::ITorqueControl *     itrqArm;
    yarp::dev::IEncoders*           encsArm;
    yarp::dev::IControlMode*        iCtrlArm;
    yarp::dev::IControlLimits*      iCtrlLimArm;

    yarp::dev::IPositionControl*    posRightArm;
    yarp::dev::IVelocityControl*    velRightArm;
    yarp::dev::ITorqueControl*      itrqRightArm;
    yarp::dev::IEncoders*           encsRightArm;
    yarp::dev::IControlMode*        iCtrlRightArm;
    yarp::dev::IControlLimits*      iCtrlLimRightArm;

    yarp::dev::IPositionControl*    posTorso;
    yarp::dev::IVelocityControl*    velTorso;
    yarp::dev::ITorqueControl*      itrqTorso;
    yarp::dev::IEncoders*           encsTorso;
    yarp::dev::IControlMode*        iCtrlTorso;
    yarp::dev::IControlLimits*      iCtrlLimTorso;

    iCub::iKin::iCubArm*            arm;
    yarp::sig::Vector               qA;

    yarp::dev::IGazeControl*        igaze;
    int                             contextGaze;
    yarp::dev::ICartesianControl   *iCartCtrlR;
    yarp::dev::ICartesianControl   *iCartCtrl;

    yarp::dev::PolyDriver           torsoDev;
    yarp::dev::PolyDriver           rightArmDev;
    yarp::dev::PolyDriver           headDev;

    yarp::dev::PolyDriver           rightCartDev;

    yarp::sig::Vector               encodersRightArm;
    yarp::sig::Vector               encodersTorso;

    std::string part;               //!< Should be "left_arm" or "right_arm"
    std::string robot;              //!< Should be "icub" or "icubSim"
    double      period;
    std::string name;
    bool        changeTipFrame;     //!<
    bool        useFakeTarget;
    bool        useHandAngle;
    yarp::sig::Vector               fakeTarget;
    double                          tiltAngle, yawAngle;

    yarp::sig::Vector   new_command_arm;
    yarp::sig::Vector   new_command_head;
    double              start_command_arm[16];  //!< Start command for the 16 arm joints
    double              away_command_arm[16];   //!< Away command for the 16 arm joints
    yarp::sig::Vector   start_command_head;     //!< Target for head in start position
    double              neck_range_min[3];
    double              neck_range_max[3];
    double              minLimArm[16];
    double              maxLimArm[16];

    bool                hasCommand;
    bool                hasTarget;

    yarp::os::BufferedPort<yarp::os::Bottle>    cmdJointsPort;
    yarp::os::Bottle                            *cmdJointsBottle;
    yarp::sig::Vector                           cmdJointsAng;

    yarp::os::BufferedPort<yarp::os::Bottle>    objPosDimPort;
    yarp::os::Bottle                            *objPoseDimBottle;
    yarp::sig::Vector                           objPoseDim;

    yarp::os::BufferedPort<yarp::os::Bottle>    tactilePort;
    yarp::os::Bottle                            *tactileBottle;

    yarp::os::BufferedPort<yarp::os::Bottle>    targetDumpedData;             //!< buffered port of dumped target data
    yarp::os::Stamp                             ts;

    yarp::sig::Vector                           joints_arm_min, joints_arm_max;

    yarp::os::RpcClient     rpcToMotorBabbling;
    yarp::os::RpcClient     rpcToLearning;

    bool    init_right_arm(); //!< Create PolyDriver for left arm
    bool    initRobot();
//    bool    moveHeadToStartPos(bool ranPos);
    bool    moveHeadToCentralPos();
    bool    moveArmToStartPos(const std::string &partName);
    bool    moveArmAway(const std::string &partName);
    bool    gotoStartPos(bool moveAway);
    bool    reachTarget();

    void    updateArmChain(const yarp::sig::Vector &q, yarp::sig::Vector &xEE_t);

    bool    sendCmdBabbling();
    bool    sendCmdLearningPredict();
    bool    sendCmdLearningMove(const int &id, const std::string &partName);

    std::vector<int> reprTaxelsForearm_sim =
    {3, 15, 27, 39, 51, 75, 87, 183,
    207, 255, 291, 303, 315, 339, 351}; //23 taxels

    std::vector<int> reprTaxelsHandL =
    {3, 15, 27, 39, 51, 99, 101, 109, 122, 134};

    std::vector<int> reprTaxelsHandR =
    {3, 15, 39, 51, 101, 118, 137};

public:
    bool    configure(yarp::os::ResourceFinder &rf);
    bool    interruptModule();
    bool    close();
    bool    attach(yarp::os::RpcServer &source);
    double  getPeriod();
    bool    updateModule();
//    reachBallDemo();

    //Thrift
    bool home_arms()
    {
        bool ok = false;
//        ok = moveArmToStartPos("left");
        ok = moveArmToStartPos("right");
        if (ok)
            yDebug("[%s] Moved all arms home sucessfully!!",name.c_str());
        else
            yDebug("[%s] Failed moving all arms home!!",name.c_str());
        return ok;
    }

    bool home_all()
    {
        bool ok = false;
        ok = moveHeadToCentralPos();
        ok = ok & home_arms();
        if (ok)
            yDebug("[%s] Moved all parts home sucessfully!!",name.c_str());
        else
            yDebug("[%s] Failed moving all parts home!!",name.c_str());
        return ok;
    }

    bool reach(bool _useCalib)
    {
        yDebug("[%s] Start reaching the target",name.c_str());
        hasCommand = true;
//        if (hasTarget)
//            return reachTarget();
//        else
//        {
//            yWarning("[%s] No target", name.c_str());
//            return false;
//        }
    }

    bool babble_then_reach(int32_t _id, const std::string &_part)
    {
        bool ok = sendCmdBabbling();
        yarp::os::Time::delay(3);
        ok = ok & sendCmdLearningPredict();
        yarp::os::Time::delay(1);

        if (_id==-1)    // reach all taxel of part
        {
//            if (_part=="b" or _part=="both")
//            {
//                for (int i=0; i<=reprTaxelsHandR.size(); i++)
//                {
//                    sendCmdLearningMove(reprTaxelsHandR[i], "h");

//                }
//            }
//            else if (_part=="h" or _part=="hand")
//            {

//            }
//            else if (_part=="a" or _part=="arm")
//            {

//            }
        }
        else
            ok = ok & sendCmdLearningMove(_id,_part);
        return ok;
    }

    bool set_tilt_angle(const double _ang)
    {
        tiltAngle = _ang;
        return true;
    }

    double get_tilt_angle()
    {
        return tiltAngle;
    }

    bool set_yaw_angle(const double _ang)
    {
        yawAngle = _ang;
        return true;
    }

    double get_yaw_angle()
    {
        return yawAngle;
    }

    bool enable_hand_angle()
    {
        useHandAngle = true;
        return true;
    }

    bool disable_hand_angle()
    {
        useHandAngle = false;
        return true;
    }

};

#endif // REACHBALLDEMO_H