/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2010-2012 University of Bonn. All rights reserved.
 * 
 *
 *   This file is part of MoleCuilder.
 *
 *    MoleCuilder 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.
 *
 *    MoleCuilder 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 MoleCuilder.  If not, see <http://www.gnu.org/licenses/>.
 */

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

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "CodePatterns/MemDebug.hpp"

#include <fstream>

#include "tesselationhelpers.hpp"

#include "BoundaryLineSet.hpp"
#include "BoundaryPointSet.hpp"
#include "BoundaryPolygonSet.hpp"
#include "BoundaryTriangleSet.hpp"
#include "CandidateForTesselation.hpp"
#include "CodePatterns/Info.hpp"
#include "CodePatterns/Log.hpp"
#include "CodePatterns/Verbose.hpp"
#include "LinearAlgebra/Line.hpp"
#include "LinearAlgebra/LinearSystemOfEquations.hpp"
#include "LinearAlgebra/Plane.hpp"
#include "LinearAlgebra/RealSpaceMatrix.hpp"
#include "LinearAlgebra/Vector.hpp"
#include "LinearAlgebra/vector_ops.hpp"
#include "LinkedCell/IPointCloud.hpp"
#include "LinkedCell/linkedcell.hpp"
#include "tesselation.hpp"

void GetSphere(Vector * const center, const Vector &a, const Vector &b, const Vector &c, const double RADIUS)
{
	//Info FunctionInfo(__func__);
  RealSpaceMatrix mat;
  double m11, m12, m13, m14;

  for(int i=0;i<3;i++) {
    mat.set(i, 0, a[i]);
    mat.set(i, 1, b[i]);
    mat.set(i, 2, c[i]);
  }
  m11 = mat.determinant();

  for(int i=0;i<3;i++) {
    mat.set(i, 0, a[i]*a[i] + b[i]*b[i] + c[i]*c[i]);
    mat.set(i, 1, b[i]);
    mat.set(i, 2, c[i]);
  }
  m12 = mat.determinant();

  for(int i=0;i<3;i++) {
    mat.set(i, 0, a[i]*a[i] + b[i]*b[i] + c[i]*c[i]);
    mat.set(i, 1, a[i]);
    mat.set(i, 2, c[i]);
  }
  m13 = mat.determinant();

  for(int i=0;i<3;i++) {
    mat.set(i, 0, a[i]*a[i] + b[i]*b[i] + c[i]*c[i]);
    mat.set(i, 1, a[i]);
    mat.set(i, 2, b[i]);
  }
  m14 = mat.determinant();

  if (fabs(m11) < MYEPSILON)
    ELOG(1, "three points are colinear.");

  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)
    ELOG(1, "The given center is further way by " << fabs(a.distance(*center) - RADIUS) << " from a than RADIUS.");
};



