/*
 * TesselationHelpers.cpp
 *
 *  Created on: Aug 3, 2009
 *      Author: heber
 */

#include "Helpers/MemDebug.hpp"

#include <fstream>

#include "info.hpp"
#include "linkedcell.hpp"
#include "log.hpp"
#include "tesselation.hpp"
#include "tesselationhelpers.hpp"
#include "vector.hpp"
#include "Line.hpp"
#include "vector_ops.hpp"
#include "verbose.hpp"
#include "Plane.hpp"

double DetGet(gsl_matrix * const A, const int inPlace)
{
	Info FunctionInfo(__func__);
  /*
  inPlace = 1 => A is replaced with the LU decomposed copy.
  inPlace = 0 => A is retained, and a copy is used for LU.
  */

  double det;
  int signum;
  gsl_permutation *p = gsl_permutation_alloc(A->size1);
  gsl_matrix *tmpA=0;

  if (inPlace)
  tmpA = A;
  else {
  gsl_matrix *tmpA = gsl_matrix_alloc(A->size1, A->size2);
  gsl_matrix_memcpy(tmpA , A);
  }


  gsl_linalg_LU_decomp(tmpA , p , &signum);
  det = gsl_linalg_LU_det(tmpA , signum);
  gsl_permutation_free(p);
  if (! inPlace)
  gsl_matrix_free(tmpA);

  return det;
};

void GetSphere(Vector * const center, const Vector &a, const Vector &b, const Vector &c, const double RADIUS)
{
	Info FunctionInfo(__func__);
  gsl_matrix *A = gsl_matrix_calloc(3,3);
  double m11, m12, m13, m14;

  for(int i=0;i<3;i++) {
    gsl_matrix_set(A, i, 0, a[i]);
    gsl_matrix_set(A, i, 1, b[i]);
    gsl_matrix_set(A, i, 2, c[i]);
  }
  m11 = DetGet(A, 1);

  for(int i=0;i<3;i++) {
    gsl_matrix_set(A, i, 0, a[i]*a[i] + b[i]*b[i] + c[i]*c[i]);
    gsl_matrix_set(A, i, 1, b[i]);
    gsl_matrix_set(A, i, 2, c[i]);
  }
  m12 = DetGet(A, 1);

  for(int i=0;i<3;i++) {
    gsl_matrix_set(A, i, 0, a[i]*a[i] + b[i]*b[i] + c[i]*c[i]);
    gsl_matrix_set(A, i, 1, a[i]);
    gsl_matrix_set(A, i, 2, c[i]);
  }
  m13 = DetGet(A, 1);

  for(int i=0;i<3;i++) {
    gsl_matrix_set(A, i, 0, a[i]*a[i] + b[i]*b[i] + c[i]*c[i]);
    gsl_matrix_set(A, i, 1, a[i]);
    gsl_matrix_set(A, i, 2, b[i]);
  }
  m14 = DetGet(A, 1);

  if (fabs(m11) < MYEPSILON)
    DoeLog(1) && (eLog()<< Verbose(1) << "three points are colinear." << endl);

  center->at(0) =  0.5 * m12/ m11;
  center->at(1) = -0.5 * m13/ m11;
  center->at(2) =  0.5 * m14/ m11;

  if (fabs(a.distance(*center) - RADIUS) > MYEPSILON)
    DoeLog(1) && (eLog()<< Verbose(1) << "The given center is further way by " << fabs(a.distance(*center) - RADIUS) << " from a than RADIUS." << endl);

  gsl_matrix_free(A);
};



/**
 * Function returns center of sphere with RADIUS, which rests on points a, b, c
 * @param Center this vector will be used for return
 * @param a vector first point of triangle
 * @param b vector second point of triangle
 * @param c vector third point of triangle
 * @param *Umkreismittelpunkt new center point of circumference
 * @param Direction vector indicates up/down
 * @param AlternativeDirection Vector, needed in case the triangles have 90 deg angle
 * @param Halfplaneindicator double indicates whether Direction is up or down
 * @param AlternativeIndicator double indicates in case of orthogonal triangles which direction of AlternativeDirection is suitable
 * @param alpha double angle at a
 * @param beta double, angle at b
 * @param gamma, double, angle at c
 * @param Radius, double
 * @param Umkreisradius double radius of circumscribing circle
 */
void GetCenterOfSphere(Vector* const & Center, const Vector &a, const Vector &b, const Vector &c, Vector * const NewUmkreismittelpunkt, const Vector* const Direction, const Vector* const AlternativeDirection,
    const double HalfplaneIndicator, const double AlternativeIndicator, const double alpha, const double beta, const double gamma, const double RADIUS, const double Umkreisradius)
{
	Info FunctionInfo(__func__);
  Vector TempNormal, helper;
  double Restradius;
  Vector OtherCenter;
  Center->Zero();
  helper = sin(2.*alpha) * a;
  (*Center) += helper;
  helper = sin(2.*beta) * b;
  (*Center) += helper;
  helper = sin(2.*gamma) * c;
  (*Center) += helper;
  //*Center = a * sin(2.*alpha) + b * sin(2.*beta) + c * sin(2.*gamma) ;
  Center->Scale(1./(sin(2.*alpha) + sin(2.*beta) + sin(2.*gamma)));
  (*NewUmkreismittelpunkt) = (*Center);
  DoLog(1) && (Log() << Verbose(1) << "Center of new circumference is " << *NewUmkreismittelpunkt << ".\n");
  // Here we calculated center of circumscribing circle, using barycentric coordinates
  DoLog(1) && (Log() << Verbose(1) << "Center of circumference is " << *Center << " in direction " << *Direction << ".\n");

  TempNormal = a - b;
  helper = a - c;
  TempNormal.VectorProduct(helper);
  if (fabs(HalfplaneIndicator) < MYEPSILON)
    {
      if ((TempNormal.ScalarProduct(*AlternativeDirection) <0 && AlternativeIndicator >0) || (TempNormal.ScalarProduct(*AlternativeDirection) >0 && AlternativeIndicator <0))
        {
          TempNormal *= -1;
        }
    }
  else
    {
      if (((TempNormal.ScalarProduct(*Direction)<0) && (HalfplaneIndicator >0)) || ((TempNormal.ScalarProduct(*Direction)>0) && (HalfplaneIndicator<0)))
        {
          TempNormal *= -1;
        }
    }

  TempNormal.Normalize();
  Restradius = sqrt(RADIUS*RADIUS - Umkreisradius*Umkreisradius);
  DoLog(1) && (Log() << Verbose(1) << "Height of center of circumference to center of sphere is " << Restradius << ".\n");
  TempNormal.Scale(Restradius);
  DoLog(1) && (Log() << Verbose(1) << "Shift vector to sphere of circumference is " << TempNormal << ".\n");
  (*Center) += TempNormal;
  DoLog(1) && (Log() << Verbose(1) << "Center of sphere of circumference is " << *Center << ".\n");
  GetSphere(&OtherCenter, a, b, c, RADIUS);
  DoLog(1) && (Log() << Verbose(1) << "OtherCenter of sphere of circumference is " << OtherCenter << ".\n");
};


