/* school_alg.c, Copyright (c) 2005-2006 David C. Lambert <dcl@panix.com>
 *
 * Permission to use, copy, modify, distribute, and sell this software and its
 * documentation for any purpose is hereby granted without fee, provided that
 * the above copyright notice appear in all copies and that both that
 * copyright notice and this permission notice appear in supporting
 * documentation.  No representations are made about the suitability of this
 * software for any purpose.  It is provided "as is" without express or 
 * implied warranty.
 */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

#include "yarandom.h"
#include "glschool_alg.h"

/* for xscreensaver */
#undef drand48
#define drand48()	frand(1.0)

#define RAD2DEG		(180.0/3.1415926535)


static inline double
norm(double *dv)
{
	return sqrt(dv[0]*dv[0] + dv[1]*dv[1] + dv[2]*dv[2]);
}


static inline void
addVector(double *v, double *d)
{
	v[0] += d[0];
	v[1] += d[1];
	v[2] += d[2];
}


static inline void
clearVector(double *v)
{
	v[0] = v[1] = v[2] = 0.0;
}


static inline void
scaleVector(double *v, double s)
{
	v[0] *= s;
	v[1] *= s;
	v[2] *= s;
}


static inline void
addScaledVector(double *v, double *d, double s)
{
	v[0] += d[0]*s;
	v[1] += d[1]*s;
	v[2] += d[2]*s;
}


static inline void
getDifferenceVector(double *v0, double *v1, double *diff)
{
	diff[0] = v0[0] - v1[0];
	diff[1] = v0[1] - v1[1];
	diff[2] = v0[2] - v1[2];
}


void
glschool_initFish(Fish *f, double *mins, double *ranges)
{
	int i;

	for(i = 0; i < 3; i++) {
		FISH_IPOS(f, i) = mins[i] + drand48()*ranges[i];
		FISH_IACC(f, i) = 0.0;
		FISH_IVEL(f, i) = drand48();
		FISH_IMAGIC(f, i) = 0.70 + 0.60*drand48();
		FISH_IOLDVEL(f, i) = 0.0;
	}
}


void
glschool_initFishes(School *s)
{
	int		i;
	Fish	*f = (Fish *)0;
	int		nFish = SCHOOL_NFISH(s);
	BBox	*bbox = &SCHOOL_BBOX(s);
	double	*mins = BBOX_MINS(bbox);
	double	*ranges = SCHOOL_BBRANGES(s);
	Fish	*theFishes = SCHOOL_FISHES(s);

	for(i = 0, f = theFishes; i < nFish; i++, f++)
		glschool_initFish(f, mins, ranges);
}


static void
applyFishMovements(Fish *f, BBox *bbox, double minVel, double maxVel, double momentum)
{
	int			i;
	int			oob = 0;
	double		vMag = 0.0;

	for(i = 0; i < 3; i++) {
		double	pos = FISH_IPOS(f, i);
		oob = (pos > BBOX_IMAX(bbox, i) || pos < BBOX_IMIN(bbox, i));
		if (oob == 0) FISH_IVEL(f, i) += FISH_IACC(f, i) * FISH_IMAGIC(f, i);
		vMag += (FISH_IVEL(f, i) * FISH_IVEL(f, i));
	}
	vMag = sqrt(vMag);

	if (vMag > maxVel)
		scaleVector(FISH_VEL(f), maxVel/vMag);
	else if (vMag < minVel)
		scaleVector(FISH_VEL(f), minVel/vMag);

	for(i = 0; i < 3; i++) {
		FISH_IVEL(f, i) = momentum * FISH_IOLDVEL(f, i) + (1.0-momentum) * FISH_IVEL(f, i);
		FISH_IPOS(f, i) += FISH_IVEL(f, i);
		FISH_IOLDVEL(f, i) = FISH_IVEL(f, i);

		if (FISH_IPOS(f, i) < BBOX_IMIN(bbox, i))
			FISH_IPOS(f, i) = BBOX_IMAX(bbox, i);
		else if (FISH_IPOS(f, i) > BBOX_IMAX(bbox, i))
			FISH_IPOS(f, i) = BBOX_IMIN(bbox, i);
	}
}


