#include <math.h>
#include <string.h>


#include "tmalloc.h"
#include "tlog.h"

#include "galaxy.h"

#if defined(SORT_STAR) || defined (SORT_ENERGYSTAR)
static int
maxN2(int n) {
	int i=0;

	while(n) {
		n >>= 1;
		i++;
	}

	return 1<<i;
}

static double
safeSumSortedArray(double *arr, int N) {
	int i;

	while(N>1) {
		for(i=0;i<N;i+=2)
			arr[i>>1] = arr[i]+arr[i+1];
		N>>=1;
	}

	return arr[0];
}

#endif

void
initGalaxy(Galaxy *galaxy, u_int32_t nstars) {
	memset(galaxy,0,sizeof(Galaxy));

	if ( nstars < 2 ) 
		nstars=2;
	galaxy->nstars = nstars;

	galaxy->stars=(Star*)t0malloc( nstars*sizeof(Star) );
	galaxy->forces=(Vector*)t0malloc( CNT_FARRAY(nstars)*sizeof(Vector) );
}

void 
freeGalaxy(Galaxy *galaxy) {
	if ( galaxy->stars ) 
		tfree(galaxy->stars);
	if ( galaxy->forces ) 
		tfree(galaxy->forces);
	memset(galaxy,0,sizeof(Galaxy));
}

static void
cntForce(Vector *force, Star *istar, Star *jstar) {
	double          R2,F;
	R2 = (jstar->c.x - istar->c.x)*(jstar->c.x - istar->c.x) +
	     (jstar->c.y - istar->c.y)*(jstar->c.y - istar->c.y) +
	     (jstar->c.z - istar->c.z)*(jstar->c.z - istar->c.z);
	
	F = G*istar->mass*jstar->mass / (R2*sqrt(R2));

	force->x = F * fabs(jstar->c.x - istar->c.x);
	force->y = F * fabs(jstar->c.y - istar->c.y);
	force->z = F * fabs(jstar->c.z - istar->c.z);
} 

void 
forceGalaxy(Galaxy *galaxy) {
	Vector		*force = galaxy->forces;
	Star		*jstar, *istar = galaxy->stars;
	
	while( istar < galaxy->stars + galaxy->nstars - 1 ) {	
		jstar = istar+1;
		while( jstar < galaxy->stars + galaxy->nstars ) {
			cntForce(force, istar, jstar);
			force++; jstar++;
		}
		istar++;
	}
}

#ifdef	SORT_STAR

#define SUMFORCES(AXIS)									\
											\
static int										\
cmpAcc_##AXIS(const void *a, const void *b) {						\
	return ( fabs((*(Vector**)a)->AXIS) > fabs((*(Vector**)b)->AXIS) ) ? 1 : -1;	\
}											\
											\
static double										\
sumAxis_##AXIS( Vector **a, int n ) {							\
	int N = maxN2(n), i;								\
	double  arr[N];									\
											\
	qsort(a, n, sizeof(Vector*), cmpAcc_##AXIS);					\
											\
	memset(arr, 0, sizeof(double)*(N-n));						\
	for(i=0;i<n;i++)								\
		arr[i+N-n] = a[i]->AXIS;						\
											\
	return safeSumSortedArray(arr, N);						\
}

SUMFORCES(x)
SUMFORCES(y)
SUMFORCES(z)

#endif


