#include "molecules.hpp"
#include "boundary.hpp"


// =================spaeter gebrauchte
void Find_next_suitable_point(atom a, atom b, atom Candidate, int n, Vector *d1, Vector *d2, double *Storage, const double RADIUS, molecule mol);
void Find_non_convex_border(Tesselation Tess, molecule mol);


// ======================================== Points on Boundary =================================

BoundaryPointSet::BoundaryPointSet()
{
  LinesCount = 0;
  Nr = -1;
};

BoundaryPointSet::BoundaryPointSet(atom *Walker)
{
  node = Walker;
  LinesCount = 0;
  Nr = Walker->nr;
};

BoundaryPointSet::~BoundaryPointSet()
{
  cout << Verbose(5) << "Erasing point nr. " << Nr << "." << endl;
  node = NULL;
};

void BoundaryPointSet::AddLine(class BoundaryLineSet *line) 
{
  cout << Verbose(6) << "Adding line " << *line << " to " << *this << "." << endl;
  if (line->endpoints[0] == this) {
    lines.insert ( LinePair( line->endpoints[1]->Nr, line) );
  } else {
    lines.insert ( LinePair( line->endpoints[0]->Nr, line) );
  }
  LinesCount++;
};

ostream & operator << (ostream &ost, BoundaryPointSet &a) 
{
  ost << "[" << a.Nr << "|" << a.node->Name << "]";
  return ost;
};

// ======================================== Lines on Boundary =================================

BoundaryLineSet::BoundaryLineSet()
{
  for (int i=0;i<2;i++)
    endpoints[i] = NULL;
  TrianglesCount = 0;
  Nr = -1;
};

BoundaryLineSet::BoundaryLineSet(class BoundaryPointSet *Point[2], int number)
{
  // set number
  Nr = number;
  // set endpoints in ascending order
  SetEndpointsOrdered(endpoints, Point[0], Point[1]);
  // add this line to the hash maps of both endpoints
  Point[0]->AddLine(this);
  Point[1]->AddLine(this);
  // clear triangles list
  TrianglesCount = 0;
  cout << Verbose(5) << "New Line with endpoints " << *this << "." << endl;
};

BoundaryLineSet::~BoundaryLineSet()
{
  for (int i=0;i<2;i++) {
    cout << Verbose(5) << "Erasing Line Nr. " << Nr << " in boundary point " << *endpoints[i] << "." << endl; 
    endpoints[i]->lines.erase(Nr);
    LineMap::iterator tester = endpoints[i]->lines.begin();
    tester++;
    if (tester == endpoints[i]->lines.end()) {
      cout << Verbose(5) << *endpoints[i] << " has no more lines it's attached to, erasing." << endl;
      delete(endpoints[i]);
    } else
      cout << Verbose(5) << *endpoints[i] << " has still lines it's attached to." << endl;
  }
};

void BoundaryLineSet::AddTriangle(class BoundaryTriangleSet *triangle) 
{
  cout << Verbose(6) << "Add " << triangle->Nr << " to line " << *this << "." << endl;
  triangles.insert ( TrianglePair( TrianglesCount, triangle) );
  TrianglesCount++;
};

ostream & operator << (ostream &ost, BoundaryLineSet &a) 
{
  ost << "[" << a.Nr << "|" << a.endpoints[0]->node->Name << "," << a.endpoints[1]->node->Name << "]";
  return ost;
};

// ======================================== Triangles on Boundary =================================


BoundaryTriangleSet::BoundaryTriangleSet()
{
  for (int i=0;i<3;i++) {
    endpoints[i] = NULL;
    lines[i] = NULL;
  }
  Nr = -1;
};

BoundaryTriangleSet::BoundaryTriangleSet(class BoundaryLineSet *line[3], int number)
{
  // set number
  Nr = number;
  // set lines
  cout << Verbose(5) << "New triangle " << Nr << ":" << endl;
  for (int i=0;i<3;i++) {
    lines[i] = line[i];
    lines[i]->AddTriangle(this);
  }
  // get ascending order of endpoints
  map <int, class BoundaryPointSet * > OrderMap;
  for(int i=0;i<3;i++)  // for all three lines
    for (int j=0;j<2;j++) { // for both endpoints
      OrderMap.insert ( pair <int, class BoundaryPointSet * >( line[i]->endpoints[j]->Nr, line[i]->endpoints[j]) );
      // and we don't care whether insertion fails 
    }
  // set endpoints
  int Counter = 0;
  cout << Verbose(6) << " with end points ";
  for (map <int, class BoundaryPointSet * >::iterator runner = OrderMap.begin(); runner != OrderMap.end(); runner++) {
    endpoints[Counter] = runner->second;
    cout << " " << *endpoints[Counter];
    Counter++;
  }
  if (Counter < 3) {
    cerr << "ERROR! We have a triangle with only two distinct endpoints!" << endl;
    //exit(1);
  }
  cout << "." << endl;
};

BoundaryTriangleSet::~BoundaryTriangleSet()
{
  for (int i=0;i<3;i++) {
    cout << Verbose(5) << "Erasing triangle Nr." << Nr << endl;
    lines[i]->triangles.erase(Nr);
    TriangleMap::iterator tester = lines[i]->triangles.begin();
    tester++;
    if (tester == lines[i]->triangles.end()) {
      cout << Verbose(5) << *lines[i] << " is no more attached to any triangle, erasing." << endl;
      delete(lines[i]);
    } else
      cout << Verbose(5) << *lines[i] << " is still attached to a triangle." << endl;
  }
};

void BoundaryTriangleSet::GetNormalVector(Vector &NormalVector)
{
  // get normal vector
  NormalVector.MakeNormalVector(&endpoints[0]->node->x, &endpoints[1]->node->x, &endpoints[2]->node->x);
  
  // make it always point inward (any offset vector onto plane projected onto normal vector suffices)
  if (endpoints[0]->node->x.Projection(&NormalVector) > 0)
    NormalVector.Scale(-1.);
};

ostream & operator << (ostream &ost, BoundaryTriangleSet &a) 
{
  ost << "[" << a.Nr << "|" << a.endpoints[0]->node->Name << "," << a.endpoints[1]->node->Name << "," << a.endpoints[2]->node->Name << "]";
  return ost;
};

// ========================================== F U N C T I O N S =================================

/** Finds the endpoint two lines are sharing.
 * \param *line1 first line
 * \param *line2 second line
 * \return point which is shared or NULL if none
 */
class BoundaryPointSet * GetCommonEndpoint(class BoundaryLineSet * line1, class BoundaryLineSet * line2)
{
  class BoundaryLineSet * lines[2] = {line1, line2};
  class BoundaryPointSet *node = NULL;
  map <int, class BoundaryPointSet * > OrderMap;
  pair < map <int, class BoundaryPointSet * >::iterator, bool > OrderTest;
  for(int i=0;i<2;i++)  // for both lines
    for (int j=0;j<2;j++) { // for both endpoints
      OrderTest = OrderMap.insert ( pair <int, class BoundaryPointSet * >( lines[i]->endpoints[j]->Nr, lines[i]->endpoints[j]) );
      if (!OrderTest.second) { // if insertion fails, we have common endpoint
        node = OrderTest.first->second;
        cout << Verbose(5) << "Common endpoint of lines " << *line1 << " and " << *line2 << " is: " << *node << "." << endl;
        j=2;
        i=2;
        break;
      }
    }
  return node;
};

/** Determines the boundary points of a cluster.
 * Does a projection per axis onto the orthogonal plane, transforms into spherical coordinates, sorts them by the angle
 * and looks at triples: if the middle has less a distance than the allowed maximum height of the triangle formed by the plane's
 * center and first and last point in the triple, it is thrown out.
 * \param *out output stream for debugging
 * \param *mol molecule structure representing the cluster
 */