void
glschool_applyMovements(School *s)
{
	int		i = 0;
	Fish	*f = (Fish *)0;
	int		nFish = SCHOOL_NFISH(s);
	double	minVel = SCHOOL_MINVEL(s);
	double	maxVel = SCHOOL_MAXVEL(s);
	double	momentum = SCHOOL_MOMENTUM(s);
	BBox	*bbox = &SCHOOL_BBOX(s);
	Fish	*theFishes = SCHOOL_FISHES(s);

	for(i = 0, f = theFishes; i < nFish; i++, f++)
		applyFishMovements(f, bbox, minVel, maxVel, momentum);
}


School *
glschool_initSchool(int nFish, double accLimit, double maxV, double minV, double distExp, double momentum,
		   double minRadius, double avoidFact, double matchFact, double centerFact, double targetFact,
		   double distComp)
{
	School	*s = (School *)0;

	if ((s = (School *)malloc(sizeof(School))) == (School *)0) {
		perror("initSchool School allocation failed: ");
		return s;
	}

	if ((SCHOOL_FISHES(s) = (Fish *)malloc(sizeof(Fish)*nFish)) == (Fish *)0) {
		perror("initSchool Fish array allocation failed: ");
		free(s);
		return (School *)0;
	}

	SCHOOL_NFISH(s) = nFish;
	SCHOOL_ACCLIMIT(s) = accLimit;
	SCHOOL_MAXVEL(s) = maxV;
	SCHOOL_MINVEL(s) = minV;
	SCHOOL_DISTEXP(s) = distExp;
	SCHOOL_MOMENTUM(s) = momentum;
	SCHOOL_MINRADIUS(s) = minRadius;
	SCHOOL_MINRADIUSEXP(s) = pow(minRadius, distExp);
	SCHOOL_MATCHFACT(s) = matchFact;
	SCHOOL_AVOIDFACT(s) = avoidFact;
	SCHOOL_CENTERFACT(s) = centerFact;
	SCHOOL_TARGETFACT(s) = targetFact;
	SCHOOL_DISTCOMP(s) = distComp;

	return s;
}

void
glschool_freeSchool(School *s)
{
	free(SCHOOL_FISHES(s));
	free(s);
}

void
glschool_setBBox(School *s, double xMin, double xMax, double yMin, double yMax, double zMin, double zMax)
{
	int		i;
	BBox	*bbox = &SCHOOL_BBOX(s);

	BBOX_XMIN(bbox) = xMin; BBOX_XMAX(bbox) = xMax;
	BBOX_YMIN(bbox) = yMin; BBOX_YMAX(bbox) = yMax;
	BBOX_ZMIN(bbox) = zMin; BBOX_ZMAX(bbox) = zMax;

	for(i = 0; i < 3; i++) {
		SCHOOL_IMID(s, i) = BBOX_IMID(bbox, i);
		SCHOOL_IRANGE(s, i) = BBOX_IRANGE(bbox, i);
	}
}


void
glschool_newGoal(School *s)
{
	SCHOOL_IGOAL(s,0) = 0.85*(drand48()-0.5)*SCHOOL_IRANGE(s,0) + SCHOOL_IMID(s,0);
	SCHOOL_IGOAL(s,1) = 0.40*(drand48()-0.5)*SCHOOL_IRANGE(s,1) + SCHOOL_IMID(s,1);
	SCHOOL_IGOAL(s,2) = 0.85*(drand48()-0.5)*SCHOOL_IRANGE(s,2) + SCHOOL_IMID(s,2);
}


double
glschool_computeNormalAndThetaToPlusZ(double *v, double *xV)
{
	double	x1 = 0.0;
	double	y1 = 0.0;
	double	z1 = 1.0;
	double	x2 = v[0];
	double	y2 = v[1];
	double	z2 = v[2];
	double	theta = 0.0;
	double	xVNorm = 0.0;
	double	sinTheta = 0.0;
	double	v2Norm = norm(v);

	if (v2Norm == 0.0) {
		xV[1] = 1.0;
		xV[0] = xV[2] = 0.0;
		return theta;
	}
	xV[0] = (y1*z2 - z1*y2);
	xV[1] = -(x1*z2 - z1*x2);
	xV[2] = (x1*y2 - y1*x2);
	xVNorm = norm(xV);

	sinTheta = xVNorm/v2Norm;
	return (asin(sinTheta) * RAD2DEG);
}