/**
 * 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);
  LOG(4, "DEBUG: Center of new circumference is " << *NewUmkreismittelpunkt << ".");
  // Here we calculated center of circumscribing circle, using barycentric coordinates
  LOG(4, "DEBUG: Center of circumference is " << *Center << " in direction " << *Direction << ".");

  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);
  LOG(5, "DEBUG: Height of center of circumference to center of sphere is " << Restradius << ".");
  TempNormal.Scale(Restradius);
  LOG(5, "DEBUG: Shift vector to sphere of circumference is " << TempNormal << ".");
  (*Center) += TempNormal;
  LOG(5, "DEBUG: Center of sphere of circumference is " << *Center << ".");
  GetSphere(&OtherCenter, a, b, c, RADIUS);
  LOG(5, "DEBUG: OtherCenter of sphere of circumference is " << OtherCenter << ".");
};


/** 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 &Center, const Vector &a, const Vector &b, const Vector &c)
{
	//Info FunctionInfo(__func__);
  Vector helper;
  Vector SideA = b - c;
  Vector SideB = c - a;
  Vector SideC = a - b;

  helper[0] = SideA.NormSquared()*(SideB.NormSquared()+SideC.NormSquared() - SideA.NormSquared());
  helper[1] = SideB.NormSquared()*(SideC.NormSquared()+SideA.NormSquared() - SideB.NormSquared());
  helper[2] = SideC.NormSquared()*(SideA.NormSquared()+SideB.NormSquared() - SideC.NormSquared());

  Center.Zero();
  Center += helper[0] * a;
  Center += helper[1] * b;
  Center += helper[2] * c;
  if (fabs(helper[0]+helper[1]+helper[2]) > MYEPSILON)
    Center.Scale(1./(helper[0]+helper[1]+helper[2]));
  LOG(4, "DEBUG: Center (2nd algo) is at " << Center << ".");
};

/** 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.
 * \param HULLEPSILON machine precision for tesselation points
 * \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, const double HULLEPSILON)
{
	//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) {
    ELOG(1, "Something's very wrong here: NewSphereCenter is not on the band's plane as desired by " <<fabs(helper.ScalarProduct(CirclePlaneNormal))  << "!");
    helper.ProjectOntoPlane(CirclePlaneNormal);
  }
  radius = helper.NormSquared();
  // test whether the new center vector has length of CircleRadius
  if (fabs(radius - CircleRadius) > HULLEPSILON)
    ELOG(1, "The projected center of the new sphere has radius " << radius << " instead of " << CircleRadius << ".");
  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;
  LOG(3, "DEBUG: RelativeNewSphereCenter is " << helper << ", RelativeOldSphereCenter is " << RelativeOldSphereCenter << " and resulting angle is " << alpha << ".");
  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)) {
    LOG(4, "DEBUG: Distance between old and new center is " << radius << " and between new center and baseline center is " << helper.Norm() << ".");
    return alpha;
  } else {
    LOG(3, "DEBUG: NewSphereCenter " << RelativeNewSphereCenter << " is too close to RelativeOldSphereCenter" << RelativeOldSphereCenter << ".");
    return 2.*M_PI;
  }
};

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

/** 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;
  }

  LOG(1, "INFO: " << point << " has angle " << phi << " with respect to reference " << reference << ".");

  return phi;
}


/** Calculates the volume of a general tetraeder.
 * \param *a first vector
 * \param *b second vector
 * \param *c third vector
 * \param *d fourth 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;
};

/** Calculates the area of a general triangle.
 * We use the Heron's formula of area, [Bronstein, S. 138]
 * \param &A first vector
 * \param &B second vector
 * \param &C third vector
 * \return \f$ \frac{1}{6} \cdot ((a-d) \times (a-c) \cdot  (a-b)) \f$
 */
double CalculateAreaofGeneralTriangle(const Vector &A, const Vector &B, const Vector &C)
{
  //Info FunctionInfo(__func__);

  const double sidea = B.distance(C);
  const double sideb = A.distance(C);
  const double sidec = A.distance(B);
  const double s = (sidea+sideb+sidec)/2.;

  const double area = sqrt(s*(s-sidea)*(s-sideb)*(s-sidec));
  return area;
};