void
accelerationGalaxy(Galaxy *galaxy) {
	u_int32_t	i,j;
	Star		*istar = galaxy->stars,*jstar;
#ifdef	SORT_STAR
	Vector*		ptrforces[galaxy->nstars];
#else
	Vector 		*force;
#endif

	for(i=0; i<galaxy->nstars; i++ ) {
		istar = galaxy->stars+i;
		jstar = galaxy->stars;

		memcpy( &(istar->pa), &(istar->a), sizeof(Vector) );

#ifdef	SORT_STAR
		for(j=0; j<galaxy->nstars; j++ ) { 
			if ( j<i ) { 
				ptrforces[j] = GET_FORCE(galaxy, j, i);
				ptrforces[j]->x = ( jstar->c.x>istar->c.x ) ? fabs(ptrforces[j]->x) : -fabs(ptrforces[j]->x);
				ptrforces[j]->y = ( jstar->c.y>istar->c.y ) ? fabs(ptrforces[j]->y) : -fabs(ptrforces[j]->y);
				ptrforces[j]->z = ( jstar->c.z>istar->c.z ) ? fabs(ptrforces[j]->z) : -fabs(ptrforces[j]->z);
			} else if ( j>i ) {
				ptrforces[j-1] = GET_FORCE(galaxy, i, j);
				cntForce( ptrforces[j-1],  istar, jstar );
				ptrforces[j-1]->x = ( jstar->c.x>istar->c.x ) ? ptrforces[j-1]->x : -ptrforces[j-1]->x;
				ptrforces[j-1]->y = ( jstar->c.y>istar->c.y ) ? ptrforces[j-1]->y : -ptrforces[j-1]->y;
				ptrforces[j-1]->z = ( jstar->c.z>istar->c.z ) ? ptrforces[j-1]->z : -ptrforces[j-1]->z;
			}
			jstar++;
		}

		istar->a.x = sumAxis_x(ptrforces, galaxy->nstars-1);
		istar->a.y = sumAxis_y(ptrforces, galaxy->nstars-1);
		istar->a.z = sumAxis_z(ptrforces, galaxy->nstars-1);
		
#else
		memset(&(istar->a), 0, sizeof(Vector));

		force=GET_FORCE(galaxy, 0, i);	
		for(j=0;j<i;j++) {
			istar->a.x += ( jstar->c.x>istar->c.x ) ? force->x : -force->x;
			istar->a.y += ( jstar->c.y>istar->c.y ) ? force->y : -force->y;
			istar->a.z += ( jstar->c.z>istar->c.z ) ? force->z : -force->z;
			force++; jstar++;
		}
		jstar++;
		for(j=i+1;j<galaxy->nstars;j++) {
			force = GET_FORCE(galaxy, i, j);
			cntForce(force, istar, jstar);
			
			istar->a.x += ( jstar->c.x>istar->c.x ) ? force->x : -force->x;
			istar->a.y += ( jstar->c.y>istar->c.y ) ? force->y : -force->y;
			istar->a.z += ( jstar->c.z>istar->c.z ) ? force->z : -force->z;
			jstar++;
		}
#endif

		istar->a.x /= istar->mass; 
		istar->a.y /= istar->mass; 
		istar->a.z /= istar->mass;

#ifdef TAYLOR3
		if ( galaxy->firsttime )
			memcpy( &(istar->pa), &(istar->a), sizeof(Vector) );
#endif
	} 
#ifdef TAYLOR3
	galaxy->firsttime = 0;
#endif
}

void
stepGalaxy(Galaxy *galaxy, double delta) {
	Star		*star = galaxy->stars;

	while( star - galaxy->stars < galaxy->nstars ) {
#ifdef TAYLOR3
		star->c.x += delta*( delta*( (star->a.x - star->pa.x)/3.0 + star->a.x )/2.0 + star->v.x );
		star->v.x += delta*( star->a.x + (star->a.x - star->pa.x)/2.0 ); 
		star->c.y += delta*( delta*( (star->a.y - star->pa.y)/3.0 + star->a.y )/2.0 + star->v.y );
		star->v.y += delta*( star->a.y + (star->a.y - star->pa.y)/2.0 ); 
		star->c.z += delta*( delta*( (star->a.z - star->pa.z)/3.0 + star->a.z )/2.0 + star->v.z );
		star->v.z += delta*( star->a.z + (star->a.z - star->pa.z)/2.0 ); 
#else
		star->c.x += delta*(star->v.x + star->a.x*delta/2.0); 
		star->v.x += star->a.x * delta; 
		star->c.y += delta*(star->v.y + star->a.y*delta/2.0); 
		star->v.y += star->a.y * delta; 
		star->c.z += delta*(star->v.z + star->a.z*delta/2.0); 
		star->v.z += star->a.z * delta;
#endif 

		star++;
	}
}