Boundaries * GetBoundaryPoints(ofstream *out, molecule *mol)
{
  atom *Walker = NULL;
  PointMap PointsOnBoundary;
  LineMap LinesOnBoundary;
  TriangleMap TrianglesOnBoundary;
  
  *out << Verbose(1) << "Finding all boundary points." << endl;
  Boundaries *BoundaryPoints = new Boundaries [NDIM];  // first is alpha, second is (r, nr)
  BoundariesTestPair BoundaryTestPair;
  Vector AxisVector, AngleReferenceVector, AngleReferenceNormalVector;
  double radius, angle;
  // 3a. Go through every axis
  for (int axis=0; axis<NDIM; axis++)  {
    AxisVector.Zero();
    AngleReferenceVector.Zero();
    AngleReferenceNormalVector.Zero();
    AxisVector.x[axis] = 1.;
    AngleReferenceVector.x[(axis+1)%NDIM] = 1.;
    AngleReferenceNormalVector.x[(axis+2)%NDIM] = 1.;
  //    *out << Verbose(1) << "Axisvector is ";
  //    AxisVector.Output(out);
  //    *out << " and AngleReferenceVector is ";
  //    AngleReferenceVector.Output(out);
  //    *out << "." << endl;
  //    *out << " and AngleReferenceNormalVector is ";
  //    AngleReferenceNormalVector.Output(out);
  //    *out << "." << endl;
    // 3b. construct set of all points, transformed into cylindrical system and with left and right neighbours
    Walker = mol->start;
    while (Walker->next != mol->end) {
      Walker = Walker->next;
      Vector ProjectedVector;
      ProjectedVector.CopyVector(&Walker->x);
      ProjectedVector.ProjectOntoPlane(&AxisVector);
      // correct for negative side
      //if (Projection(y) < 0)
        //angle = 2.*M_PI - angle;
      radius = ProjectedVector.Norm();
      if (fabs(radius) > MYEPSILON)
        angle = ProjectedVector.Angle(&AngleReferenceVector);
      else
        angle = 0.;  // otherwise it's a vector in Axis Direction and unimportant for boundary issues
        
      //*out << "Checking sign in quadrant : " << ProjectedVector.Projection(&AngleReferenceNormalVector) << "." << endl;
      if (ProjectedVector.Projection(&AngleReferenceNormalVector) > 0) {
        angle = 2.*M_PI - angle;
      }
      //*out << Verbose(2) << "Inserting " << *Walker << ": (r, alpha) = (" << radius << "," << angle << "): ";
      //ProjectedVector.Output(out);
      //*out << endl;
      BoundaryTestPair = BoundaryPoints[axis].insert( BoundariesPair (angle, DistancePair (radius, Walker) ) );
      if (BoundaryTestPair.second) { // successfully inserted
      } else { // same point exists, check first r, then distance of original vectors to center of gravity
        *out << Verbose(2) << "Encountered two vectors whose projection onto axis " << axis << " is equal: " << endl;
        *out << Verbose(2) << "Present vector: ";
        BoundaryTestPair.first->second.second->x.Output(out);
        *out << endl;
        *out << Verbose(2) << "New vector: ";
        Walker->x.Output(out);
        *out << endl;
        double tmp = ProjectedVector.Norm(); 
        if (tmp > BoundaryTestPair.first->second.first) {
          BoundaryTestPair.first->second.first = tmp;
          BoundaryTestPair.first->second.second = Walker; 
          *out << Verbose(2) << "Keeping new vector." << endl;
        } else if (tmp == BoundaryTestPair.first->second.first) {
          if (BoundaryTestPair.first->second.second->x.ScalarProduct(&BoundaryTestPair.first->second.second->x) < Walker->x.ScalarProduct(&Walker->x)) { // Norm() does a sqrt, which makes it a lot slower
            BoundaryTestPair.first->second.second = Walker;
            *out << Verbose(2) << "Keeping new vector." << endl;
          } else {
            *out << Verbose(2) << "Keeping present vector." << endl;
          }
        } else {
            *out << Verbose(2) << "Keeping present vector." << endl;
        }
      }
    }
    // printing all inserted for debugging
  //    {
  //      *out << Verbose(2) << "Printing list of candidates for axis " << axis << " which we have inserted so far." << endl;
  //      int i=0;
  //      for(Boundaries::iterator runner = BoundaryPoints[axis].begin(); runner != BoundaryPoints[axis].end(); runner++) {
  //        if (runner != BoundaryPoints[axis].begin())
  //          *out << ", " << i << ": " << *runner->second.second;
  //        else
  //          *out << i << ": " << *runner->second.second;
  //        i++;
  //      }
  //      *out << endl;
  //    }
    // 3c. throw out points whose distance is less than the mean of left and right neighbours
    bool flag = false;
    do { // do as long as we still throw one out per round
      *out << Verbose(1) << "Looking for candidates to kick out by convex condition ... " << endl;
      flag = false;
      Boundaries::iterator left = BoundaryPoints[axis].end();
      Boundaries::iterator right = BoundaryPoints[axis].end();
      for(Boundaries::iterator runner = BoundaryPoints[axis].begin(); runner != BoundaryPoints[axis].end(); runner++) {
        // set neighbours correctly
        if (runner == BoundaryPoints[axis].begin()) {
          left = BoundaryPoints[axis].end();
        } else {
          left = runner;
        }
        left--;
        right = runner;
        right++;
        if (right == BoundaryPoints[axis].end()) {
          right = BoundaryPoints[axis].begin();
        }
        // check distance
        
        // construct the vector of each side of the triangle on the projected plane (defined by normal vector AxisVector)
        {
          Vector SideA, SideB, SideC, SideH;
          SideA.CopyVector(&left->second.second->x);
          SideA.ProjectOntoPlane(&AxisVector);
  //          *out << "SideA: ";
  //          SideA.Output(out);
  //          *out << endl;
          
          SideB.CopyVector(&right->second.second->x);
          SideB.ProjectOntoPlane(&AxisVector);
  //          *out << "SideB: ";
  //          SideB.Output(out);
  //          *out << endl;
          
          SideC.CopyVector(&left->second.second->x);
          SideC.SubtractVector(&right->second.second->x);
          SideC.ProjectOntoPlane(&AxisVector);
  //          *out << "SideC: ";
  //          SideC.Output(out);
  //          *out << endl;
  
          SideH.CopyVector(&runner->second.second->x);
          SideH.ProjectOntoPlane(&AxisVector);
  //          *out << "SideH: ";
  //          SideH.Output(out);
  //          *out << endl;
          
          // calculate each length
          double a = SideA.Norm();
          //double b = SideB.Norm();
          //double c = SideC.Norm();
          double h = SideH.Norm();
          // calculate the angles
          double alpha = SideA.Angle(&SideH);
          double beta = SideA.Angle(&SideC); 
          double gamma = SideB.Angle(&SideH);
          double delta = SideC.Angle(&SideH);
          double MinDistance = a * sin(beta)/(sin(delta)) * (((alpha < M_PI/2.) || (gamma < M_PI/2.)) ? 1. : -1.);
  //          *out << Verbose(2) << " I calculated: a = " << a << ", h = " << h << ", beta(" << left->second.second->Name << "," << left->second.second->Name << "-" << right->second.second->Name << ") = " << beta << ", delta(" << left->second.second->Name << "," << runner->second.second->Name << ") = " << delta << ", Min = " << MinDistance << "." << endl; 
          //*out << Verbose(1) << "Checking CoG distance of runner " << *runner->second.second << " " << h << " against triangle's side length spanned by (" << *left->second.second << "," << *right->second.second << ") of " << MinDistance << "." << endl;
          if ((fabs(h/fabs(h) - MinDistance/fabs(MinDistance)) < MYEPSILON) && (h <  MinDistance)) {
            // throw out point
            //*out << Verbose(1) << "Throwing out " << *runner->second.second << "." << endl;
            BoundaryPoints[axis].erase(runner);
            flag = true;
          }
        }
      }
    } while (flag);
  } 
  return BoundaryPoints;
};