/** 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) {
        LOG(1, "Node nr. " << i << " is not yet present.");
        result = true;
      } else if (nodes[i]->lines.find(nodes[j]->node->getNr()) != 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->getNr());
        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
        LOG(1, "The line between " << *nodes[i] << " and " << *nodes[j] << " is not yet present, hence no need for a degenerate triangle.");
        result = true;
      }
    }
  if ((!result) && (counter > 1)) {
    LOG(1, "INFO: Degenerate triangle is ok, at least two, here " << counter << ", existing lines are used.");
    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
//    ELOG(1, "sortCandidates was called for two different baselines: " << candidate1->BaseLine << " and " << candidate2->BaseLine << ".");
//    //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(1, *candidate1->point << " has angle " << phi);
//  LOG(1, *candidate2->point << " has angle " << psi);
//
//  // 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_deprecated* 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];
  LOG(2, "DEBUG: Center cell is " << N[0] << ", " << N[1] << ", " << N[2] << " with No. " << LC->index << ".");

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

  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_deprecated* 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];
  LOG(2, "DEBUG: Center cell is " << N[0] << ", " << N[1] << ", " << N[2] << " with No. " << LC->index << ".");

  LC->GetNeighbourBounds(Nlower, Nupper);
  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 TesselPointSTLList *List = LC->GetCurrentCell();
        //LOG(1, "The current cell " << LC->n[0] << "," << LC->n[1] << "," << LC->n[2]);
        if (List != NULL) {
          for (TesselPointSTLList::const_iterator Runner = List->begin(); Runner != List->end(); Runner++) {
            helper = (Point) - ((*Runner)->getPosition());
            double currentNorm = helper.NormSquared();
            if (currentNorm < distance) {
              secondDistance = distance;
              SecondPoint = closestPoint;
              distance = currentNorm;
              closestPoint = (*Runner);
              //LOG(1, "INFO: New Nearest Neighbour is " << *closestPoint << ".");
            } else if (currentNorm < secondDistance) {
              secondDistance = currentNorm;
              SecondPoint = (*Runner);
              //LOG(1, "INFO: New Second Nearest Neighbour is " << *SecondPoint << ".");
            }
          }
        } else {
          ELOG(1, "The current cell " << LC->n[0] << "," << LC->n[1] << "," << LC->n[2] << " is invalid!");
        }
      }
  // output
  if (closestPoint != NULL) {
    if (DoLog(3)) {
      std::stringstream output;
      output << "Closest point is " << *closestPoint;
      if (SecondPoint != NULL)
        output << " and second closest is " << *SecondPoint;
      LOG(3, "DEBUG: " << output.str() << ".");
    }
  }
  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->getPosition()) - (Base->endpoints[0]->node->getPosition());
  Vector OtherBaseline = (OtherBase->endpoints[1]->node->getPosition()) - (OtherBase->endpoints[0]->node->getPosition());
  Vector Normal = Baseline;
  Normal.VectorProduct(OtherBaseline);
  Normal.Normalize();
  LOG(1, "First direction is " << Baseline << ", second direction is " << OtherBaseline << ", normal of intersection plane is " << Normal << ".");

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

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

  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, IPointCloud & cloud)
{
	//Info FunctionInfo(__func__);
  TesselPoint *Walker = NULL;
  int i;
  Vector *center = cloud.GetCenter();
  if (vrmlfile != NULL) {
    LOG(1, "INFO: 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->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->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 {
    ELOG(1, "Given vrmlfile is " << vrmlfile << ".");
  }
  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, IPointCloud & 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->getPosition()) +
                               (Tess->LastTriangle->endpoints[1]->node->getPosition()) +
                               (Tess->LastTriangle->endpoints[2]->node->getPosition()));
    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, IPointCloud & cloud)
{
	//Info FunctionInfo(__func__);
  TesselPoint *Walker = NULL;
  int i;
  Vector *center = cloud.GetCenter();
  if (rasterfile != NULL) {
    LOG(1, "INFO: 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->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->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 {
    ELOG(1, "Given rasterfile is " << rasterfile << ".");
  }
  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, IPointCloud & 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();
    ASSERT(MaxId >= 0, "WriteTecplotFile() - negative MaxId? No atoms present?");
    int *LookupList = new int[MaxId+1];
    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;
      ASSERT(Walker->getNr() <= MaxId, "WriteTecplotFile() - Id of particle greater than MaxId.");
      LookupList[Walker->getNr()] = Counter++;
      for (int i=0;i<NDIM;i++) {
        const double tmp = Walker->at(i);
        *tecplot << ((fabs(tmp) < MYEPSILON) ? 0 : tmp) << " ";
      }
      *tecplot << target->second->value << endl;
    }
    *tecplot << endl;
    // print connectivity
    LOG(1, "The following triangles were created:");
    for (TriangleMap::const_iterator runner = TesselStruct->TrianglesOnBoundary.begin(); runner != TesselStruct->TrianglesOnBoundary.end(); runner++) {
      LOG(1, " " << runner->second->endpoints[0]->node->getName() << "<->" << runner->second->endpoints[1]->node->getName() << "<->" << runner->second->endpoints[2]->node->getName());
      *tecplot << LookupList[runner->second->endpoints[0]->node->getNr()] << " " << LookupList[runner->second->endpoints[1]->node->getNr()] << " " << LookupList[runner->second->endpoints[2]->node->getNr()] << 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;
  class BoundaryTriangleSet *triangle = NULL;
  double ConcavityPerLine = 0.;
  double ConcavityPerTriangle = 0.;
  double area = 0.;
  double totalarea = 0.;

  for (PointMap::const_iterator PointRunner = TesselStruct->PointsOnBoundary.begin(); PointRunner != TesselStruct->PointsOnBoundary.end(); PointRunner++) {
    point = PointRunner->second;
    LOG(1, "INFO: Current point is " << *point << ".");

    // calculate mean concavity over all connected line
    ConcavityPerLine = 0.;
    for (LineMap::iterator LineRunner = point->lines.begin(); LineRunner != point->lines.end(); LineRunner++) {
      line = LineRunner->second;
      //LOG(1, "INFO: Current line of point " << *point << " is " << *line << ".");
      ConcavityPerLine -= line->CalculateConvexity();
    }
    ConcavityPerLine /= point->lines.size();

    // weigh with total area of the surrounding triangles
    totalarea  = 0.;
    TriangleSet *triangles = TesselStruct->GetAllTriangles(PointRunner->second);
    for (TriangleSet::iterator TriangleRunner = triangles->begin(); TriangleRunner != triangles->end(); ++TriangleRunner) {
      totalarea += CalculateAreaofGeneralTriangle((*TriangleRunner)->endpoints[0]->node->getPosition() , (*TriangleRunner)->endpoints[1]->node->getPosition() , (*TriangleRunner)->endpoints[2]->node->getPosition());
    }
    ConcavityPerLine *= totalarea;

    // calculate mean concavity over all attached triangles
    ConcavityPerTriangle = 0.;
    for (TriangleSet::const_iterator TriangleRunner = triangles->begin(); TriangleRunner != triangles->end(); ++TriangleRunner) {
      line = (*TriangleRunner)->GetThirdLine(PointRunner->second);
      triangle = line->GetOtherTriangle(*TriangleRunner);
      area = CalculateAreaofGeneralTriangle(triangle->endpoints[0]->node->getPosition() , triangle->endpoints[1]->node->getPosition() , triangle->endpoints[2]->node->getPosition());
      area += CalculateAreaofGeneralTriangle((*TriangleRunner)->endpoints[0]->node->getPosition() , (*TriangleRunner)->endpoints[1]->node->getPosition() , (*TriangleRunner)->endpoints[2]->node->getPosition());
      area *= -line->CalculateConvexity();
      if (area > 0)
        ConcavityPerTriangle += area;
//      else
//        ConcavityPerTriangle -= area;
    }
    ConcavityPerTriangle /= triangles->size()/totalarea;
    delete(triangles);

    // add up
    point->value = ConcavityPerLine + ConcavityPerTriangle;
  }
};



/** Calculates the concavity for each of the BoundaryPointSet's in a Tesselation.
 * Sets BoundaryPointSet::value equal to the nearest distance to convex envelope.
 * \param *out output stream for debugging
 * \param *TesselStruct pointer to Tesselation structure
 * \param *Convex pointer to convex Tesselation structure as reference
 */