void
unstepGalaxy(Galaxy *galaxy, double delta) {
	Star		*star = galaxy->stars;

	while( star - galaxy->stars < galaxy->nstars ) {
#ifdef TAYLOR3
		star->v.x -= delta*( star->a.x + (star->a.x - star->pa.x)/2.0 ); 
		star->c.x -= delta*( delta*( (star->a.x - star->pa.x)/3.0 + star->a.x )/2.0 + star->v.x );
		star->v.y -= delta*( star->a.y + (star->a.y - star->pa.y)/2.0 ); 
		star->c.y -= delta*( delta*( (star->a.y - star->pa.y)/3.0 + star->a.y )/2.0 + star->v.y );
		star->v.z -= delta*( star->a.z + (star->a.z - star->pa.z)/2.0 ); 
		star->c.z -= delta*( delta*( (star->a.z - star->pa.z)/3.0 + star->a.z )/2.0 + star->v.z );
#else
		star->v.x -= star->a.x * delta; 
		star->c.x -= delta*(star->v.x + star->a.x*delta/2.0); 
		star->v.y -= star->a.y * delta; 
		star->c.y -= delta*(star->v.y + star->a.y*delta/2.0); 
		star->v.z -= star->a.z * delta; 
		star->c.z -= delta*(star->v.z + star->a.z*delta/2.0);
#endif 

		star++;
	}
}

#ifdef SORT_ENERGYSTAR
static double
getImpulseX(Star *star) {
	return star->mass * star->v.x; 
}

static double
getImpulseY(Star *star) {
	return star->mass * star->v.y; 
}

static double
getImpulseZ(Star *star) {
	return star->mass * star->v.z; 
}

static double
getMomentX(Star *star) {
	return star->mass * ( star->v.y * star->c.z - star->c.y * star->v.z );
}	

static double
getMomentY(Star *star) {
	return star->mass * ( star->v.z * star->c.x - star->c.z * star->v.x );
}	

static double
getMomentZ(Star *star) {
	return star->mass * ( star->v.x * star->c.y - star->c.x * star->v.y );
}	

static double
getKinetic(Star *star) {
	return star->mass * (star->v.x * star->v.x + star->v.y * star->v.y + star->v.z * star->v.z) / 2.0;
}

static int
cmpDouble(const void *a, const void *b) {
	return ( fabs(*(double*)a) > fabs(*(double*)b) ) ? 1 : -1;	
}

static double
safeSumArray(double *arr, int N) {
	qsort(arr, N, sizeof(double), cmpDouble);
	return safeSumSortedArray(arr,N);
}

static double
safeSumStar(Star *stars, int n, double (*extractor)(Star*)) {
	int N=maxN2(n), i;
	double arr[N];
	
	for(i=0;i<n;i++) 
		arr[i] = (*extractor)(stars+i);
	memset( arr+n, 0, sizeof(double)*(N-n) );

	return safeSumArray(arr, N);
}

#endif

void
EnergyImpulseGalaxy(Galaxy *galaxy) {
	Star            *istar=galaxy->stars, *jstar;
	double potential=0.0;
	double kinetic=0.0;
	double impulseX=0.0, impulseY=0.0, impulseZ=0.0, momentX=0.0, momentY=0.0, momentZ=0.0;
#ifdef SORT_ENERGYSTAR
	double		pe[ maxN2(CNT_FARRAY(galaxy->nstars)) ], *ptrp;

	ptrp=pe;
	memset(pe, 0, sizeof(double)*maxN2(CNT_FARRAY(galaxy->nstars)));

#endif
	while( istar - galaxy->stars < galaxy->nstars ) {

#ifndef SORT_ENERGYSTAR
		impulseX = impulseX + istar->mass * istar->v.x;
		impulseY = impulseY + istar->mass * istar->v.y;
		impulseZ = impulseZ + istar->mass * istar->v.z;
		
		momentX +=  istar->mass * ( istar->v.y * istar->c.z - istar->c.y * istar->v.z );
		momentY +=  istar->mass * ( istar->v.z * istar->c.x - istar->c.z * istar->v.x );
		momentZ +=  istar->mass * ( istar->v.x * istar->c.y - istar->c.x * istar->v.y );

		kinetic += istar->mass * (istar->v.x * istar->v.x + istar->v.y * istar->v.y + istar->v.z * istar->v.z) / 2.0;
#endif
		
		jstar=istar+1;
		while( jstar - galaxy->stars < galaxy->nstars ) {
#ifdef SORT_ENERGYSTAR
			*ptrp++ = 
#else
			potential +=
#endif 
					G * istar->mass * jstar->mass / sqrt(
				(jstar->c.x - istar->c.x)*(jstar->c.x - istar->c.x) +
				(jstar->c.y - istar->c.y)*(jstar->c.y - istar->c.y) +
				(jstar->c.z - istar->c.z)*(jstar->c.z - istar->c.z)
			);
			jstar++;
		}
		istar++;
	}

#ifdef SORT_ENERGYSTAR
	potential = safeSumArray(pe, maxN2(ptrp-pe));
	impulseX = safeSumStar(galaxy->stars, galaxy->nstars, getImpulseX);
	impulseY = safeSumStar(galaxy->stars, galaxy->nstars, getImpulseY);
	impulseZ = safeSumStar(galaxy->stars, galaxy->nstars, getImpulseZ);
	momentX = safeSumStar(galaxy->stars, galaxy->nstars, getMomentX);
	momentY = safeSumStar(galaxy->stars, galaxy->nstars, getMomentY);
	momentZ = safeSumStar(galaxy->stars, galaxy->nstars, getMomentZ);
	kinetic = safeSumStar(galaxy->stars, galaxy->nstars, getKinetic); 
#endif
	galaxy->Impulse = sqrt(impulseX*impulseX + impulseY*impulseY + impulseZ*impulseZ);
	galaxy->Moment  = sqrt(momentX *momentX  + momentY *momentY  + momentZ *momentZ );
	galaxy->kineticEnergy = kinetic;
	if ( !finite(potential) ) potential=0; 
	galaxy->potentialEnergy = -potential;
}