/** Determines greatest diameters of a cluster defined by its convex envelope.
 * Looks at lines parallel to one axis and where they intersect on the projected planes
 * \param *out output stream for debugging
 * \param *BoundaryPoints NDIM set of boundary points defining the convex envelope on each projected plane
 * \param *mol molecule structure representing the cluster
 * \param IsAngstroem whether we have angstroem or atomic units
 * \return NDIM array of the diameters
 */ 
double * GetDiametersOfCluster(ofstream *out, Boundaries *BoundaryPtr, molecule *mol, bool IsAngstroem)
{
  // get points on boundary of NULL was given as parameter
  bool BoundaryFreeFlag = false;
  Boundaries *BoundaryPoints = BoundaryPtr;
  if (BoundaryPoints == NULL) {
    BoundaryFreeFlag = true;
    BoundaryPoints = GetBoundaryPoints(out, mol);
  } else {
    *out << Verbose(1) << "Using given boundary points set." << endl;
  }
  
  // determine biggest "diameter" of cluster for each axis
  Boundaries::iterator Neighbour, OtherNeighbour;
  double *GreatestDiameter = new double[NDIM];
  for(int i=0;i<NDIM;i++)
    GreatestDiameter[i] = 0.;
  double OldComponent, tmp, w1, w2;
  Vector DistanceVector, OtherVector;
  int component, Othercomponent;
  for(int axis=0;axis<NDIM;axis++) { // regard each projected plane
    //*out << Verbose(1) << "Current axis is " << axis << "." << endl;
    for (int j=0;j<2;j++) { // and for both axis on the current plane
      component = (axis+j+1)%NDIM;
      Othercomponent = (axis+1+((j+1) & 1))%NDIM;
      //*out << Verbose(1) << "Current component is " << component << ", Othercomponent is " << Othercomponent << "." << endl;
      for(Boundaries::iterator runner = BoundaryPoints[axis].begin(); runner != BoundaryPoints[axis].end(); runner++) {
        //*out << Verbose(2) << "Current runner is " << *(runner->second.second) << "." << endl;
        // seek for the neighbours pair where the Othercomponent sign flips
        Neighbour = runner;
        Neighbour++;
        if (Neighbour == BoundaryPoints[axis].end())  // make it wrap around
          Neighbour = BoundaryPoints[axis].begin();
        DistanceVector.CopyVector(&runner->second.second->x);
        DistanceVector.SubtractVector(&Neighbour->second.second->x);
        do {  // seek for neighbour pair where it flips
          OldComponent = DistanceVector.x[Othercomponent]; 
          Neighbour++;
          if (Neighbour == BoundaryPoints[axis].end())  // make it wrap around
            Neighbour = BoundaryPoints[axis].begin();
          DistanceVector.CopyVector(&runner->second.second->x);
          DistanceVector.SubtractVector(&Neighbour->second.second->x);
          //*out << Verbose(3) << "OldComponent is " << OldComponent << ", new one is " << DistanceVector.x[Othercomponent] << "." << endl;
        } while ((runner != Neighbour) && ( fabs( OldComponent/fabs(OldComponent) - DistanceVector.x[Othercomponent]/fabs(DistanceVector.x[Othercomponent]) ) < MYEPSILON)); // as long as sign does not flip
        if (runner != Neighbour) {
          OtherNeighbour = Neighbour;
          if (OtherNeighbour == BoundaryPoints[axis].begin())  // make it wrap around
            OtherNeighbour = BoundaryPoints[axis].end();
          OtherNeighbour--;
          //*out << Verbose(2) << "The pair, where the sign of OtherComponent flips, is: " << *(Neighbour->second.second) << " and " << *(OtherNeighbour->second.second) << "." << endl; 
          // now we have found the pair: Neighbour and OtherNeighbour
          OtherVector.CopyVector(&runner->second.second->x);
          OtherVector.SubtractVector(&OtherNeighbour->second.second->x);
          //*out << Verbose(2) << "Distances to Neighbour and OtherNeighbour are " << DistanceVector.x[component] << " and " << OtherVector.x[component] << "." << endl;
          //*out << Verbose(2) << "OtherComponents to Neighbour and OtherNeighbour are " << DistanceVector.x[Othercomponent] << " and " << OtherVector.x[Othercomponent] << "." << endl;
          // do linear interpolation between points (is exact) to extract exact intersection between Neighbour and OtherNeighbour
          w1 = fabs(OtherVector.x[Othercomponent]);
          w2 = fabs(DistanceVector.x[Othercomponent]);
          tmp = fabs((w1*DistanceVector.x[component] + w2*OtherVector.x[component])/(w1+w2));
          // mark if it has greater diameter
          //*out << Verbose(2) << "Comparing current greatest " << GreatestDiameter[component] << " to new " << tmp << "." << endl;
          GreatestDiameter[component] = (GreatestDiameter[component] > tmp) ? GreatestDiameter[component] : tmp;
        } //else
          //*out << Verbose(2) << "Saw no sign flip, probably top or bottom node." << endl;
      }
    }
  }
  *out << Verbose(0) << "RESULT: The biggest diameters are " << GreatestDiameter[0] << " and " << GreatestDiameter[1] << " and " << GreatestDiameter[2] << " " << (IsAngstroem ? "angstrom" : "atomiclength") << "." << endl;

  // free reference lists
  if (BoundaryFreeFlag)
    delete[](BoundaryPoints);

  return GreatestDiameter;
};


/** Determines the volume of a cluster.
 * Determines first the convex envelope, then tesselates it and calculates its volume.
 * \param *out output stream for debugging
 * \param *tecplot output stream for tecplot data
 * \param *configuration needed for path to store convex envelope file
 * \param *BoundaryPoints NDIM set of boundary points on the projected plane per axis, on return if desired
 * \param *mol molecule structure representing the cluster
 * \return determined volume of the cluster in cubed config:GetIsAngstroem() 
 */
