/*
 * ellipsoid.cpp
 *
 *	Created on: Jan 20, 2009
 *			Author: heber
 */

#include "ellipsoid.hpp"

/** Determines squared distance for a given point \a x to surface of ellipsoid.
 * \param x given point
 * \param EllipsoidCenter center of ellipsoid
 * \param EllipsoidLength[3] three lengths of half axis of ellipsoid
 * \param EllipsoidAngle[3] three rotation angles of ellipsoid
 * \return squared distance from point to surface
 */
double SquaredDistanceToEllipsoid(Vector &x, Vector &EllipsoidCenter, double *EllipsoidLength, double *EllipsoidAngle)
{
	Vector helper, RefPoint;
	double distance = -1.;
	double Matrix[NDIM*NDIM];
	double InverseLength[3];
	double psi,theta,phi; // euler angles in ZX'Z'' convention

	//cout << Verbose(3) << "Begin of SquaredDistanceToEllipsoid" << endl;

	for(int i=0;i<3;i++)
		InverseLength[i] = 1./EllipsoidLength[i];

	// 1. translate coordinate system so that ellipsoid center is in origin
	helper.CopyVector(&x);
	helper.SubtractVector(&EllipsoidCenter);
	RefPoint.CopyVector(&helper);
	//cout << Verbose(4) << "Translated given point is at " << RefPoint << "." << endl;

	// 2. transform coordinate system by inverse of rotation matrix and of diagonal matrix
	psi = EllipsoidAngle[0];
	theta = EllipsoidAngle[1];
	phi = EllipsoidAngle[2];
	Matrix[0] = cos(psi)*cos(phi) - sin(psi)*cos(theta)*sin(phi);
	Matrix[1] = -cos(psi)*sin(phi) - sin(psi)*cos(theta)*cos(phi);
	Matrix[2] = sin(psi)*sin(theta);
	Matrix[3] = sin(psi)*cos(phi) + cos(psi)*cos(theta)*sin(phi);
	Matrix[4] = cos(psi)*cos(theta)*cos(phi) - sin(psi)*sin(phi);
	Matrix[5] = -cos(psi)*sin(theta);
	Matrix[6] = sin(theta)*sin(phi);
	Matrix[7] = sin(theta)*cos(phi);
	Matrix[8] = cos(theta);
	helper.MatrixMultiplication(Matrix);
	helper.Scale(InverseLength);
	//cout << Verbose(4) << "Transformed RefPoint is at " << helper << "." << endl;

	// 3. construct intersection point with unit sphere and ray between origin and x
	helper.Normalize(); // is simply normalizes vector in distance direction
	//cout << Verbose(4) << "Transformed intersection is at " << helper << "." << endl;

	// 4. transform back the constructed intersection point
	psi = -EllipsoidAngle[0];
	theta = -EllipsoidAngle[1];
	phi = -EllipsoidAngle[2];
	helper.Scale(EllipsoidLength);
	Matrix[0] = cos(psi)*cos(phi) - sin(psi)*cos(theta)*sin(phi);
	Matrix[1] = -cos(psi)*sin(phi) - sin(psi)*cos(theta)*cos(phi);
	Matrix[2] = sin(psi)*sin(theta);
	Matrix[3] = sin(psi)*cos(phi) + cos(psi)*cos(theta)*sin(phi);
	Matrix[4] = cos(psi)*cos(theta)*cos(phi) - sin(psi)*sin(phi);
	Matrix[5] = -cos(psi)*sin(theta);
	Matrix[6] = sin(theta)*sin(phi);
	Matrix[7] = sin(theta)*cos(phi);
	Matrix[8] = cos(theta);
	helper.MatrixMultiplication(Matrix);
	//cout << Verbose(4) << "Intersection is at " << helper << "." << endl;

	// 5. determine distance between backtransformed point and x
	distance = RefPoint.DistanceSquared(&helper);
	//cout << Verbose(4) << "Squared distance between intersection and RefPoint is " << distance << "." << endl;

	return distance;
	//cout << Verbose(3) << "End of SquaredDistanceToEllipsoid" << endl;
};