void 
initLiveGalaxy(Galaxy *galaxy) {
	if ( galaxy->errorLimit<=0) galaxy->errorLimit = 1e-8;
	if ( galaxy->desiredDelta<=0 ) galaxy->desiredDelta=3600;

	galaxy->error = galaxy->errorLimit;
	galaxy->delta=galaxy->desiredDelta;
	galaxy->elapsedTime=0.0;

#ifdef TAYLOR3
	galaxy->firsttime=1;
#endif
	EnergyImpulseGalaxy(galaxy);
}


#define iszero(x)	( fpclassify(x) & FP_ZERO )
#define  CHK_ERRORVAL( what, err, val, prevval )						\
if  ( finite(val) && finite(prevval) ) {							\
	err = fabs(  (val)/(prevval) - 1.0 );							\
	if ( !finite(err) ) 									\
		err =  galaxy->error;								\
} else {											\
	fprintf(stderr, "%s is not finite: val:%G prevval:%G\n", what, (val), (prevval)); 	\
	err = galaxy->error;									\
}

void
liveGalaxy(Galaxy *galaxy, pthread_mutex_t *mutex) {
	double prevEnergy = galaxy->kineticEnergy + galaxy->potentialEnergy;
	double prevImpulse = galaxy->Impulse;
	double prevMoment = galaxy->Moment;
	double errorI, errorE, errorM, error, prevError;

	accelerationGalaxy(galaxy);
	prevError = galaxy->error;

	if ( mutex ) pthread_mutex_lock(mutex);
	while(1) {
		stepGalaxy(galaxy, galaxy->delta);
		EnergyImpulseGalaxy(galaxy);

		CHK_ERRORVAL("Energy", errorE, galaxy->kineticEnergy + galaxy->potentialEnergy, prevEnergy);
		CHK_ERRORVAL("Impulse",errorI, galaxy->Impulse, prevImpulse);
		CHK_ERRORVAL("Moment", errorM, galaxy->Moment, prevMoment);

		/*error=fmax(errorE, fmax(errorI, errorM));*/
		error = errorE; 
	
		if ( error > galaxy->errorLimit ) {
			unstepGalaxy(galaxy, galaxy->delta);
			galaxy->delta /= 2.0;
		} else if ( error*1e2 < galaxy->errorLimit && prevError < galaxy->errorLimit ) {
			unstepGalaxy(galaxy, galaxy->delta);
			galaxy->delta *= 2.0;
		} else {
			galaxy->error = error;
			break;
		}
		prevError=error;
	}
	if ( mutex ) pthread_mutex_unlock(mutex);

	galaxy->elapsedTime += galaxy->delta;
}

void 
printStar(Star *star) {
	printf("\tMass: %e\n", star->mass);
	printf("\tPosit: x:%e y:%e z:%e\n", star->c.x, star->c.y, star->c.z);
	printf("\tSpeed: x:%e y:%e z:%e\n", star->v.x, star->v.y, star->v.z);
}