double VolumeOfConvexEnvelope(ofstream *out, ofstream *tecplot, config *configuration, Boundaries *BoundaryPtr, molecule *mol) 
{
  bool IsAngstroem = configuration->GetIsAngstroem();
  atom *Walker = NULL;
  struct Tesselation *TesselStruct = new Tesselation;
  bool BoundaryFreeFlag = false;
  Boundaries *BoundaryPoints = BoundaryPtr;
  double volume = 0.;
  double PyramidVolume = 0.;
  double G,h; 
  Vector x,y;
  double a,b,c;

  Find_non_convex_border(*TesselStruct, *mol);
  /*
  // 1. calculate center of gravity
  *out << endl;
  Vector *CenterOfGravity = mol->DetermineCenterOfGravity(out);
  
  // 2. translate all points into CoG
  *out << Verbose(1) << "Translating system to Center of Gravity." << endl;
  Walker = mol->start;
  while (Walker->next != mol->end) {
    Walker = Walker->next;
    Walker->x.Translate(CenterOfGravity);
  }
  
  // 3. Find all points on the boundary
  if (BoundaryPoints == NULL) {
    BoundaryFreeFlag = true;
    BoundaryPoints = GetBoundaryPoints(out, mol);
  } else {
    *out << Verbose(1) << "Using given boundary points set." << endl;
  }
  
  // 4. fill the boundary point list
  for (int axis=0;axis<NDIM;axis++)
    for(Boundaries::iterator runner = BoundaryPoints[axis].begin(); runner != BoundaryPoints[axis].end(); runner++) {
      TesselStruct->AddPoint(runner->second.second);
    }

  *out << Verbose(2) << "I found " << TesselStruct->PointsOnBoundaryCount << " points on the convex boundary." << endl;
  // now we have the whole set of edge points in the BoundaryList

  // listing for debugging
//  *out << Verbose(1) << "Listing PointsOnBoundary:";
//  for(PointMap::iterator runner = PointsOnBoundary.begin(); runner != PointsOnBoundary.end(); runner++) {
//    *out << " " << *runner->second;
//  }
//  *out << endl;
  
  // 5a. guess starting triangle
  TesselStruct->GuessStartingTriangle(out);
  
  // 5b. go through all lines, that are not yet part of two triangles (only of one so far)
  TesselStruct->TesselateOnBoundary(out, configuration, mol);

  *out << Verbose(2) << "I created " << TesselStruct->TrianglesOnBoundaryCount << " triangles with " << TesselStruct->LinesOnBoundaryCount << " lines and " << TesselStruct->PointsOnBoundaryCount << " points." << endl;
  */


  // 6a. Every triangle forms a pyramid with the center of gravity as its peak, sum up the volumes
  *out << Verbose(1) << "Calculating the volume of the pyramids formed out of triangles and center of gravity." << endl;
  for (TriangleMap::iterator runner = TesselStruct->TrianglesOnBoundary.begin(); runner != TesselStruct->TrianglesOnBoundary.end(); runner++) { // go through every triangle, calculate volume of its pyramid with CoG as peak
    x.CopyVector(&runner->second->endpoints[0]->node->x);
    x.SubtractVector(&runner->second->endpoints[1]->node->x);
    y.CopyVector(&runner->second->endpoints[0]->node->x);
    y.SubtractVector(&runner->second->endpoints[2]->node->x);
    a = sqrt(runner->second->endpoints[0]->node->x.Distance(&runner->second->endpoints[1]->node->x));
    b = sqrt(runner->second->endpoints[0]->node->x.Distance(&runner->second->endpoints[2]->node->x));
    c = sqrt(runner->second->endpoints[2]->node->x.Distance(&runner->second->endpoints[1]->node->x));
    G =  sqrt( ( (a*a+b*b+c*c)*(a*a+b*b+c*c) - 2*(a*a*a*a + b*b*b*b + c*c*c*c) )/16.); // area of tesselated triangle
    x.MakeNormalVector(&runner->second->endpoints[0]->node->x, &runner->second->endpoints[1]->node->x, &runner->second->endpoints[2]->node->x);
    x.Scale(runner->second->endpoints[1]->node->x.Projection(&x));
    h = x.Norm(); // distance of CoG to triangle
    PyramidVolume = (1./3.) * G * h;    // this formula holds for _all_ pyramids (independent of n-edge base or (not) centered peak)
    *out << Verbose(2) << "Area of triangle is " << G << " " << (IsAngstroem ? "angstrom" : "atomiclength") << "^2, height is " << h << " and the volume is " << PyramidVolume << " " << (IsAngstroem ? "angstrom" : "atomiclength") << "^3." << endl;
    volume += PyramidVolume; 
  }
  *out << Verbose(0) << "RESULT: The summed volume is " << setprecision(10) << volume << " " << (IsAngstroem ? "angstrom" : "atomiclength") << "^3." << endl;

  /*
  // 7. translate all points back from CoG
  *out << Verbose(1) << "Translating system back from Center of Gravity." << endl;
  CenterOfGravity->Scale(-1);
  Walker = mol->start;
  while (Walker->next != mol->end) {
    Walker = Walker->next;
    Walker->x.Translate(CenterOfGravity);
  }
  */





  // 8. Store triangles in tecplot file
  if (tecplot != NULL) {
    *tecplot << "TITLE = \"3D CONVEX SHELL\"" << endl;
    *tecplot << "VARIABLES = \"X\" \"Y\" \"Z\"" << endl;
    *tecplot << "ZONE T=\"TRIANGLES\", N=" <<  TesselStruct->PointsOnBoundaryCount << ", E=" <<  TesselStruct->TrianglesOnBoundaryCount << ", DATAPACKING=POINT, ZONETYPE=FETRIANGLE" << endl;
    int *LookupList = new int[mol->AtomCount];
    for (int i=0;i<mol->AtomCount;i++)
      LookupList[i] = -1;
    
    // print atom coordinates
    *out << Verbose(2) << "The following triangles were created:";
    int Counter = 1;
    atom *Walker = NULL;
    for (PointMap::iterator target =  TesselStruct->PointsOnBoundary.begin(); target !=  TesselStruct->PointsOnBoundary.end(); target++) {
      Walker = target->second->node;
      LookupList[Walker->nr] = Counter++;
      *tecplot << Walker->x.x[0] << " " << Walker->x.x[1] << " " << Walker->x.x[2] << " " << endl;
    }
    *tecplot << endl;
      // print connectivity
    for (TriangleMap::iterator runner =  TesselStruct->TrianglesOnBoundary.begin(); runner !=  TesselStruct->TrianglesOnBoundary.end(); runner++) {
      *out << " " << runner->second->endpoints[0]->node->Name << "<->" << runner->second->endpoints[1]->node->Name << "<->" << runner->second->endpoints[2]->node->Name;
      *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);
    *out << endl;
  }

  // free reference lists
  if (BoundaryFreeFlag)
    delete[](BoundaryPoints);
  
  return volume;
};


/** Creates multiples of the by \a *mol given cluster and suspends them in water with a given final density.
 * We get cluster volume by VolumeOfConvexEnvelope() and its diameters by GetDiametersOfCluster()
 * \param *out output stream for debugging
 * \param *configuration needed for path to store convex envelope file
 * \param *mol molecule structure representing the cluster
 * \param ClusterVolume guesstimated cluster volume, if equal 0 we used VolumeOfConvexEnvelope() instead.
 * \param celldensity desired average density in final cell
 */
void PrepareClustersinWater(ofstream *out, config *configuration, molecule *mol, double ClusterVolume, double celldensity)
{
  // transform to PAS
  mol->PrincipalAxisSystem(out, true);
  
  // some preparations beforehand
  bool IsAngstroem = configuration->GetIsAngstroem();
  Boundaries *BoundaryPoints = GetBoundaryPoints(out, mol);
  double clustervolume;
  if (ClusterVolume == 0)
    clustervolume = VolumeOfConvexEnvelope(out, NULL, configuration, BoundaryPoints, mol);
  else 
    clustervolume = ClusterVolume;
  double *GreatestDiameter = GetDiametersOfCluster(out, BoundaryPoints, mol, IsAngstroem);
  Vector BoxLengths;
  int repetition[NDIM] = {1, 1, 1};
  int TotalNoClusters = 1;
  for (int i=0;i<NDIM;i++)
    TotalNoClusters *= repetition[i];

  // sum up the atomic masses
  double totalmass = 0.;
  atom *Walker = mol->start;
  while (Walker->next != mol->end) {
    Walker = Walker->next;
    totalmass += Walker->type->mass;
  }
  *out << Verbose(0) << "RESULT: The summed mass is " << setprecision(10) << totalmass << " atomicmassunit." << endl;
  
  *out << Verbose(0) << "RESULT: The average density is " << setprecision(10) << totalmass/clustervolume << " atomicmassunit/" << (IsAngstroem ? "angstrom" : "atomiclength") << "^3." << endl;
  
  // solve cubic polynomial
  *out << Verbose(1) << "Solving equidistant suspension in water problem ..." << endl;
  double cellvolume;
  if (IsAngstroem)
    cellvolume = (TotalNoClusters*totalmass/SOLVENTDENSITY_A - (totalmass/clustervolume))/(celldensity-1);
  else
    cellvolume = (TotalNoClusters*totalmass/SOLVENTDENSITY_a0 - (totalmass/clustervolume))/(celldensity-1);
  *out << Verbose(1) << "Cellvolume needed for a density of " << celldensity << " g/cm^3 is " << cellvolume << " " << (IsAngstroem ? "angstrom" : "atomiclength") << "^3." << endl;
  
  double minimumvolume = TotalNoClusters*(GreatestDiameter[0]*GreatestDiameter[1]*GreatestDiameter[2]);
  *out << Verbose(1) << "Minimum volume of the convex envelope contained in a rectangular box is " << minimumvolume << " atomicmassunit/" << (IsAngstroem ? "angstrom" : "atomiclength") << "^3." << endl;
  if (minimumvolume > cellvolume) {
    cerr << Verbose(0) << "ERROR: the containing box already has a greater volume than the envisaged cell volume!" << endl;
    cout << Verbose(0) << "Setting Box dimensions to minimum possible, the greatest diameters." << endl;
    for(int i=0;i<NDIM;i++)
      BoxLengths.x[i] = GreatestDiameter[i];
    mol->CenterEdge(out, &BoxLengths);
  } else {
    BoxLengths.x[0] = (repetition[0]*GreatestDiameter[0] + repetition[1]*GreatestDiameter[1] + repetition[2]*GreatestDiameter[2]);
    BoxLengths.x[1] = (repetition[0]*repetition[1]*GreatestDiameter[0]*GreatestDiameter[1] 
              + repetition[0]*repetition[2]*GreatestDiameter[0]*GreatestDiameter[2] 
              + repetition[1]*repetition[2]*GreatestDiameter[1]*GreatestDiameter[2]);
    BoxLengths.x[2] = minimumvolume - cellvolume;
    double x0 = 0.,x1 = 0.,x2 = 0.;
    if (gsl_poly_solve_cubic(BoxLengths.x[0],BoxLengths.x[1],BoxLengths.x[2],&x0,&x1,&x2) == 1) // either 1 or 3 on return
      *out << Verbose(0) << "RESULT: The resulting spacing is: " << x0 << " ." << endl;
    else {
      *out << Verbose(0) << "RESULT: The resulting spacings are: " << x0 << " and " << x1 << " and " << x2 << " ." << endl;
      x0 = x2;  // sorted in ascending order
    }
  
    cellvolume = 1;
    for(int i=0;i<NDIM;i++) {
      BoxLengths.x[i] = repetition[i] * (x0 + GreatestDiameter[i]);
      cellvolume *= BoxLengths.x[i];
    }
  
    // set new box dimensions
    *out << Verbose(0) << "Translating to box with these boundaries." << endl;
    mol->CenterInBox((ofstream *)&cout, &BoxLengths);
  }
  // update Box of atoms by boundary
  mol->SetBoxDimension(&BoxLengths);
  *out << Verbose(0) << "RESULT: The resulting cell dimensions are: " << BoxLengths.x[0] << " and " << BoxLengths.x[1] << " and " << BoxLengths.x[2] << " with total volume of " << cellvolume << " " << (IsAngstroem ? "angstrom" : "atomiclength") << "^3." << endl;
};