/** structure for ellipsoid minimisation containing points to fit to.
 */
struct EllipsoidMinimisation {
	int N;			//!< dimension of vector set
	Vector *x;	//!< array of vectors
};

/** Sum of squared distance to ellipsoid to be minimised.
 * \param *x parameters for the ellipsoid
 * \param *params EllipsoidMinimisation with set of data points to minimise distance to and dimension
 * \return sum of squared distance, \sa SquaredDistanceToEllipsoid()
 */
double SumSquaredDistance (const gsl_vector * x, void * params)
{
	Vector *set= ((struct EllipsoidMinimisation *)params)->x;
	int N = ((struct EllipsoidMinimisation *)params)->N;
	double SumDistance = 0.;
	double distance;
	Vector Center;
	double EllipsoidLength[3], EllipsoidAngle[3];

	// put parameters into suitable ellipsoid form
	for (int i=0;i<3;i++) {
		Center.x[i] = gsl_vector_get(x, i+0);
		EllipsoidLength[i] = gsl_vector_get(x, i+3);
		EllipsoidAngle[i] = gsl_vector_get(x, i+6);
	}

	// go through all points and sum distance
	for (int i=0;i<N;i++) {
		distance = SquaredDistanceToEllipsoid(set[i], Center, EllipsoidLength, EllipsoidAngle);
		if (!isnan(distance)) {
			SumDistance += distance;
		} else {
			SumDistance = GSL_NAN;
			break;
		}
	}

	//cout << "Current summed distance is " << SumDistance << "." << endl;
	return SumDistance;
};

/** Finds best fitting ellipsoid parameter set in Least square sense for a given point set.
 * \param *out output stream for debugging
 * \param *set given point set
 * \param N number of points in set
 * \param EllipsoidParamter[3] three parameters in ellipsoid equation
 * \return true - fit successful, false - fit impossible
 */
bool FitPointSetToEllipsoid(ofstream *out, Vector *set, int N, Vector *EllipsoidCenter, double *EllipsoidLength, double *EllipsoidAngle)
{
	int status = GSL_SUCCESS;
	*out << Verbose(2) << "Begin of FitPointSetToEllipsoid " << endl;
	if (N >= 3) { // check that enough points are given (9 d.o.f.)
		struct EllipsoidMinimisation par;
		const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
		gsl_multimin_fminimizer *s = NULL;
		gsl_vector *ss, *x;
		gsl_multimin_function minex_func;

		size_t iter = 0;
		double size;

		/* Starting point */
		x = gsl_vector_alloc (9);
		for (int i=0;i<3;i++) {
			gsl_vector_set (x, i+0, EllipsoidCenter->x[i]);
			gsl_vector_set (x, i+3, EllipsoidLength[i]);
			gsl_vector_set (x, i+6, EllipsoidAngle[i]);
		}
		par.x = set;
		par.N = N;

		/* Set initial step sizes */
		ss = gsl_vector_alloc (9);
		for (int i=0;i<3;i++) {
			gsl_vector_set (ss, i+0, 0.1);
			gsl_vector_set (ss, i+3, 1.0);
			gsl_vector_set (ss, i+6, M_PI/20.);
		}

		/* Initialize method and iterate */
		minex_func.n = 9;
		minex_func.f = &SumSquaredDistance;
		minex_func.params = (void *)&par;

		s = gsl_multimin_fminimizer_alloc (T, 9);
		gsl_multimin_fminimizer_set (s, &minex_func, x, ss);

		do {
			iter++;
			status = gsl_multimin_fminimizer_iterate(s);

			if (status)
				break;

			size = gsl_multimin_fminimizer_size (s);
			status = gsl_multimin_test_size (size, 1e-2);

			if (status == GSL_SUCCESS) {
				for (int i=0;i<3;i++) {
					EllipsoidCenter->x[i] = gsl_vector_get (s->x,i+0);
					EllipsoidLength[i] = gsl_vector_get (s->x, i+3);
					EllipsoidAngle[i] = gsl_vector_get (s->x, i+6);
				}
				*out << setprecision(3) << Verbose(4) << "Converged fit at: " << *EllipsoidCenter << ", lengths " << EllipsoidLength[0] << ", " << EllipsoidLength[1] << ", " << EllipsoidLength[2] << ", angles " << EllipsoidAngle[0] << ", " << EllipsoidAngle[1] << ", " << EllipsoidAngle[2] << " with summed distance " << s->fval << "." << endl;
			}

		} while (status == GSL_CONTINUE && iter < 1000);

		gsl_vector_free(x);
		gsl_vector_free(ss);
		gsl_multimin_fminimizer_free (s);

	} else {
		*out << Verbose(3) << "Not enough points provided for fit to ellipsoid." << endl;
		return false;
	}
	*out << Verbose(2) << "End of FitPointSetToEllipsoid" << endl;
	if (status == GSL_SUCCESS)
		return true;
	else
		return false;
};