/** Constructs the center of the circumcircle defined by three points \a *a, \a *b and \a *c.
 * \param *Center new center on return
 * \param *a first point
 * \param *b second point
 * \param *c third point
 */
void GetCenterofCircumcircle(Vector * const Center, const Vector &a, const Vector &b, const Vector &c)
{
	Info FunctionInfo(__func__);
  Vector helper;
  double alpha, beta, gamma;
  Vector SideA = b - c;
  Vector SideB = c - a;
  Vector SideC = a - b;
  alpha = M_PI - SideB.Angle(SideC);
  beta = M_PI - SideC.Angle(SideA);
  gamma = M_PI - SideA.Angle(SideB);
  //Log() << Verbose(1) << "INFO: alpha = " << alpha/M_PI*180. << ", beta = " << beta/M_PI*180. << ", gamma = " << gamma/M_PI*180. << "." << endl;
  if (fabs(M_PI - alpha - beta - gamma) > HULLEPSILON) {
    DoeLog(2) && (eLog()<< Verbose(2) << "GetCenterofCircumcircle: Sum of angles " << (alpha+beta+gamma)/M_PI*180. << " > 180 degrees by " << fabs(M_PI - alpha - beta - gamma)/M_PI*180. << "!" << endl);
  }

  Center->Zero();
  helper = sin(2.*alpha) * a;
  (*Center) += helper;
  helper = sin(2.*beta) * b;
  (*Center) += helper;
  helper = sin(2.*gamma) * c;
  (*Center) += helper;
  Center->Scale(1./(sin(2.*alpha) + sin(2.*beta) + sin(2.*gamma)));
};

/** Returns the parameter "path length" for a given \a NewSphereCenter relative to \a OldSphereCenter on a circle on the plane \a CirclePlaneNormal with center \a CircleCenter and radius \a CircleRadius.
 * Test whether the \a NewSphereCenter is really on the given plane and in distance \a CircleRadius from \a CircleCenter.
 * It calculates the angle, making it unique on [0,2.*M_PI) by comparing to SearchDirection.
 * Also the new center is invalid if it the same as the old one and does not lie right above (\a NormalVector) the base line (\a CircleCenter).
 * \param CircleCenter Center of the parameter circle
 * \param CirclePlaneNormal normal vector to plane of the parameter circle
 * \param CircleRadius radius of the parameter circle
 * \param NewSphereCenter new center of a circumcircle
 * \param OldSphereCenter old center of a circumcircle, defining the zero "path length" on the parameter circle
 * \param NormalVector normal vector
 * \param SearchDirection search direction to make angle unique on return.
 * \return Angle between \a NewSphereCenter and \a OldSphereCenter relative to \a CircleCenter, 2.*M_PI if one test fails
 */
double GetPathLengthonCircumCircle(const Vector &CircleCenter, const Vector &CirclePlaneNormal, const double CircleRadius, const Vector &NewSphereCenter, const Vector &OldSphereCenter, const Vector &NormalVector, const Vector &SearchDirection)
{
	Info FunctionInfo(__func__);
  Vector helper;
  double radius, alpha;

  Vector RelativeOldSphereCenter = OldSphereCenter - CircleCenter;
  Vector RelativeNewSphereCenter = NewSphereCenter - CircleCenter;
  helper = RelativeNewSphereCenter;
  // test whether new center is on the parameter circle's plane
  if (fabs(helper.ScalarProduct(CirclePlaneNormal)) > HULLEPSILON) {
    DoeLog(1) && (eLog()<< Verbose(1) << "Something's very wrong here: NewSphereCenter is not on the band's plane as desired by " <<fabs(helper.ScalarProduct(CirclePlaneNormal))  << "!" << endl);
    helper.ProjectOntoPlane(CirclePlaneNormal);
  }
  radius = helper.NormSquared();
  // test whether the new center vector has length of CircleRadius
  if (fabs(radius - CircleRadius) > HULLEPSILON)
    DoeLog(1) && (eLog()<< Verbose(1) << "The projected center of the new sphere has radius " << radius << " instead of " << CircleRadius << "." << endl);
  alpha = helper.Angle(RelativeOldSphereCenter);
  // make the angle unique by checking the halfplanes/search direction
  if (helper.ScalarProduct(SearchDirection) < -HULLEPSILON)  // acos is not unique on [0, 2.*M_PI), hence extra check to decide between two half intervals
    alpha = 2.*M_PI - alpha;
  DoLog(1) && (Log() << Verbose(1) << "INFO: RelativeNewSphereCenter is " << helper << ", RelativeOldSphereCenter is " << RelativeOldSphereCenter << " and resulting angle is " << alpha << "." << endl);
  radius = helper.distance(RelativeOldSphereCenter);
  helper.ProjectOntoPlane(NormalVector);
  // check whether new center is somewhat away or at least right over the current baseline to prevent intersecting triangles
  if ((radius > HULLEPSILON) || (helper.Norm() < HULLEPSILON)) {
    DoLog(1) && (Log() << Verbose(1) << "INFO: Distance between old and new center is " << radius << " and between new center and baseline center is " << helper.Norm() << "." << endl);
    return alpha;
  } else {
    DoLog(1) && (Log() << Verbose(1) << "INFO: NewSphereCenter " << RelativeNewSphereCenter << " is too close to RelativeOldSphereCenter" << RelativeOldSphereCenter << "." << endl);
    return 2.*M_PI;
  }
};