int
glschool_computeGroupVectors(School *s, Fish *ref, double *avoidance, double *centroid, double *avgVel)
{
	int		i;
	double	dist;
	double	adjDist;
	double	diffVect[3];
	int		neighborCount = 0;
	Fish	*test = (Fish *)0;
	int		nFish = SCHOOL_NFISH(s);
	double	distExp = SCHOOL_DISTEXP(s);
	double	distComp = SCHOOL_DISTCOMP(s);
	double	minRadiusExp = SCHOOL_MINRADIUSEXP(s);
	Fish	*fishes = SCHOOL_FISHES(s);

	for(i = 0, test = fishes; i < nFish; i++, test++) {
		if (test == ref) continue;

		getDifferenceVector(FISH_POS(ref), FISH_POS(test), diffVect);

		dist = norm(diffVect) - distComp;
		if (dist < 0.0) dist = 0.1;

		adjDist = pow(dist, distExp);
		if (adjDist > minRadiusExp) continue;

		neighborCount++;

		addVector(avgVel, FISH_VEL(test));
		addVector(centroid, FISH_POS(test));

		addScaledVector(avoidance, diffVect, 1.0/adjDist);
	}
	if (neighborCount > 0) {
		scaleVector(avgVel, 1.0/neighborCount);
		scaleVector(centroid, 1.0/neighborCount);
	}
	return neighborCount;
}


void
glschool_computeAccelerations(School *s)
{
	int		i;
	int		j;
	int		neighborCount;
	double	dist;
	double	adjDist;
	double	accMag;
	double	avgVel[3];
	double	diffVect[3];
	double	centroid[3];
	double	avoidance[3];
	Fish	*ref = (Fish *)0;
	int		nFish = SCHOOL_NFISH(s);
	double	*goal = SCHOOL_GOAL(s);
	double	distExp = SCHOOL_DISTEXP(s);
	double	distComp = SCHOOL_DISTCOMP(s);
	double	avoidFact = SCHOOL_AVOIDFACT(s);
	double	matchFact = SCHOOL_MATCHFACT(s);
	double	centerFact = SCHOOL_CENTERFACT(s);
	double	targetFact = SCHOOL_TARGETFACT(s);
	double	accLimit = SCHOOL_ACCLIMIT(s);
	double	minRadius = SCHOOL_MINRADIUS(s);
	Fish	*fishes = SCHOOL_FISHES(s);

	for(i = 0, ref = fishes; i < nFish; i++, ref++) {
		clearVector(avgVel);
		clearVector(centroid);
		clearVector(avoidance);
		clearVector(FISH_ACC(ref));
		neighborCount = glschool_computeGroupVectors(s, ref, avoidance, centroid, avgVel);

		/* avoidanceAccel[] = avoidance[] * AvoidFact */
		scaleVector(avoidance, avoidFact);
		addVector(FISH_ACC(ref), avoidance);

		accMag = norm(FISH_ACC(ref));
		if (neighborCount > 0 && accMag < accLimit) {
			for(j = 0; j < 3; j++) {
				FISH_IAVGVEL(ref, j) = avgVel[j];
				FISH_IACC(ref, j) += ((avgVel[j] - FISH_IVEL(ref, j)) * matchFact);
			}

			accMag = norm(FISH_ACC(ref));
			if (accMag < accLimit) {
				for(j = 0; j < 3; j++)
					FISH_IACC(ref, j) += ((centroid[j] - FISH_IPOS(ref, j)) * centerFact);
			}
		}

		accMag = norm(FISH_ACC(ref));
		if (accMag < accLimit) {
			getDifferenceVector(goal, FISH_POS(ref), diffVect);

			dist = norm(diffVect) - distComp;
			if (dist < 0.0) dist = 0.1;

			/*adjDist = pow(dist, distExp);*/
			if (dist > minRadius) {
				adjDist = pow(dist, distExp);
				for(j = 0; j < 3; j++)
					FISH_IACC(ref, j) += (diffVect[j]*targetFact/adjDist);
			}
		}
	}
}