/** Picks a number of random points from a LC neighbourhood as a fitting set.
 * \param *out output stream for debugging
 * \param *T Tesselation containing boundary points
 * \param *LC linked cell list of all atoms
 * \param *&x random point set on return (not allocated!)
 * \param PointsToPick number of points in set to pick
 */
void PickRandomNeighbouredPointSet(ofstream *out, class Tesselation *T, class LinkedCell *LC, Vector *&x, size_t PointsToPick)
{
  size_t PointsLeft = 0;
  size_t PointsPicked = 0;
	int Nlower[NDIM], Nupper[NDIM];
	set<int> PickedAtomNrs;	 // ordered list of picked atoms
	set<int>::iterator current;
	int index;
	atom *Candidate = NULL;
	LinkedAtoms *List = NULL;
	*out << Verbose(2) << "Begin of PickRandomPointSet" << endl;

	// allocate array
	if (x == NULL) {
		x = new Vector[PointsToPick];
	} else {
		*out << "WARNING: Given pointer to vector array seems already allocated." << endl;
	}

	do {
		for(int i=0;i<NDIM;i++) // pick three random indices
			LC->n[i] = (rand() % LC->N[i]);
		*out << Verbose(2) << "INFO: Center cell is " << LC->n[0] << ", " << LC->n[1] << ", " << LC->n[2] << " ... ";
		// get random cell
		List = LC->GetCurrentCell();
		if (List == NULL) {	// set index to it
			continue;
		}
		*out << "with No. " << LC->index << "." << endl;

		*out << Verbose(2) << "LC Intervals:";
		for (int i=0;i<NDIM;i++) {
			Nlower[i] = ((LC->n[i]-1) >= 0) ? LC->n[i]-1 : 0;
			Nupper[i] = ((LC->n[i]+1) < LC->N[i]) ? LC->n[i]+1 : LC->N[i]-1;
			*out << " [" << Nlower[i] << "," << Nupper[i] << "] ";
		}
		*out << endl;

		// count whether there are sufficient atoms in this cell+neighbors
		PointsLeft=0;
		for (LC->n[0] = Nlower[0]; LC->n[0] <= Nupper[0]; LC->n[0]++)
			for (LC->n[1] = Nlower[1]; LC->n[1] <= Nupper[1]; LC->n[1]++)
				for (LC->n[2] = Nlower[2]; LC->n[2] <= Nupper[2]; LC->n[2]++) {
					List = LC->GetCurrentCell();
					PointsLeft += List->size();
				}
		*out << Verbose(2) << "There are " << PointsLeft << " atoms in this neighbourhood." << endl;
		if (PointsLeft < PointsToPick) {	// ensure that we can pick enough points in its neighbourhood at all.
			continue;
		}

		// pre-pick a fixed number of atoms
		PickedAtomNrs.clear();
		do {
			index = (rand() % PointsLeft);
			current = PickedAtomNrs.find(index);	// not present?
			if (current == PickedAtomNrs.end()) {
				//*out << Verbose(2) << "Picking atom nr. " << index << "." << endl;
				PickedAtomNrs.insert(index);
			}
		} while (PickedAtomNrs.size() < PointsToPick);

		index = 0; // now go through all and pick those whose from PickedAtomsNr
		PointsPicked=0;
		current = PickedAtomNrs.begin();
		for (LC->n[0] = Nlower[0]; LC->n[0] <= Nupper[0]; LC->n[0]++)
			for (LC->n[1] = Nlower[1]; LC->n[1] <= Nupper[1]; LC->n[1]++)
				for (LC->n[2] = Nlower[2]; LC->n[2] <= Nupper[2]; LC->n[2]++) {
					List = LC->GetCurrentCell();
//					*out << Verbose(2) << "Current cell is " << LC->n[0] << ", " << LC->n[1] << ", " << LC->n[2] << " with No. " << LC->index << " containing " << List->size() << " points." << endl;
					if (List != NULL) {
//						if (List->begin() != List->end())
//							*out << Verbose(2) << "Going through candidates ... " << endl;
//						else
//							*out << Verbose(2) << "Cell is empty ... " << endl;
						for (LinkedAtoms::iterator Runner = List->begin(); Runner != List->end(); Runner++) {
							if ((current != PickedAtomNrs.end()) && (*current == index)) {
								Candidate = (*Runner);
								*out << Verbose(2) << "Current picked node is " << **Runner << " with index " << index << "." << endl;
								x[PointsPicked++].CopyVector(&(Candidate->x));		// we have one more atom picked
								current++;		// next pre-picked atom
							}
							index++;	// next atom nr.
						}
//					} else {
//						*out << Verbose(2) << "List for this index not allocated!" << endl;
					}
				}
		*out << Verbose(2) << "The following points were picked: " << endl;
		for (size_t i=0;i<PointsPicked;i++)
			*out << Verbose(2) << x[i] << endl;
		if (PointsPicked == PointsToPick)	// break out of loop if we have all
			break;
	} while(1);

	*out << Verbose(2) << "End of PickRandomPointSet" << endl;
};