struct Intersection {
  Vector x1;
  Vector x2;
  Vector x3;
  Vector x4;
};

/**
 * Intersection calculation function.
 *
 * @param x to find the result for
 * @param function parameter
 */
double MinIntersectDistance(const gsl_vector * x, void *params)
{
	Info FunctionInfo(__func__);
  double retval = 0;
  struct Intersection *I = (struct Intersection *)params;
  Vector intersection;
  for (int i=0;i<NDIM;i++)
    intersection[i] = gsl_vector_get(x, i);

  Vector SideA = I->x1 -I->x2 ;
  Vector HeightA = intersection - I->x1;
  HeightA.ProjectOntoPlane(SideA);

  Vector SideB = I->x3 - I->x4;
  Vector HeightB = intersection - I->x3;
  HeightB.ProjectOntoPlane(SideB);

  retval = HeightA.ScalarProduct(HeightA) + HeightB.ScalarProduct(HeightB);
  //Log() << Verbose(1) << "MinIntersectDistance called, result: " << retval << endl;

  return retval;
};


/**
 * Calculates whether there is an intersection between two lines. The first line
 * always goes through point 1 and point 2 and the second line is given by the
 * connection between point 4 and point 5.
 *
 * @param point 1 of line 1
 * @param point 2 of line 1
 * @param point 1 of line 2
 * @param point 2 of line 2
 *
 * @return true if there is an intersection between the given lines, false otherwise
 */
bool existsIntersection(const Vector &point1, const Vector &point2, const Vector &point3, const Vector &point4)
{
	Info FunctionInfo(__func__);
  bool result;

  struct Intersection par;
    par.x1 = point1;
    par.x2 = point2;
    par.x3 = point3;
    par.x4 = point4;

    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
    gsl_multimin_fminimizer *s = NULL;
    gsl_vector *ss, *x;
    gsl_multimin_function minexFunction;

    size_t iter = 0;
    int status;
    double size;

    /* Starting point */
    x = gsl_vector_alloc(NDIM);
    gsl_vector_set(x, 0, point1[0]);
    gsl_vector_set(x, 1, point1[1]);
    gsl_vector_set(x, 2, point1[2]);

    /* Set initial step sizes to 1 */
    ss = gsl_vector_alloc(NDIM);
    gsl_vector_set_all(ss, 1.0);

    /* Initialize method and iterate */
    minexFunction.n = NDIM;
    minexFunction.f = &MinIntersectDistance;
    minexFunction.params = (void *)&par;

    s = gsl_multimin_fminimizer_alloc(T, NDIM);
    gsl_multimin_fminimizer_set(s, &minexFunction, 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) {
          DoLog(1) && (Log() << Verbose(1) << "converged to minimum" <<  endl);
        }
    } while (status == GSL_CONTINUE && iter < 100);

    // check whether intersection is in between or not
  Vector intersection;
  double t1, t2;
  for (int i = 0; i < NDIM; i++) {
    intersection[i] = gsl_vector_get(s->x, i);
  }

  Vector SideA = par.x2 - par.x1;
  Vector HeightA = intersection - par.x1;

  t1 = HeightA.ScalarProduct(SideA)/SideA.ScalarProduct(SideA);

  Vector SideB = par.x4 - par.x3;
  Vector HeightB = intersection - par.x3;

  t2 = HeightB.ScalarProduct(SideB)/SideB.ScalarProduct(SideB);

  Log() << Verbose(1) << "Intersection " << intersection << " is at "
    << t1 << " for (" << point1 << "," << point2 << ") and at "
    << t2 << " for (" << point3 << "," << point4 << "): ";

  if (((t1 >= 0) && (t1 <= 1)) && ((t2 >= 0) && (t2 <= 1))) {
    DoLog(1) && (Log() << Verbose(1) << "true intersection." << endl);
    result = true;
  } else {
    DoLog(1) && (Log() << Verbose(1) << "intersection out of region of interest." << endl);
    result = false;
  }

  // free minimizer stuff
    gsl_vector_free(x);
    gsl_vector_free(ss);
    gsl_multimin_fminimizer_free(s);

  return result;
};

/** Gets the angle between a point and a reference relative to the provided center.
 * We have two shanks point and reference between which the angle is calculated
 * and by scalar product with OrthogonalVector we decide the interval.
 * @param point to calculate the angle for
 * @param reference to which to calculate the angle
 * @param OrthogonalVector points in direction of [pi,2pi] interval
 *
 * @return angle between point and reference
 */