// =========================================================== class TESSELATION ===========================================

/** Constructor of class Tesselation.
 */
Tesselation::Tesselation()
{
  PointsOnBoundaryCount = 0; 
  LinesOnBoundaryCount = 0; 
  TrianglesOnBoundaryCount = 0;
};

/** Constructor of class Tesselation.
 * We have to free all points, lines and triangles.
 */
Tesselation::~Tesselation()
{
  for (TriangleMap::iterator runner = TrianglesOnBoundary.begin(); runner != TrianglesOnBoundary.end(); runner++) {
    delete(runner->second);
  }
};

/** Gueses first starting triangle of the convex envelope.
 * We guess the starting triangle by taking the smallest distance between two points and looking for a fitting third.
 * \param *out output stream for debugging
 * \param PointsOnBoundary set of boundary points defining the convex envelope of the cluster
 */ 
void Tesselation::GuessStartingTriangle(ofstream *out)
{
  // 4b. create a starting triangle
  // 4b1. create all distances
  DistanceMultiMap DistanceMMap;
  double distance, tmp;
  Vector PlaneVector, TrialVector;
  PointMap::iterator A, B, C; // three nodes of the first triangle
  A = PointsOnBoundary.begin(); // the first may be chosen arbitrarily

  // with A chosen, take each pair B,C and sort 
  if (A != PointsOnBoundary.end()) {
    B = A;
    B++;
    for (; B != PointsOnBoundary.end(); B++) {
      C = B;
      C++;
      for (; C != PointsOnBoundary.end(); C++) {
        tmp = A->second->node->x.Distance(&B->second->node->x);
        distance = tmp*tmp;
        tmp = A->second->node->x.Distance(&C->second->node->x);
        distance += tmp*tmp;
        tmp = B->second->node->x.Distance(&C->second->node->x);
        distance += tmp*tmp;
        DistanceMMap.insert( DistanceMultiMapPair(distance, pair<PointMap::iterator, PointMap::iterator>(B,C) ) );
      }
    }
  }
//    // listing distances
//    *out << Verbose(1) << "Listing DistanceMMap:";
//    for(DistanceMultiMap::iterator runner = DistanceMMap.begin(); runner != DistanceMMap.end(); runner++) {
//      *out << " " << runner->first << "(" << *runner->second.first->second << ", " << *runner->second.second->second << ")";
//    }
//    *out << endl;
  
  // 4b2. pick three baselines forming a triangle
  // 1. we take from the smallest sum of squared distance as the base line BC (with peak A) onward as the triangle candidate
  DistanceMultiMap::iterator baseline = DistanceMMap.begin();
  for (; baseline != DistanceMMap.end(); baseline++) {
    // we take from the smallest sum of squared distance as the base line BC (with peak A) onward as the triangle candidate
    // 2. next, we have to check whether all points reside on only one side of the triangle
    // 3. construct plane vector
    PlaneVector.MakeNormalVector(&A->second->node->x, &baseline->second.first->second->node->x, &baseline->second.second->second->node->x);
    *out << Verbose(2) << "Plane vector of candidate triangle is ";
    PlaneVector.Output(out);
    *out << endl;
    // 4. loop over all points 
    double sign = 0.;
    PointMap::iterator checker = PointsOnBoundary.begin();
    for (; checker != PointsOnBoundary.end(); checker++) {
      // (neglecting A,B,C)
      if ((checker == A) || (checker == baseline->second.first) || (checker == baseline->second.second))
        continue;
      // 4a. project onto plane vector
      TrialVector.CopyVector(&checker->second->node->x);
      TrialVector.SubtractVector(&A->second->node->x);
      distance = TrialVector.Projection(&PlaneVector);
      if (fabs(distance) < 1e-4)  // we need to have a small epsilon around 0 which is still ok
        continue;
      *out << Verbose(3) << "Projection of " << checker->second->node->Name << " yields distance of " << distance << "." << endl;
      tmp = distance/fabs(distance);
      // 4b. Any have different sign to than before? (i.e. would lie outside convex hull with this starting triangle)
      if ((sign != 0) && (tmp != sign)) {
        // 4c. If so, break 4. loop and continue with next candidate in 1. loop
        *out << Verbose(2) << "Current candidates: " << A->second->node->Name << "," << baseline->second.first->second->node->Name  << "," << baseline->second.second->second->node->Name << " leave " << checker->second->node->Name << " outside the convex hull." << endl;
        break;
      } else { // note the sign for later
        *out << Verbose(2) << "Current candidates: " << A->second->node->Name << "," << baseline->second.first->second->node->Name  << "," << baseline->second.second->second->node->Name << " leave " << checker->second->node->Name << " inside the convex hull." << endl;
        sign = tmp;
      }
      // 4d. Check whether the point is inside the triangle (check distance to each node
      tmp = checker->second->node->x.Distance(&A->second->node->x);
      int innerpoint = 0;
      if ((tmp < A->second->node->x.Distance(&baseline->second.first->second->node->x)) 
          && (tmp < A->second->node->x.Distance(&baseline->second.second->second->node->x)))
        innerpoint++;
      tmp = checker->second->node->x.Distance(&baseline->second.first->second->node->x);
      if ((tmp < baseline->second.first->second->node->x.Distance(&A->second->node->x)) 
          && (tmp < baseline->second.first->second->node->x.Distance(&baseline->second.second->second->node->x)))
        innerpoint++;
      tmp = checker->second->node->x.Distance(&baseline->second.second->second->node->x);
      if ((tmp < baseline->second.second->second->node->x.Distance(&baseline->second.first->second->node->x)) 
          && (tmp < baseline->second.second->second->node->x.Distance(&A->second->node->x)))
        innerpoint++;
      // 4e. If so, break 4. loop and continue with next candidate in 1. loop
      if (innerpoint == 3)
        break;
    }
    // 5. come this far, all on same side? Then break 1. loop and construct triangle
    if (checker == PointsOnBoundary.end()) {
      *out << "Looks like we have a candidate!" << endl;
      break;
    }
  }
  if (baseline != DistanceMMap.end()) {
    BPS[0] = baseline->second.first->second;
    BPS[1] = baseline->second.second->second;
    BLS[0] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
    BPS[0] = A->second;
    BPS[1] = baseline->second.second->second;
    BLS[1] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
    BPS[0] = baseline->second.first->second;
    BPS[1] = A->second;
    BLS[2] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);  
   
    // 4b3. insert created triangle
    BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
    TrianglesOnBoundary.insert( TrianglePair(TrianglesOnBoundaryCount, BTS) );
    TrianglesOnBoundaryCount++;
    for(int i=0;i<NDIM;i++) {
      LinesOnBoundary.insert( LinePair(LinesOnBoundaryCount, BTS->lines[i]) );
      LinesOnBoundaryCount++;
    }
  
    *out << Verbose(1) << "Starting triangle is " << *BTS << "." << endl;
  } else {
    *out << Verbose(1) << "No starting triangle found." << endl;
    exit(255);
  }
};