/** Picks a number of random points from a set of boundary points as a fitting set.
 * \param *out output stream for debugging
 * \param *T Tesselation containing boundary points
 * \param *&x random point set on return (not allocated!)
 * \param PointsToPick number of points in set to pick
 */
void PickRandomPointSet(ofstream *out, class Tesselation *T, Vector *&x, size_t PointsToPick)
{
  size_t PointsLeft = (size_t) T->PointsOnBoundaryCount;
  size_t PointsPicked = 0;
	double value, threshold;
	PointMap *List = &T->PointsOnBoundary;
	*out << Verbose(2) << "Begin of PickRandomPointSet" << endl;

	// allocate array
	if (x == NULL) {
		x = new Vector[PointsToPick];
	} else {
		*out << "WARNING: Given pointer to vector array seems already allocated." << endl;
	}

	if (List != NULL)
		for (PointMap::iterator Runner = List->begin(); Runner != List->end(); Runner++) {
			threshold = 1. - (double)(PointsToPick - PointsPicked)/(double)PointsLeft;
			value = (double)rand()/(double)RAND_MAX;
			//*out << Verbose(3) << "Current node is " << *Runner->second->node << " with " << value << " ... " << threshold << ": ";
			if (value > threshold) {
				x[PointsPicked].CopyVector(&(Runner->second->node->x));
				PointsPicked++;
				//*out << "IN." << endl;
			} else {
				//*out << "OUT." << endl;
			}
			PointsLeft--;
		}
	*out << Verbose(2) << "The following points were picked: " << endl;
	for (size_t i=0;i<PointsPicked;i++)
		*out << Verbose(3) << x[i] << endl;

	*out << Verbose(2) << "End of PickRandomPointSet" << endl;
};