double GetAngle(const Vector &point, const Vector &reference, const Vector &OrthogonalVector)
{
	Info FunctionInfo(__func__);
  if (reference.IsZero())
    return M_PI;

  // calculate both angles and correct with in-plane vector
  if (point.IsZero())
    return M_PI;
  double phi = point.Angle(reference);
  if (OrthogonalVector.ScalarProduct(point) > 0) {
    phi = 2.*M_PI - phi;
  }

  DoLog(1) && (Log() << Verbose(1) << "INFO: " << point << " has angle " << phi << " with respect to reference " << reference << "." << endl);

  return phi;
}


/** Calculates the volume of a general tetraeder.
 * \param *a first vector
 * \param *a first vector
 * \param *a first vector
 * \param *a first vector
 * \return \f$ \frac{1}{6} \cdot ((a-d) \times (a-c) \cdot  (a-b)) \f$
 */
double CalculateVolumeofGeneralTetraeder(const Vector &a, const Vector &b, const Vector &c, const Vector &d)
{
	Info FunctionInfo(__func__);
  Vector Point, TetraederVector[3];
  double volume;

  TetraederVector[0] = a;
  TetraederVector[1] = b;
  TetraederVector[2] = c;
  for (int j=0;j<3;j++)
    TetraederVector[j].SubtractVector(d);
  Point = TetraederVector[0];
  Point.VectorProduct(TetraederVector[1]);
  volume = 1./6. * fabs(Point.ScalarProduct(TetraederVector[2]));
  return volume;
};


/** Checks for a new special triangle whether one of its edges is already present with one one triangle connected.
 * This enforces that special triangles (i.e. degenerated ones) should at last close the open-edge frontier and not
 * make it bigger (i.e. closing one (the baseline) and opening two new ones).
 * \param TPS[3] nodes of the triangle
 * \return true - there is such a line (i.e. creation of degenerated triangle is valid), false - no such line (don't create)
 */
bool CheckLineCriteriaForDegeneratedTriangle(const BoundaryPointSet * const nodes[3])
{
	Info FunctionInfo(__func__);
  bool result = false;
  int counter = 0;

  // check all three points
  for (int i=0;i<3;i++)
    for (int j=i+1; j<3; j++) {
      if (nodes[i] == NULL) {
        DoLog(1) && (Log() << Verbose(1) << "Node nr. " << i << " is not yet present." << endl);
        result = true;
      } else if (nodes[i]->lines.find(nodes[j]->node->nr) != nodes[i]->lines.end()) {  // there already is a line
        LineMap::const_iterator FindLine;
        pair<LineMap::const_iterator,LineMap::const_iterator> FindPair;
        FindPair = nodes[i]->lines.equal_range(nodes[j]->node->nr);
        for (FindLine = FindPair.first; FindLine != FindPair.second; ++FindLine) {
          // If there is a line with less than two attached triangles, we don't need a new line.
          if (FindLine->second->triangles.size() < 2) {
            counter++;
            break;  // increase counter only once per edge
          }
        }
      } else { // no line
        DoLog(1) && (Log() << Verbose(1) << "The line between " << *nodes[i] << " and " << *nodes[j] << " is not yet present, hence no need for a degenerate triangle." << endl);
        result = true;
      }
    }
  if ((!result) && (counter > 1)) {
    DoLog(1) && (Log() << Verbose(1) << "INFO: Degenerate triangle is ok, at least two, here " << counter << ", existing lines are used." << endl);
    result = true;
  }
  return result;
};


///** Sort function for the candidate list.
// */
//bool SortCandidates(const CandidateForTesselation* candidate1, const CandidateForTesselation* candidate2)
//{
//	Info FunctionInfo(__func__);
//  Vector BaseLineVector, OrthogonalVector, helper;
//  if (candidate1->BaseLine != candidate2->BaseLine) {  // sanity check
//    DoeLog(1) && (eLog()<< Verbose(1) << "sortCandidates was called for two different baselines: " << candidate1->BaseLine << " and " << candidate2->BaseLine << "." << endl);
//    //return false;
//    exit(1);
//  }
//  // create baseline vector
//  BaseLineVector.CopyVector(candidate1->BaseLine->endpoints[1]->node->node);
//  BaseLineVector.SubtractVector(candidate1->BaseLine->endpoints[0]->node->node);
//  BaseLineVector.Normalize();
//
//  // create normal in-plane vector to cope with acos() non-uniqueness on [0,2pi] (note that is pointing in the "right" direction already, hence ">0" test!)
//  helper.CopyVector(candidate1->BaseLine->endpoints[0]->node->node);
//  helper.SubtractVector(candidate1->point->node);
//  OrthogonalVector.CopyVector(&helper);
//  helper.VectorProduct(&BaseLineVector);
//  OrthogonalVector.SubtractVector(&helper);
//  OrthogonalVector.Normalize();
//
//  // calculate both angles and correct with in-plane vector
//  helper.CopyVector(candidate1->point->node);
//  helper.SubtractVector(candidate1->BaseLine->endpoints[0]->node->node);
//  double phi = BaseLineVector.Angle(&helper);
//  if (OrthogonalVector.ScalarProduct(&helper) > 0) {
//    phi = 2.*M_PI - phi;
//  }
//  helper.CopyVector(candidate2->point->node);
//  helper.SubtractVector(candidate1->BaseLine->endpoints[0]->node->node);
//  double psi = BaseLineVector.Angle(&helper);
//  if (OrthogonalVector.ScalarProduct(&helper) > 0) {
//    psi = 2.*M_PI - psi;
//  }
//
//  Log() << Verbose(1) << *candidate1->point << " has angle " << phi << endl;
//  Log() << Verbose(1) << *candidate2->point << " has angle " << psi << endl;
//
//  // return comparison
//  return phi < psi;
//};

/**
 * Finds the point which is second closest to the provided one.
 *
 * @param Point to which to find the second closest other point
 * @param linked cell structure
 *
 * @return point which is second closest to the provided one
 */