/** Tesselates the convex envelope of a cluster from a single starting triangle.
 * The starting triangle is made out of three baselines. Each line in the final tesselated cluster may belong to at most
 * 2 triangles. Hence, we go through all current lines:
 * -# if the lines contains to only one triangle
 * -# We search all points in the boundary
 *    -# if the triangle with the baseline and the current point has the smallest of angles (comparison between normal vectors
 *    -# if the triangle is in forward direction of the baseline (at most 90 degrees angle between vector orthogonal to
 *       baseline in triangle plane pointing out of the triangle and normal vector of new triangle)
 *    -# then we have a new triangle, whose baselines we again add (or increase their TriangleCount)
 * \param *out output stream for debugging
 * \param *configuration for IsAngstroem
 * \param *mol the cluster as a molecule structure
 */
void Tesselation::TesselateOnBoundary(ofstream *out, config *configuration, molecule *mol)
{
  bool flag;
  PointMap::iterator winner;
  class BoundaryPointSet *peak = NULL;
  double SmallestAngle, TempAngle;
  Vector NormalVector, VirtualNormalVector, CenterVector, TempVector, PropagationVector;
  LineMap::iterator LineChecker[2];
  do {
    flag = false;
    for (LineMap::iterator baseline = LinesOnBoundary.begin(); baseline != LinesOnBoundary.end(); baseline++) 
      if (baseline->second->TrianglesCount == 1) {
        *out << Verbose(2) << "Current baseline is between " << *(baseline->second) << "." << endl; 
        // 5a. go through each boundary point if not _both_ edges between either endpoint of the current line and this point exist (and belong to 2 triangles)
        SmallestAngle = M_PI;
        BTS = baseline->second->triangles.begin()->second; // there is only one triangle so far
        // get peak point with respect to this base line's only triangle
        for(int i=0;i<3;i++)
          if ((BTS->endpoints[i] != baseline->second->endpoints[0]) && (BTS->endpoints[i] != baseline->second->endpoints[1]))
            peak = BTS->endpoints[i];
        *out << Verbose(3) << " and has peak " << *peak << "." << endl;
        // normal vector of triangle
        BTS->GetNormalVector(NormalVector);
        *out << Verbose(4) << "NormalVector of base triangle is ";
        NormalVector.Output(out);
        *out << endl;
        // offset to center of triangle
        CenterVector.Zero();
        for(int i=0;i<3;i++)
          CenterVector.AddVector(&BTS->endpoints[i]->node->x);
        CenterVector.Scale(1./3.);
        *out << Verbose(4) << "CenterVector of base triangle is ";
        CenterVector.Output(out);
        *out << endl;
        // vector in propagation direction (out of triangle)
        // project center vector onto triangle plane (points from intersection plane-NormalVector to plane-CenterVector intersection)
        TempVector.CopyVector(&baseline->second->endpoints[0]->node->x);
        TempVector.SubtractVector(&baseline->second->endpoints[1]->node->x);
        PropagationVector.MakeNormalVector(&TempVector, &NormalVector);
        TempVector.CopyVector(&CenterVector);
        TempVector.SubtractVector(&baseline->second->endpoints[0]->node->x);  // TempVector is vector on triangle plane pointing from one baseline egde towards center!
        //*out << Verbose(2) << "Projection of propagation onto temp: " << PropagationVector.Projection(&TempVector) << "." << endl; 
        if (PropagationVector.Projection(&TempVector) > 0)  // make sure normal propagation vector points outward from baseline
          PropagationVector.Scale(-1.);
        *out << Verbose(4) << "PropagationVector of base triangle is ";
        PropagationVector.Output(out);
        *out << endl;
        winner = PointsOnBoundary.end();
        for (PointMap::iterator target = PointsOnBoundary.begin(); target != PointsOnBoundary.end(); target++) 
          if ((target->second != baseline->second->endpoints[0]) && (target->second != baseline->second->endpoints[1])) { // don't take the same endpoints
            *out << Verbose(3) << "Target point is " << *(target->second) << ":";
            bool continueflag = true;
            
            VirtualNormalVector.CopyVector(&baseline->second->endpoints[0]->node->x);
            VirtualNormalVector.AddVector(&baseline->second->endpoints[0]->node->x);
            VirtualNormalVector.Scale(-1./2.);   // points now to center of base line
            VirtualNormalVector.AddVector(&target->second->node->x); // points from center of base line to target
            TempAngle = VirtualNormalVector.Angle(&PropagationVector);
            continueflag = continueflag && (TempAngle < (M_PI/2.)); // no bends bigger than Pi/2 (90 degrees)
            if (!continueflag) {
              *out << Verbose(4) << "Angle between propagation direction and base line to " << *(target->second) << " is " << TempAngle << ", bad direction!" << endl;
              continue;
            } else
              *out << Verbose(4) << "Angle between propagation direction and base line to " << *(target->second) << " is " << TempAngle << ", good direction!" << endl;
            LineChecker[0] = baseline->second->endpoints[0]->lines.find(target->first);
            LineChecker[1] = baseline->second->endpoints[1]->lines.find(target->first);
  //            if (LineChecker[0] != baseline->second->endpoints[0]->lines.end())
  //              *out << Verbose(4) << *(baseline->second->endpoints[0]) << " has line " << *(LineChecker[0]->second) << " to " << *(target->second) << " as endpoint with " << LineChecker[0]->second->TrianglesCount << " triangles." << endl;
  //            else
  //              *out << Verbose(4) << *(baseline->second->endpoints[0]) << " has no line to " << *(target->second) << " as endpoint." << endl;
  //            if (LineChecker[1] != baseline->second->endpoints[1]->lines.end())
  //              *out << Verbose(4) << *(baseline->second->endpoints[1]) << " has line " << *(LineChecker[1]->second) << " to " << *(target->second) << " as endpoint with " << LineChecker[1]->second->TrianglesCount << " triangles." << endl;
  //            else
  //              *out << Verbose(4) << *(baseline->second->endpoints[1]) << " has no line to " << *(target->second) << " as endpoint." << endl;
            // check first endpoint (if any connecting line goes to target or at least not more than 1)
            continueflag = continueflag && (( (LineChecker[0] == baseline->second->endpoints[0]->lines.end()) || (LineChecker[0]->second->TrianglesCount == 1)));
            if (!continueflag) {
              *out << Verbose(4) << *(baseline->second->endpoints[0]) << " has line " << *(LineChecker[0]->second) << " to " << *(target->second) << " as endpoint with " << LineChecker[0]->second->TrianglesCount << " triangles." << endl;
              continue;
            }
            // check second endpoint (if any connecting line goes to target or at least not more than 1)
            continueflag = continueflag && (( (LineChecker[1] == baseline->second->endpoints[1]->lines.end()) || (LineChecker[1]->second->TrianglesCount == 1)));
            if (!continueflag) {
              *out << Verbose(4) << *(baseline->second->endpoints[1]) << " has line " << *(LineChecker[1]->second) << " to " << *(target->second) << " as endpoint with " << LineChecker[1]->second->TrianglesCount << " triangles." << endl;
              continue;
            }
            // check whether the envisaged triangle does not already exist (if both lines exist and have same endpoint)
            continueflag = continueflag && (!(
                ((LineChecker[0] != baseline->second->endpoints[0]->lines.end()) && (LineChecker[1] != baseline->second->endpoints[1]->lines.end()) 
                && (GetCommonEndpoint(LineChecker[0]->second, LineChecker[1]->second) == peak))
               ));
            if (!continueflag) {
              *out << Verbose(4) << "Current target is peak!" << endl;
              continue;
            }
            // in case NOT both were found
            if (continueflag) {  // create virtually this triangle, get its normal vector, calculate angle
              flag = true;
              VirtualNormalVector.MakeNormalVector(&baseline->second->endpoints[0]->node->x, &baseline->second->endpoints[1]->node->x, &target->second->node->x);
              // make it always point inward
              if (baseline->second->endpoints[0]->node->x.Projection(&VirtualNormalVector) > 0)
                VirtualNormalVector.Scale(-1.);
              // calculate angle
              TempAngle = NormalVector.Angle(&VirtualNormalVector);
              *out << Verbose(4) << "NormalVector is ";
              VirtualNormalVector.Output(out);
              *out << " and the angle is " << TempAngle << "." << endl;
              if (SmallestAngle > TempAngle) {  // set to new possible winner
                SmallestAngle = TempAngle;
                winner = target;
              }
            }
          }
        // 5b. The point of the above whose triangle has the greatest angle with the triangle the current line belongs to (it only belongs to one, remember!): New triangle
        if (winner != PointsOnBoundary.end()) {
          *out << Verbose(2) << "Winning target point is " << *(winner->second) << " with angle " << SmallestAngle << "." << endl;
          // create the lins of not yet present
          BLS[0] = baseline->second;
          // 5c. add lines to the line set if those were new (not yet part of a triangle), delete lines that belong to two triangles)
          LineChecker[0] = baseline->second->endpoints[0]->lines.find(winner->first);
          LineChecker[1] = baseline->second->endpoints[1]->lines.find(winner->first);
          if (LineChecker[0] == baseline->second->endpoints[0]->lines.end()) { // create
            BPS[0] = baseline->second->endpoints[0];
            BPS[1] = winner->second;
            BLS[1] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
            LinesOnBoundary.insert( LinePair(LinesOnBoundaryCount, BLS[1]) );
            LinesOnBoundaryCount++;
          } else
            BLS[1] = LineChecker[0]->second;
          if (LineChecker[1] == baseline->second->endpoints[1]->lines.end()) { // create
            BPS[0] = baseline->second->endpoints[1];
            BPS[1] = winner->second;
            BLS[2] = new class BoundaryLineSet(BPS, LinesOnBoundaryCount);
            LinesOnBoundary.insert( LinePair(LinesOnBoundaryCount, BLS[2]) );
            LinesOnBoundaryCount++;
          } else
            BLS[2] = LineChecker[1]->second;
          BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
          TrianglesOnBoundary.insert( TrianglePair(TrianglesOnBoundaryCount, BTS) );
          TrianglesOnBoundaryCount++;
        } else {
          *out << Verbose(1) << "I could not determine a winner for this baseline " << *(baseline->second) << "." << endl;
        }
  
        // 5d. If the set of lines is not yet empty, go to 5. and continue
      } else
        *out << Verbose(2) << "Baseline candidate " << *(baseline->second) << " has a triangle count of " << baseline->second->TrianglesCount << "." << endl;
  } while (flag);
  
};