/** Finds best fitting ellipsoid parameter set in least square sense for a given point set.
 * \param *out output stream for debugging
 * \param *T Tesselation containing boundary points
 * \param *LCList linked cell list of all atoms
 * \param N number of unique points in ellipsoid fit, must be greater equal 6
 * \param number of fits (i.e. parameter sets in output file)
 * \param *filename name for output file
 */
void FindDistributionOfEllipsoids(ofstream *out, class Tesselation *T, class LinkedCell *LCList, int N, int number, const char *filename)
{
	ofstream output;
	Vector *x = NULL;
	Vector Center;
	Vector EllipsoidCenter;
	double EllipsoidLength[3];
	double EllipsoidAngle[3];
	double distance, MaxDistance, MinDistance;
	*out << Verbose(0) << "Begin of FindDistributionOfEllipsoids" << endl;

	// construct center of gravity of boundary point set for initial ellipsoid center
	Center.Zero();
	for (PointMap::iterator Runner = T->PointsOnBoundary.begin(); Runner != T->PointsOnBoundary.end(); Runner++)
		Center.AddVector(&Runner->second->node->x);
	Center.Scale(1./T->PointsOnBoundaryCount);
	*out << Verbose(1) << "Center is at " << Center << "." << endl;

	// Output header
	output.open(filename, ios::trunc);
	output << "# Nr.\tCenterX\tCenterY\tCenterZ\ta\tb\tc\tpsi\ttheta\tphi" << endl;

	// loop over desired number of parameter sets
	for (;number >0;number--) {
		*out << Verbose(1) << "Determining data set " << number << " ... " << endl;
		// pick the point set
		x = NULL;
		//PickRandomPointSet(out, T, LCList, x, N);
		PickRandomNeighbouredPointSet(out, T, LCList, x, N);

		// calculate some sensible starting values for parameter fit
		MaxDistance = 0.;
		MinDistance = x[0].ScalarProduct(&x[0]);
		for (int i=0;i<N;i++) {
			distance = x[i].ScalarProduct(&x[i]);
			if (distance > MaxDistance)
				MaxDistance = distance;
			if (distance < MinDistance)
				MinDistance = distance;
		}
		//*out << Verbose(2) << "MinDistance " << MinDistance << ", MaxDistance " << MaxDistance << "." << endl;
		EllipsoidCenter.CopyVector(&Center);	// use Center of Gravity as initial center of ellipsoid
		for (int i=0;i<3;i++)
			EllipsoidAngle[i] = 0.;
		EllipsoidLength[0] = sqrt(MaxDistance);
		EllipsoidLength[1] = sqrt((MaxDistance+MinDistance)/2.);
		EllipsoidLength[2] = sqrt(MinDistance);

		// fit the parameters
		if (FitPointSetToEllipsoid(out, x, N, &EllipsoidCenter, &EllipsoidLength[0], &EllipsoidAngle[0])) {
			*out << Verbose(1) << "Picking succeeded!" << endl;
			// output obtained parameter set
			output << number << "\t";
			for (int i=0;i<3;i++)
				output << setprecision(9) << EllipsoidCenter.x[i] << "\t";
			for (int i=0;i<3;i++)
				output << setprecision(9) << EllipsoidLength[i] << "\t";
			for (int i=0;i<3;i++)
				output << setprecision(9) << EllipsoidAngle[i] << "\t";
			output << endl;
		} else { // increase N to pick one more
			*out << Verbose(1) << "Picking failed!" << endl;
			number++;
		}
		delete[](x);	// free allocated memory for point set
	}
	// close output and finish
	output.close();

	*out << Verbose(0) << "End of FindDistributionOfEllipsoids" << endl;
};