TesselPoint* FindSecondClosestTesselPoint(const Vector* Point, const LinkedCell* const LC)
{
	Info FunctionInfo(__func__);
  TesselPoint* closestPoint = NULL;
  TesselPoint* secondClosestPoint = NULL;
  double distance = 1e16;
  double secondDistance = 1e16;
  Vector helper;
  int N[NDIM], Nlower[NDIM], Nupper[NDIM];

  LC->SetIndexToVector(Point); // ignore status as we calculate bounds below sensibly
  for(int i=0;i<NDIM;i++) // store indices of this cell
    N[i] = LC->n[i];
  DoLog(1) && (Log() << Verbose(1) << "INFO: Center cell is " << N[0] << ", " << N[1] << ", " << N[2] << " with No. " << LC->index << "." << endl);

  LC->GetNeighbourBounds(Nlower, Nupper);
  //Log() << Verbose(1) << endl;
  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]++) {
        const LinkedCell::LinkedNodes *List = LC->GetCurrentCell();
        //Log() << Verbose(1) << "The current cell " << LC->n[0] << "," << LC->n[1] << "," << LC->n[2] << endl;
        if (List != NULL) {
          for (LinkedCell::LinkedNodes::const_iterator Runner = List->begin(); Runner != List->end(); Runner++) {
            helper = (*Point) - (*(*Runner)->node);
            double currentNorm = helper. Norm();
            if (currentNorm < distance) {
              // remember second point
              secondDistance = distance;
              secondClosestPoint = closestPoint;
              // mark down new closest point
              distance = currentNorm;
              closestPoint = (*Runner);
              //Log() << Verbose(2) << "INFO: New Second Nearest Neighbour is " << *secondClosestPoint << "." << endl;
            }
          }
        } else {
          eLog() << Verbose(1) << "The current cell " << LC->n[0] << "," << LC->n[1] << ","
            << LC->n[2] << " is invalid!" << endl;
        }
      }

  return secondClosestPoint;
};

/**
 * Finds the point which is closest to the provided one.
 *
 * @param Point to which to find the closest other point
 * @param SecondPoint the second closest other point on return, NULL if none found
 * @param linked cell structure
 *
 * @return point which is closest to the provided one, NULL if none found
 */
TesselPoint* FindClosestTesselPoint(const Vector* Point, TesselPoint *&SecondPoint, const LinkedCell* const LC)
{
	Info FunctionInfo(__func__);
  TesselPoint* closestPoint = NULL;
  SecondPoint = NULL;
  double distance = 1e16;
  double secondDistance = 1e16;
  Vector helper;
  int N[NDIM], Nlower[NDIM], Nupper[NDIM];

  LC->SetIndexToVector(Point); // ignore status as we calculate bounds below sensibly
  for(int i=0;i<NDIM;i++) // store indices of this cell
    N[i] = LC->n[i];
  DoLog(1) && (Log() << Verbose(1) << "INFO: Center cell is " << N[0] << ", " << N[1] << ", " << N[2] << " with No. " << LC->index << "." << endl);

  LC->GetNeighbourBounds(Nlower, Nupper);
  //Log() << Verbose(1) << endl;
  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]++) {
        const LinkedCell::LinkedNodes *List = LC->GetCurrentCell();
        //Log() << Verbose(1) << "The current cell " << LC->n[0] << "," << LC->n[1] << "," << LC->n[2] << endl;
        if (List != NULL) {
          for (LinkedCell::LinkedNodes::const_iterator Runner = List->begin(); Runner != List->end(); Runner++) {
            helper = (*Point) - (*(*Runner)->node);
            double currentNorm = helper.NormSquared();
            if (currentNorm < distance) {
              secondDistance = distance;
              SecondPoint = closestPoint;
              distance = currentNorm;
              closestPoint = (*Runner);
              //Log() << Verbose(1) << "INFO: New Nearest Neighbour is " << *closestPoint << "." << endl;
            } else if (currentNorm < secondDistance) {
              secondDistance = currentNorm;
              SecondPoint = (*Runner);
              //Log() << Verbose(1) << "INFO: New Second Nearest Neighbour is " << *SecondPoint << "." << endl;
            }
          }
        } else {
          eLog() << Verbose(1) << "The current cell " << LC->n[0] << "," << LC->n[1] << ","
            << LC->n[2] << " is invalid!" << endl;
        }
      }
  // output
  if (closestPoint != NULL) {
    DoLog(1) && (Log() << Verbose(1) << "Closest point is " << *closestPoint);
    if (SecondPoint != NULL)
      DoLog(0) && (Log() << Verbose(0) << " and second closest is " << *SecondPoint);
    DoLog(0) && (Log() << Verbose(0) << "." << endl);
  }
  return closestPoint;
};

/** Returns the closest point on \a *Base with respect to \a *OtherBase.
 * \param *out output stream for debugging
 * \param *Base reference line
 * \param *OtherBase other base line
 * \return Vector on reference line that has closest distance
 */