/** Adds an atom to the tesselation::PointsOnBoundary list.
 * \param *Walker atom to add
 */
void Tesselation::AddPoint(atom *Walker)
{
  PointTestPair InsertUnique;
  BPS[0] = new class BoundaryPointSet(Walker);
  InsertUnique = PointsOnBoundary.insert( PointPair(Walker->nr, BPS[0]) );
  if (InsertUnique.second)  // if new point was not present before, increase counter
    PointsOnBoundaryCount++;
};






//====================================================================================================================
//
// ab hier habe ich das verzapft.
//
//====================================================================================================================


void Find_next_suitable_point(atom a, atom b, atom Candidate, int n, Vector *d1, Vector *d2, double *Storage, const double RADIUS, molecule mol)
{
  /* d2 ist der Normalenvektor auf dem Dreieck,
   * d1 ist der Vektor, der normal auf der Kante und d2 steht.
   */
  Vector dif_a;
  Vector dif_b;
  Vector Chord;
  Vector AngleCheck;
  atom *Walker;

  dif_a.CopyVector(&a.x);
  dif_a.SubtractVector(&Candidate.x);
  dif_b.CopyVector(&b.x);
  dif_b.SubtractVector(&Candidate.x);
  Chord.CopyVector(&a.x);
  Chord.SubtractVector(&b.x);
  AngleCheck.CopyVector(&dif_a);
  AngleCheck.ProjectOntoPlane(&Chord);

  //Storage eintrag fuer aktuelles Atom
  if (Chord.Norm()/(2*sin(dif_a.Angle(&dif_b)))<RADIUS) //Using Formula for relation of chord length with inner angle to find of Ball will touch atom
  {

    if (dif_a.ScalarProduct(d1)/fabs(dif_a.ScalarProduct(d1))>Storage[1]) //This will give absolute preference to those in "right-hand" quadrants
      {
	Storage[0]=(double)Candidate.nr;
	Storage[1]=1;
	Storage[2]=AngleCheck.Angle(d2);
      }
    else
      {
	if ((dif_a.ScalarProduct(d1)/fabs(dif_a.ScalarProduct(d1)) == Storage[1] && Storage[1]>0 &&  Storage[2]< AngleCheck.Angle(d2)) or \
	    (dif_a.ScalarProduct(d1)/fabs(dif_a.ScalarProduct(d1)) == Storage[1] && Storage[1]<0 &&  Storage[2]> AngleCheck.Angle(d2)))
	  //Depending on quadrant we prefer higher or lower atom with respect to Triangle normal first.
	  {
	    Storage[0]=(double)Candidate.nr;
	    Storage[1]=dif_a.ScalarProduct(d1)/fabs(dif_a.ScalarProduct(d1));
	    Storage[2]=AngleCheck.Angle(d2);
	  }
      }
  }

  if (n<5)
    {
      for(int i=0; i<mol.NumberOfBondsPerAtom[Candidate.nr];i++)
	{
	  while (Candidate.nr != (mol.ListOfBondsPerAtom[Candidate.nr][i]->leftatom->nr ==Candidate.nr ? mol.ListOfBondsPerAtom[Candidate.nr][i]->leftatom->nr : mol.ListOfBondsPerAtom[Candidate.nr][i]->rightatom->nr))
	    {
	      Walker = Walker->next;
	    }

	  Find_next_suitable_point(a, b, *Walker, n+1, d1, d2, Storage, RADIUS, mol);
	}
    }
};