void CalculateConstrictionPerBoundaryPoint(const Tesselation * const TesselStruct, const Tesselation * const Convex)
{
  //Info FunctionInfo(__func__);
  double distance = 0.;

  for (PointMap::const_iterator PointRunner = TesselStruct->PointsOnBoundary.begin(); PointRunner != TesselStruct->PointsOnBoundary.end(); PointRunner++) {
    ELOG(1, "INFO: Current point is " << * PointRunner->second << ".");

    distance = 0.;
    for (TriangleMap::const_iterator TriangleRunner = Convex->TrianglesOnBoundary.begin(); TriangleRunner != Convex->TrianglesOnBoundary.end(); TriangleRunner++) {
      const double CurrentDistance = Convex->GetDistanceSquaredToTriangle(PointRunner->second->node->getPosition() , TriangleRunner->second);
      if (CurrentDistance < distance)
        distance = CurrentDistance;
    }

    PointRunner->second->value = distance;
  }
};

/** 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;

  LOG(1, "Check: List of Baselines with not two connected triangles:");
  for (testline = TesselStruct->LinesOnBoundary.begin(); testline != TesselStruct->LinesOnBoundary.end(); testline++) {
    if (testline->second->triangles.size() != 2) {
      LOG(2, *testline->second << "\t" << testline->second->triangles.size());
      counter++;
    }
  }
  if (counter == 0) {
    LOG(1, "None.");
    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) {
    ELOG(1, "CountTrianglePairContainingPolygon works only on polygons with 4 nodes!");
    return 0;
  }

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

  LOG(3, "DEBUG: Polygon is " << *P);
  // 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) {
          LOG(4, "DEBUG:  Current pair of triangles: " << **Walker << "," << **PairWalker << " with " << size << " distinct endpoints:" << PairTrianglenodes);
          // now check
          if (PairTrianglenodes.ContainsPresentTupel(P)) {
            counter++;
            LOG(5, "  ACCEPT: Matches with " << *P);
          } else {
            LOG(5, "  REJECT: No match with " << *P);
          }
        } else {
          LOG(5, "  REJECT: Less than four endpoints.");
        }
      }
    }
    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++;
      LOG(5, "DEBUG: " << *(*Runner) << " of second polygon is found in the first one.");
      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)
      LOG(4, "DEBUG: Inserting endpoint " << *(*Runner) << " into first polygon.");
  }
  P2->endpoints.clear();
  delete(P2);
};