Vector * GetClosestPointBetweenLine(const BoundaryLineSet * const Base, const BoundaryLineSet * const OtherBase)
{
	Info FunctionInfo(__func__);
  // construct the plane of the two baselines (i.e. take both their directional vectors)
  Vector Baseline = (*Base->endpoints[1]->node->node) - (*Base->endpoints[0]->node->node);
  Vector OtherBaseline = (*OtherBase->endpoints[1]->node->node) - (*OtherBase->endpoints[0]->node->node);
  Vector Normal = Baseline;
  Normal.VectorProduct(OtherBaseline);
  Normal.Normalize();
  DoLog(1) && (Log() << Verbose(1) << "First direction is " << Baseline << ", second direction is " << OtherBaseline << ", normal of intersection plane is " << Normal << "." << endl);

  // project one offset point of OtherBase onto this plane (and add plane offset vector)
  Vector NewOffset = (*OtherBase->endpoints[0]->node->node) - (*Base->endpoints[0]->node->node);
  NewOffset.ProjectOntoPlane(Normal);
  NewOffset += (*Base->endpoints[0]->node->node);
  Vector NewDirection = NewOffset + OtherBaseline;

  // calculate the intersection between this projected baseline and Base
  Vector *Intersection = new Vector;
  Line line1 = makeLineThrough(*(Base->endpoints[0]->node->node),*(Base->endpoints[1]->node->node));
  Line line2 = makeLineThrough(NewOffset, NewDirection);
  *Intersection = line1.getIntersection(line2);
  Normal = (*Intersection) - (*Base->endpoints[0]->node->node);
  DoLog(1) && (Log() << Verbose(1) << "Found closest point on " << *Base << " at " << *Intersection << ", factor in line is " << fabs(Normal.ScalarProduct(Baseline)/Baseline.NormSquared()) << "." << endl);

  return Intersection;
};

/** Returns the distance to the plane defined by \a *triangle
 * \param *out output stream for debugging
 * \param *x Vector to calculate distance to
 * \param *triangle triangle defining plane
 * \return distance between \a *x and plane defined by \a *triangle, -1 - if something went wrong
 */
double DistanceToTrianglePlane(const Vector *x, const BoundaryTriangleSet * const triangle)
{
	Info FunctionInfo(__func__);
  double distance = 0.;
  if (x == NULL) {
    return -1;
  }
  distance = x->DistanceToSpace(triangle->getPlane());
  return distance;
};

/** Creates the objects in a VRML file.
 * \param *out output stream for debugging
 * \param *vrmlfile output stream for tecplot data
 * \param *Tess Tesselation structure with constructed triangles
 * \param *mol molecule structure with atom positions
 */
void WriteVrmlFile(ofstream * const vrmlfile, const Tesselation * const Tess, const PointCloud * const cloud)
{
	Info FunctionInfo(__func__);
  TesselPoint *Walker = NULL;
  int i;
  Vector *center = cloud->GetCenter();
  if (vrmlfile != NULL) {
    //Log() << Verbose(1) << "Writing Raster3D file ... ";
    *vrmlfile << "#VRML V2.0 utf8" << endl;
    *vrmlfile << "#Created by molecuilder" << endl;
    *vrmlfile << "#All atoms as spheres" << endl;
    cloud->GoToFirst();
    while (!cloud->IsEnd()) {
      Walker = cloud->GetPoint();
      *vrmlfile << "Sphere {" << endl << "  "; // 2 is sphere type
      for (i=0;i<NDIM;i++)
        *vrmlfile << Walker->node->at(i)-center->at(i) << " ";
      *vrmlfile << "\t0.1\t1. 1. 1." << endl; // radius 0.05 and white as colour
      cloud->GoToNext();
    }

    *vrmlfile << "# All tesselation triangles" << endl;
    for (TriangleMap::const_iterator TriangleRunner = Tess->TrianglesOnBoundary.begin(); TriangleRunner != Tess->TrianglesOnBoundary.end(); TriangleRunner++) {
      *vrmlfile << "1" << endl << "  "; // 1 is triangle type
      for (i=0;i<3;i++) { // print each node
        for (int j=0;j<NDIM;j++)  // and for each node all NDIM coordinates
          *vrmlfile << TriangleRunner->second->endpoints[i]->node->node->at(j)-center->at(j) << " ";
        *vrmlfile << "\t";
      }
      *vrmlfile << "1. 0. 0." << endl;  // red as colour
      *vrmlfile << "18" << endl << "  0.5 0.5 0.5" << endl; // 18 is transparency type for previous object
    }
  } else {
    DoeLog(1) && (eLog()<< Verbose(1) << "Given vrmlfile is " << vrmlfile << "." << endl);
  }
  delete(center);
};

/** Writes additionally the current sphere (i.e. the last triangle to file).
 * \param *out output stream for debugging
 * \param *rasterfile output stream for tecplot data
 * \param *Tess Tesselation structure with constructed triangles
 * \param *mol molecule structure with atom positions
 */
void IncludeSphereinRaster3D(ofstream * const rasterfile, const Tesselation * const Tess, const PointCloud * const cloud)
{
	Info FunctionInfo(__func__);
  Vector helper;

  if (Tess->LastTriangle != NULL) {
    // include the current position of the virtual sphere in the temporary raster3d file
    Vector *center = cloud->GetCenter();
    // make the circumsphere's center absolute again
    Vector helper = (1./3.) * ((*Tess->LastTriangle->endpoints[0]->node->node) +
                               (*Tess->LastTriangle->endpoints[1]->node->node) +
                               (*Tess->LastTriangle->endpoints[2]->node->node));
    helper -= (*center);
    // and add to file plus translucency object
    *rasterfile << "# current virtual sphere\n";
    *rasterfile << "8\n  25.0    0.6     -1.0 -1.0 -1.0     0.2        0 0 0 0\n";
    *rasterfile << "2\n  " << helper[0] << " " << helper[1] << " " << helper[2] << "\t" << 5. << "\t1 0 0\n";
    *rasterfile << "9\n  terminating special property\n";
    delete(center);
  }
};

/** Creates the objects in a raster3d file (renderable with a header.r3d).
 * \param *out output stream for debugging
 * \param *rasterfile output stream for tecplot data
 * \param *Tess Tesselation structure with constructed triangles
 * \param *mol molecule structure with atom positions
 */