void Tesselation::Find_next_suitable_triangle(molecule *mol, BoundaryLineSet Line, BoundaryTriangleSet T, const double& RADIUS)
{
  Vector CenterOfLine = Line.endpoints[0]->node->x;
  Vector direction1;
  Vector direction2;
  Vector helper;
  atom* Walker;

  double Storage[3];
  Storage[0]=-1.;   // Id must be positive, we see should nothing be done
  Storage[1]=-1.;   // This direction is either +1 or -1 one, so any result will take precedence over initial values
  Storage[2]=-10.;  // This is also lower then any value produced by an eligible atom, which are all positive

  
  helper.CopyVector(&(Line.endpoints[0]->node->x));
  for (int i =0; i<3; i++)
    {
      if (T.endpoints[i]->node->nr != Line.endpoints[0]->node->nr && T.endpoints[i]->node->nr!=Line.endpoints[1]->node->nr)
	{
	  helper.SubtractVector(&T.endpoints[i]->node->x);
	  break;
	}
    }


  direction1.CopyVector(&Line.endpoints[0]->node->x);
  direction1.SubtractVector(&Line.endpoints[1]->node->x);
  direction1.VectorProduct(T.NormalVector);

  if (direction1.ScalarProduct(&helper)>0)
    {
      direction1.Scale(-1);
    }

  Find_next_suitable_point(*Line.endpoints[0]->node, *Line.endpoints[1]->node, *Line.endpoints[0]->node, 0, &direction1, T.NormalVector, Storage, RADIUS, *mol);
  
  // Konstruiere nun neues Dreieck am Ende der Liste der Dreiecke
  // Next Triangle is Line, atom with number in Storage[0]

  Walker= mol->start;
  while (Walker->nr != (int)Storage[0])
    {
      Walker = Walker->next;
    }

  AddPoint(Walker);

  BPS[0] = new class BoundaryPointSet(Walker);
  BPS[1] = new class BoundaryPointSet(Line.endpoints[0]->node);
  BLS[0] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
  BPS[0] = new class BoundaryPointSet(Walker);
  BPS[1] = new class BoundaryPointSet(Line.endpoints[1]->node);
  BLS[1] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
  BLS[2] = new class BoundaryLineSet(Line);

  BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
  TrianglesOnBoundary.insert( TrianglePair(TrianglesOnBoundaryCount, BTS) );
  TrianglesOnBoundaryCount++;

  for(int i=0;i<NDIM;i++) // sind Linien bereits vorhanden ???
    {
      if (LinesOnBoundary.find(BTS->lines[i]) == LinesOnBoundary.end)
	{
	  LinesOnBoundary.insert( LinePair(LinesOnBoundaryCount, BTS->lines[i]) );
	  LinesOnBoundaryCount++;
	}
    }
  BTS->GetNormalVector(*BTS->NormalVector);

  if( (BTS->NormalVector->ScalarProduct(T.NormalVector)<0 && Storage[1]>0) || \
      (BTS->NormalVector->ScalarProduct(T.NormalVector)>0 && Storage[1]<0))
    {
      BTS->NormalVector->Scale(-1);
    }
  
};


void Find_second_point_for_Tesselation(atom a, atom Candidate, int n, Vector Oben, double* Storage, molecule mol)
{
  int i;
  Vector *AngleCheck;
  atom* Walker;

  AngleCheck->CopyVector(&Candidate.x);
  AngleCheck->SubtractVector(&a.x);
  if (AngleCheck->ScalarProduct(&Oben) < Storage[1])
    {
      Storage[0]=(double)(Candidate.nr);
      Storage[1]=AngleCheck->ScalarProduct(&Oben);
    };

  if (n<5)
    {
      for (i = 0; i< mol.NumberOfBondsPerAtom[Candidate.nr]; i++)
	{
	  Walker = mol.start;
	  while (Candidate.nr != (mol.ListOfBondsPerAtom[Candidate.nr][i]->leftatom->nr ==Candidate.nr ? mol.ListOfBondsPerAtom[Candidate.nr][i]->leftatom->nr : mol.ListOfBondsPerAtom[Candidate.nr][i]->rightatom->nr))
	    {
	      Walker = Walker->next;
	    };
	  
	  Find_second_point_for_Tesselation(a, *Walker, n+1, Oben, Storage, mol);
	    };
    };
  

};


void Tesselation::Find_starting_triangle(molecule mol, const double RADIUS)
{
  int i=0;
  atom Walker;
  atom Walker2;
  atom Walker3;
  int max_index[3];
  double max_coordinate[3];
  Vector Oben;
  Vector helper;

  Oben.Zero();


  for(i =0; i<3; i++)
    {
      max_index[i] =-1;
      max_coordinate[i] =-1;
    }

  Walker = *mol.start;
  while (Walker.next != NULL)
    {
      for (i=0; i<3; i++)
	{
	  if (Walker.x.x[i]>max_coordinate[i])
	    {
	      max_coordinate[i]=Walker.x.x[i];
	      max_index[i]=Walker.nr;
	    }
	}
    }

  //Koennen dies fuer alle Richtungen, legen hier erstmal Richtung auf k=0
  const int k=0;

  Oben.x[k]=1;
  Walker = *mol.start;
  while (Walker.nr != max_index[k])
    {
      Walker = *Walker.next;
    }

  double Storage[3];
  Storage[0]=-1.;   // Id must be positive, we see should nothing be done
  Storage[1]=-2.;   // This will contain the angle, which will be always positive (when looking for second point), when looking for third point this will be the quadrant.
  Storage[2]=-10.;  // This will be an angle looking for the third point.


  for (i=0; i< mol.NumberOfBondsPerAtom[Walker.nr]; i++)
    {
      Walker2 = *mol.start;
      while (Walker2.nr != (mol.ListOfBondsPerAtom[Walker.nr][i]->leftatom->nr == Walker.nr ? mol.ListOfBondsPerAtom[Walker.nr][i]->rightatom->nr : mol.ListOfBondsPerAtom[Walker.nr][i]->leftatom->nr) ) 
	{
	  Walker2 = *Walker2.next;
	}

      Find_second_point_for_Tesselation(Walker, Walker2, 0, Oben, Storage, mol);
    }

  Walker2 = *mol.start;

  while (Walker2.nr != int(Storage[0]))
    {
      Walker = *Walker.next;
    }
  
  helper.CopyVector(&Walker.x);
  helper.SubtractVector(&Walker2.x);
  Oben.ProjectOntoPlane(&helper);
  helper.VectorProduct(&Oben);

       Find_next_suitable_point(Walker, Walker2, *(mol.ListOfBondsPerAtom[Walker.nr][i]->leftatom->nr == Walker.nr ? mol.ListOfBondsPerAtom[Walker.nr][i]->rightatom : mol.ListOfBondsPerAtom[Walker.nr][i]->leftatom), 0, &helper, &Oben, Storage, RADIUS, mol);
  Walker3 = *mol.start;
  while (Walker3.nr != int(Storage[0]))
    {
      Walker3 = *Walker3.next;
    }

  //Starting Triangle is Walker, Walker2, index Storage[0]

  AddPoint(&Walker);
  AddPoint(&Walker2);
  AddPoint(&Walker3);

  BPS[0] = new class BoundaryPointSet(&Walker);
  BPS[1] = new class BoundaryPointSet(&Walker2);
  BLS[0] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
  BPS[0] = new class BoundaryPointSet(&Walker);
  BPS[1] = new class BoundaryPointSet(&Walker3);
  BLS[1] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
  BPS[0] = new class BoundaryPointSet(&Walker);
  BPS[1] = new class BoundaryPointSet(&Walker2);
  BLS[2] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);  

  BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
  TrianglesOnBoundary.insert( TrianglePair(TrianglesOnBoundaryCount, BTS) );
  TrianglesOnBoundaryCount++;

  for(int i=0;i<NDIM;i++) 
    {
      LinesOnBoundary.insert( LinePair(LinesOnBoundaryCount, BTS->lines[i]) );
      LinesOnBoundaryCount++;
    };

       BTS->GetNormalVector(*BTS->NormalVector);

       if( BTS->NormalVector->ScalarProduct(&Oben)<0)
	 {
	   BTS->NormalVector->Scale(-1);
	 }
};


void Find_non_convex_border(Tesselation* Tess, molecule mol) 
{
  const double RADIUS =6;
  Tess->Find_starting_triangle(mol, RADIUS);

  for (LineMap::iterator baseline = Tess->LinesOnBoundary.begin(); baseline != Tess->LinesOnBoundary.end(); baseline++) 
    if (baseline->second->TrianglesCount == 1) 
      {
	Tess->Find_next_suitable_triangle(&mol, baseline->second, baseline->second->triangles.begin()->second, RADIUS); //the line is there, so there is a triangle, but only one.

      }
    else
      {
	printf("There is a line with %d triangles adjacent", baseline->second->TrianglesCount);
      }
};