void WriteRaster3dFile(ofstream * const rasterfile, const Tesselation * const Tess, const PointCloud * const cloud)
{
	Info FunctionInfo(__func__);
  TesselPoint *Walker = NULL;
  int i;
  Vector *center = cloud->GetCenter();
  if (rasterfile != NULL) {
    //Log() << Verbose(1) << "Writing Raster3D file ... ";
    *rasterfile << "# Raster3D object description, created by MoleCuilder" << endl;
    *rasterfile << "@header.r3d" << endl;
    *rasterfile << "# All atoms as spheres" << endl;
    cloud->GoToFirst();
    while (!cloud->IsEnd()) {
      Walker = cloud->GetPoint();
      *rasterfile << "2" << endl << "  ";  // 2 is sphere type
      for (int j=0;j<NDIM;j++) { // and for each node all NDIM coordinates
        const double tmp = Walker->node->at(j)-center->at(j);
        *rasterfile << ((fabs(tmp) < MYEPSILON) ? 0 : tmp) << " ";
      }
      *rasterfile << "\t0.1\t1. 1. 1." << endl; // radius 0.05 and white as colour
      cloud->GoToNext();
    }

    *rasterfile << "# All tesselation triangles" << endl;
    *rasterfile << "8\n  25. -1.   1. 1. 1.   0.0    0 0 0 2\n  SOLID     1.0 0.0 0.0\n  BACKFACE  0.3 0.3 1.0   0 0\n";
    for (TriangleMap::const_iterator TriangleRunner = Tess->TrianglesOnBoundary.begin(); TriangleRunner != Tess->TrianglesOnBoundary.end(); TriangleRunner++) {
      *rasterfile << "1" << endl << "  ";  // 1 is triangle type
      for (i=0;i<3;i++) {  // print each node
        for (int j=0;j<NDIM;j++) { // and for each node all NDIM coordinates
          const double tmp = TriangleRunner->second->endpoints[i]->node->node->at(j)-center->at(j);
          *rasterfile << ((fabs(tmp) < MYEPSILON) ? 0 : tmp) << " ";
        }
        *rasterfile << "\t";
      }
      *rasterfile << "1. 0. 0." << endl;  // red as colour
      //*rasterfile << "18" << endl << "  0.5 0.5 0.5" << endl;  // 18 is transparency type for previous object
    }
    *rasterfile << "9\n#  terminating special property\n";
  } else {
    DoeLog(1) && (eLog()<< Verbose(1) << "Given rasterfile is " << rasterfile << "." << endl);
  }
  IncludeSphereinRaster3D(rasterfile, Tess, cloud);
  delete(center);
};

/** This function creates the tecplot file, displaying the tesselation of the hull.
 * \param *out output stream for debugging
 * \param *tecplot output stream for tecplot data
 * \param N arbitrary number to differentiate various zones in the tecplot format
 */
void WriteTecplotFile(ofstream * const tecplot, const Tesselation * const TesselStruct, const PointCloud * const cloud, const int N)
{
	Info FunctionInfo(__func__);
  if ((tecplot != NULL) && (TesselStruct != NULL)) {
    // write header
    *tecplot << "TITLE = \"3D CONVEX SHELL\"" << endl;
    *tecplot << "VARIABLES = \"X\" \"Y\" \"Z\" \"U\"" << endl;
    *tecplot << "ZONE T=\"";
    if (N < 0) {
      *tecplot << cloud->GetName();
    } else {
      *tecplot << N << "-";
      if (TesselStruct->LastTriangle != NULL) {
        for (int i=0;i<3;i++)
          *tecplot << (i==0 ? "" : "_") << TesselStruct->LastTriangle->endpoints[i]->node->getName();
      } else {
        *tecplot << "none";
      }
    }
    *tecplot << "\", N=" << TesselStruct->PointsOnBoundary.size() << ", E=" << TesselStruct->TrianglesOnBoundary.size() << ", DATAPACKING=POINT, ZONETYPE=FETRIANGLE" << endl;
    const int MaxId=cloud->GetMaxId();
    int *LookupList = new int[MaxId];
    for (int i=0; i< MaxId ; i++){
      LookupList[i] = -1;
    }

    // print atom coordinates
    int Counter = 1;
    TesselPoint *Walker = NULL;
    for (PointMap::const_iterator target = TesselStruct->PointsOnBoundary.begin(); target != TesselStruct->PointsOnBoundary.end(); ++target) {
      Walker = target->second->node;
      LookupList[Walker->nr] = Counter++;
      for (int i=0;i<NDIM;i++) {
        const double tmp = Walker->node->at(i);
        *tecplot << ((fabs(tmp) < MYEPSILON) ? 0 : tmp) << " ";
      }
      *tecplot << target->second->value << endl;
    }
    *tecplot << endl;
    // print connectivity
    DoLog(1) && (Log() << Verbose(1) << "The following triangles were created:" << endl);
    for (TriangleMap::const_iterator runner = TesselStruct->TrianglesOnBoundary.begin(); runner != TesselStruct->TrianglesOnBoundary.end(); runner++) {
      DoLog(1) && (Log() << Verbose(1) << " " << runner->second->endpoints[0]->node->getName() << "<->" << runner->second->endpoints[1]->node->getName() << "<->" << runner->second->endpoints[2]->node->getName() << endl);
      *tecplot << LookupList[runner->second->endpoints[0]->node->nr] << " " << LookupList[runner->second->endpoints[1]->node->nr] << " " << LookupList[runner->second->endpoints[2]->node->nr] << endl;
    }
    delete[] (LookupList);
  }
};

/** Calculates the concavity for each of the BoundaryPointSet's in a Tesselation.
 * Sets BoundaryPointSet::value equal to the number of connected lines that are not convex.
 * \param *out output stream for debugging
 * \param *TesselStruct pointer to Tesselation structure
 */
void CalculateConcavityPerBoundaryPoint(const Tesselation * const TesselStruct)
{
	Info FunctionInfo(__func__);
  class BoundaryPointSet *point = NULL;
  class BoundaryLineSet *line = NULL;

  // calculate remaining concavity
  for (PointMap::const_iterator PointRunner = TesselStruct->PointsOnBoundary.begin(); PointRunner != TesselStruct->PointsOnBoundary.end(); PointRunner++) {
    point = PointRunner->second;
    DoLog(1) && (Log() << Verbose(1) << "INFO: Current point is " << *point << "." << endl);
    point->value = 0;
    for (LineMap::iterator LineRunner = point->lines.begin(); LineRunner != point->lines.end(); LineRunner++) {
      line = LineRunner->second;
      //Log() << Verbose(1) << "INFO: Current line of point " << *point << " is " << *line << "." << endl;
      if (!line->CheckConvexityCriterion())
        point->value += 1;
    }
  }
};


/** Checks whether each BoundaryLineSet in the Tesselation has two triangles.
 * \param *out output stream for debugging
 * \param *TesselStruct
 * \return true - all have exactly two triangles, false - some not, list is printed to screen
 */
bool CheckListOfBaselines(const Tesselation * const TesselStruct)
{
	Info FunctionInfo(__func__);
  LineMap::const_iterator testline;
  bool result = false;
  int counter = 0;

  DoLog(1) && (Log() << Verbose(1) << "Check: List of Baselines with not two connected triangles:" << endl);
  for (testline = TesselStruct->LinesOnBoundary.begin(); testline != TesselStruct->LinesOnBoundary.end(); testline++) {
    if (testline->second->triangles.size() != 2) {
      DoLog(2) && (Log() << Verbose(2) << *testline->second << "\t" << testline->second->triangles.size() << endl);
      counter++;
    }
  }
  if (counter == 0) {
    DoLog(1) && (Log() << Verbose(1) << "None." << endl);
    result = true;
  }
  return result;
}

/** Counts the number of triangle pairs that contain the given polygon.
 * \param *P polygon with endpoints to look for
 * \param *T set of triangles to create pairs from containing \a *P
 */
int CountTrianglePairContainingPolygon(const BoundaryPolygonSet * const P, const TriangleSet * const T)
{
  Info FunctionInfo(__func__);
  // check number of endpoints in *P
  if (P->endpoints.size() != 4) {
    DoeLog(1) && (eLog()<< Verbose(1) << "CountTrianglePairContainingPolygon works only on polygons with 4 nodes!" << endl);
    return 0;
  }

  // check number of triangles in *T
  if (T->size() < 2) {
    DoeLog(1) && (eLog()<< Verbose(1) << "Not enough triangles to have pairs!" << endl);
    return 0;
  }

  DoLog(0) && (Log() << Verbose(0) << "Polygon is " << *P << endl);
  // create each pair, get the endpoints and check whether *P is contained.
  int counter = 0;
  PointSet Trianglenodes;
  class BoundaryPolygonSet PairTrianglenodes;
  for(TriangleSet::iterator Walker = T->begin(); Walker != T->end(); Walker++) {
    for (int i=0;i<3;i++)
      Trianglenodes.insert((*Walker)->endpoints[i]);

    for(TriangleSet::iterator PairWalker = Walker; PairWalker != T->end(); PairWalker++) {
      if (Walker != PairWalker) { // skip first
        PairTrianglenodes.endpoints = Trianglenodes;
        for (int i=0;i<3;i++)
          PairTrianglenodes.endpoints.insert((*PairWalker)->endpoints[i]);
        const int size = PairTrianglenodes.endpoints.size();
        if (size == 4) {
          DoLog(0) && (Log() << Verbose(0) << " Current pair of triangles: " << **Walker << "," << **PairWalker << " with " << size << " distinct endpoints:" << PairTrianglenodes << endl);
          // now check
          if (PairTrianglenodes.ContainsPresentTupel(P)) {
            counter++;
            DoLog(0) && (Log() << Verbose(0) << "  ACCEPT: Matches with " << *P << endl);
          } else {
            DoLog(0) && (Log() << Verbose(0) << "  REJECT: No match with " << *P << endl);
          }
        } else {
          DoLog(0) && (Log() << Verbose(0) << "  REJECT: Less than four endpoints." << endl);
        }
      }
    }
    Trianglenodes.clear();
  }
  return counter;
};

/** Checks whether two give polygons have two or more points in common.
 * \param *P1 first polygon
 * \param *P2 second polygon
 * \return true - are connected, false = are note
 */
bool ArePolygonsEdgeConnected(const BoundaryPolygonSet * const P1, const BoundaryPolygonSet * const P2)
{
  Info FunctionInfo(__func__);
  int counter = 0;
  for(PointSet::const_iterator Runner = P1->endpoints.begin(); Runner != P1->endpoints.end(); Runner++) {
    if (P2->ContainsBoundaryPoint((*Runner))) {
      counter++;
      DoLog(1) && (Log() << Verbose(1) << *(*Runner) << " of second polygon is found in the first one." << endl);
      return true;
    }
  }
  return false;
};

/** Combines second into the first and deletes the second.
 * \param *P1 first polygon, contains all nodes on return
 * \param *&P2 second polygon, is deleted.
 */
void CombinePolygons(BoundaryPolygonSet * const P1, BoundaryPolygonSet * &P2)
{
  Info FunctionInfo(__func__);
  pair <PointSet::iterator, bool> Tester;
  for(PointSet::iterator Runner = P2->endpoints.begin(); Runner != P2->endpoints.end(); Runner++) {
    Tester = P1->endpoints.insert((*Runner));
    if (Tester.second)
      DoLog(0) && (Log() << Verbose(0) << "Inserting endpoint " << *(*Runner) << " into first polygon." << endl);
  }
  P2->endpoints.clear();
  delete(P2);
};