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

/*
 * BoundaryTriangleSet.cpp
 *
 *  Created on: Jul 29, 2010
 *      Author: heber
 */

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

#include "CodePatterns/MemDebug.hpp"

#include "BoundaryTriangleSet.hpp"

#include <iostream>

#include "BoundaryLineSet.hpp"
#include "BoundaryPointSet.hpp"
#include "Atom/TesselPoint.hpp"

#include "Helpers/defs.hpp"

#include "CodePatterns/Assert.hpp"
#include "CodePatterns/Info.hpp"
#include "CodePatterns/Log.hpp"
#include "CodePatterns/Verbose.hpp"
#include "LinearAlgebra/Exceptions.hpp"
#include "LinearAlgebra/Line.hpp"
#include "LinearAlgebra/Plane.hpp"
#include "LinearAlgebra/Vector.hpp"

using namespace std;

/** Constructor for BoundaryTriangleSet.
 */
BoundaryTriangleSet::BoundaryTriangleSet() :
  Nr(-1)
{
  //Info FunctionInfo(__func__);
  for (int i = 0; i < 3; i++) {
    endpoints[i] = NULL;
    lines[i] = NULL;
  }
}
;

/** Constructor for BoundaryTriangleSet with three lines.
 * \param *line[3] lines that make up the triangle
 * \param number number of triangle
 */
BoundaryTriangleSet::BoundaryTriangleSet(class BoundaryLineSet * const line[3], const int number) :
  Nr(number)
{
  //Info FunctionInfo(__func__);
  // set number
  // set lines
  for (int i = 0; i < 3; i++) {
    lines[i] = line[i];
    lines[i]->AddTriangle(this);
  }
  // get ascending order of endpoints
  PointMap 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;
  LOG(4, "DEBUG: New triangle " << Nr << " with end points: ");
  for (PointMap::iterator runner = OrderMap.begin(); runner != OrderMap.end(); runner++) {
    endpoints[Counter] = runner->second;
    LOG(4, "DEBUG:    " << *endpoints[Counter]);
    Counter++;
  }
  ASSERT(Counter >= 3,"We have a triangle with only two distinct endpoints!");
};


/** Destructor of BoundaryTriangleSet.
 * Removes itself from each of its lines' LineMap and removes them if necessary.
 * \note When removing triangles from a class Tesselation, use RemoveTesselationTriangle()
 */
BoundaryTriangleSet::~BoundaryTriangleSet()
{
  //Info FunctionInfo(__func__);
  for (int i = 0; i < 3; i++) {
    if (lines[i] != NULL) {
      if (lines[i]->triangles.erase(Nr)) {
        //LOG(5, "DEBUG: Triangle Nr." << Nr << " erased in line " << *lines[i] << ".");
      }
      if (lines[i]->triangles.empty()) {
        //LOG(5, "DEBUG: " << *lines[i] << " is no more attached to any triangle, erasing.");
        delete (lines[i]);
        lines[i] = NULL;
      }
    }
  }
  //LOG(5, "DEBUG: Erasing triangle Nr." << Nr << " itself.");
}
;

/** Calculates the area of this triangle.
 *
 * @return surface area in between the tree points of this triangle
 */
double BoundaryTriangleSet::getArea() const
{
	Vector x;
	Vector y;
	x = getEndpoint(0) - getEndpoint(1);
  y = getEndpoint(0) - getEndpoint(2);
  const double a = x.Norm();
  const double b = y.Norm();
  const double c = getEndpoint(2).distance(getEndpoint(1));
  const double area = sqrt(((a + b + c) * (a + b + c) - 2 * (a * a + b * b + c * c)) / 16.); // area of tesselated triangle
  return area;
}

/** Calculates the normal vector for this triangle.
 * Is made unique by comparison with \a OtherVector to point in the other direction.
 * \param &OtherVector direction vector to make normal vector unique.
 */
void BoundaryTriangleSet::GetNormalVector(const Vector &OtherVector)
{
  //Info FunctionInfo(__func__);
  // get normal vector
  NormalVector = Plane((endpoints[0]->node->getPosition()),
                       (endpoints[1]->node->getPosition()),
                       (endpoints[2]->node->getPosition())).getNormal();

  // make it always point inward (any offset vector onto plane projected onto normal vector suffices)
  if (NormalVector.ScalarProduct(OtherVector) > 0.)
    NormalVector.Scale(-1.);
  LOG(4, "DEBUG: Normal Vector of " << *this << " is " << NormalVector << ".");
}
;

/** Finds the point on the triangle \a *BTS through which the line defined by \a *MolCenter and \a *x crosses.
 * We call Vector::GetIntersectionWithPlane() to receive the intersection point with the plane
 * Thus we test if it's really on the plane and whether it's inside the triangle on the plane or not.
 * The latter is done as follows: We calculate the cross point of one of the triangle's baseline with the line
 * given by the intersection and the third basepoint. Then, we check whether it's on the baseline (i.e. between
 * the first two basepoints) or not.
 * \param *out output stream for debugging
 * \param &MolCenter offset vector of line
 * \param &x second endpoint of line, minus \a *MolCenter is directional vector of line
 * \param &Intersection intersection on plane on return
 * \return true - \a *Intersection contains intersection on plane defined by triangle, false - zero vector if outside of triangle.
 */

bool BoundaryTriangleSet::GetIntersectionInsideTriangle(const Vector & MolCenter, const Vector & x, Vector &Intersection) const
{
  //Info FunctionInfo(__func__);
  Vector CrossPoint;
  Vector helper;

  try {
    Line centerLine = makeLineThrough(MolCenter, x);
    Intersection = Plane(NormalVector, (endpoints[0]->node->getPosition())).GetIntersection(centerLine);

    LOG(4, "DEBUG: Triangle is " << *this << ".");
    LOG(4, "DEBUG: Line is from " << MolCenter << " to " << x << ".");
    LOG(4, "DEBUG: Intersection is " << Intersection << ".");

    if (Intersection.DistanceSquared(endpoints[0]->node->getPosition()) < MYEPSILON) {
      LOG(4, "DEBUG: Intersection coindices with first endpoint.");
      return true;
    }   else if (Intersection.DistanceSquared(endpoints[1]->node->getPosition()) < MYEPSILON) {
      LOG(4, "DEBUG: Intersection coindices with second endpoint.");
      return true;
    }   else if (Intersection.DistanceSquared(endpoints[2]->node->getPosition()) < MYEPSILON) {
      LOG(4, "DEBUG: Intersection coindices with third endpoint.");
      return true;
    }
    // Calculate cross point between one baseline and the line from the third endpoint to intersection
    int i = 0;
    do {
      Line line1 = makeLineThrough((endpoints[i%3]->node->getPosition()),(endpoints[(i+1)%3]->node->getPosition()));
      Line line2 = makeLineThrough((endpoints[(i+2)%3]->node->getPosition()),Intersection);
      CrossPoint = line1.getIntersection(line2);
      helper = (endpoints[(i+1)%3]->node->getPosition()) - (endpoints[i%3]->node->getPosition());
      CrossPoint -= (endpoints[i%3]->node->getPosition());  // cross point was returned as absolute vector
      const double s = CrossPoint.ScalarProduct(helper)/helper.NormSquared();
      LOG(4, "DEBUG: Factor s is " << s << ".");
      if ((s < -MYEPSILON) || ((s-1.) > MYEPSILON)) {
        LOG(4, "DEBUG: Crosspoint " << CrossPoint << "outside of triangle.");
        return false;
      }
      i++;
    } while (i < 3);
    LOG(4, "DEBUG: Crosspoint " << CrossPoint << " inside of triangle.");
    return true;
  }
  catch (LinearAlgebraException &excp) {
    LOG(1, boost::diagnostic_information(excp));
    ELOG(1, "Alas! Intersection with plane failed - at least numerically - the intersection is not on the plane!");
    return false;
  }
  return true;
}


/** Finds the point on the triangle to the point \a *x.
 * We call Vector::GetIntersectionWithPlane() with \a * and the center of the triangle to receive an intersection point.
 * Then we check the in-plane part (the part projected down onto plane). We check whether it crosses one of the
 * boundary lines. If it does, we return this intersection as closest point, otherwise the projected point down.
 * Thus we test if it's really on the plane and whether it's inside the triangle on the plane or not.
 * The latter is done as follows: We calculate the cross point of one of the triangle's baseline with the line
 * given by the intersection and the third basepoint. Then, we check whether it's on the baseline (i.e. between
 * the first two basepoints) or not.
 * \param *x point
 * \param *ClosestPoint desired closest point inside triangle to \a *x, is absolute vector
 * \return Distance squared between \a *x and closest point inside triangle
 */
double BoundaryTriangleSet::GetClosestPointInsideTriangle(const Vector &x, Vector &ClosestPoint) const
{
  //Info FunctionInfo(__func__);
  Vector Direction;

  // 1. get intersection with plane
  LOG(3, "DEBUG: Looking for closest point of triangle " << *this << " to " << x << ".");
  LOG(3, "DEBUG: endpoints are " << endpoints[0]->node->getPosition() << ","
      << endpoints[1]->node->getPosition() << ", and " << endpoints[2]->node->getPosition() << ".");
  try {
    ClosestPoint = Plane(NormalVector, (endpoints[0]->node->getPosition())).getClosestPoint(x);
  }
  catch (LinearAlgebraException &excp) {
    (ClosestPoint) = (x);
  }
  Vector InPlane(ClosestPoint); // points from plane intersection to straight-down point
  LOG(4, "DEBUG: Closest point on triangle plane is " << ClosestPoint << ".");

  // 2. Calculate in plane part of line (x, intersection)

  // Calculate cross point between one baseline and the desired point such that distance is shortest
  Vector CrossDirection[3];
  Vector CrossPoint[3];
  for (int i = 0; i < 3; i++) {
    const Vector Direction = (endpoints[i%3]->node->getPosition()) - (endpoints[(i+1)%3]->node->getPosition());
    // calculate intersection, line can never be parallel to Direction (is the same vector as PlaneNormal);
    Line l = makeLineThrough((endpoints[i%3]->node->getPosition()), (endpoints[(i+1)%3]->node->getPosition()));
    CrossPoint[i] = l.getClosestPoint(InPlane);
    // NOTE: direction of line is normalized, hence s must not necessarily be in [0,1] for the baseline
    LOG(4, "DEBUG: Closest point on line from " << (endpoints[(i+1)%3]->node->getPosition())
        << " to " << (endpoints[i%3]->node->getPosition()) << " is " << CrossPoint[i] << ".");
    CrossPoint[i] -= (endpoints[(i+1)%3]->node->getPosition());  // cross point was returned as absolute vector
    const double s = CrossPoint[i].ScalarProduct(Direction)/Direction.NormSquared();
    LOG(5, "DEBUG: Factor s is " << s << ".");
    if ((s >= -MYEPSILON) && ((s-1.) <= MYEPSILON)) {
      CrossPoint[i] += (endpoints[(i+1)%3]->node->getPosition());  // make cross point absolute again
      LOG(5, "DEBUG: Crosspoint is " << CrossPoint[i] << ", intersecting BoundaryLine between "
          << endpoints[i % 3]->node->getPosition() << " and "
          << endpoints[(i + 1) % 3]->node->getPosition() << ".");
    } else {
      // set to either endpoint of BoundaryLine
      if (s < -MYEPSILON)
        CrossPoint[i] = (endpoints[(i+1)%3]->node->getPosition());
      else
        CrossPoint[i] = (endpoints[i%3]->node->getPosition());
      LOG(5, "DEBUG: Crosspoint is " << CrossPoint[i] << ", intersecting outside of BoundaryLine between "
          << endpoints[i % 3]->node->getPosition() << " and "
          << endpoints[(i + 1) % 3]->node->getPosition() << ".");
    }
    CrossDirection[i] = CrossPoint[i] - InPlane;
  }

  bool InsideFlag = true;
  double ShortestDistance = -1.;
  for (int i = 0; i < 3; i++) {
    const double sign = CrossDirection[i].ScalarProduct(CrossDirection[(i + 1) % 3]);
    const double othersign = CrossDirection[i].ScalarProduct(CrossDirection[(i + 2) % 3]);

    if ((sign > -MYEPSILON) && (othersign > -MYEPSILON)) // have different sign
      InsideFlag = false;
    // update current best candidate
    const double distance = CrossPoint[i].DistanceSquared(x);
    if ((ShortestDistance < 0.) || (ShortestDistance > distance)) {
      ShortestDistance = distance;
      (ClosestPoint) = CrossPoint[i];
    }
  }

  if (InsideFlag) {
    (ClosestPoint) = InPlane;
    ShortestDistance = InPlane.DistanceSquared(x);
  }
  LOG(3, "DEBUG: Closest Point is " << ClosestPoint << " with shortest squared distance is " << ShortestDistance << ".");

  return ShortestDistance;
}


/** Checks whether lines is any of the three boundary lines this triangle contains.
 * \param *line line to test
 * \return true - line is of the triangle, false - is not
 */
bool BoundaryTriangleSet::ContainsBoundaryLine(const BoundaryLineSet * const line) const
{
  //Info FunctionInfo(__func__);
  for (int i = 0; i < 3; i++)
    if (line == lines[i])
      return true;
  return false;
}
;

/** Checks whether point is any of the three endpoints this triangle contains.
 * \param *point point to test
 * \return true - point is of the triangle, false - is not
 */
bool BoundaryTriangleSet::ContainsBoundaryPoint(const BoundaryPointSet * const point) const
{
  //Info FunctionInfo(__func__);
  for (int i = 0; i < 3; i++)
    if (point == endpoints[i])
      return true;
  return false;
}
;

/** Checks whether point is any of the three endpoints this triangle contains.
 * \param *point TesselPoint to test
 * \return true - point is of the triangle, false - is not
 */
bool BoundaryTriangleSet::ContainsBoundaryPoint(const TesselPoint * const point) const
{
  //Info FunctionInfo(__func__);
  for (int i = 0; i < 3; i++)
    if (point == endpoints[i]->node)
      return true;
  return false;
}
;

/** Checks whether three given \a *Points coincide with triangle's endpoints.
 * \param *Points[3] pointer to BoundaryPointSet
 * \return true - is the very triangle, false - is not
 */
bool BoundaryTriangleSet::IsPresentTupel(const BoundaryPointSet * const Points[3]) const
{
  //Info FunctionInfo(__func__);
  LOG(1, "INFO: Checking " << Points[0] << "," << Points[1] << "," << Points[2] << " against " << endpoints[0] << "," << endpoints[1] << "," << endpoints[2] << ".");
  return (((endpoints[0] == Points[0]) || (endpoints[0] == Points[1]) || (endpoints[0] == Points[2])) && ((endpoints[1] == Points[0]) || (endpoints[1] == Points[1]) || (endpoints[1] == Points[2])) && ((endpoints[2] == Points[0]) || (endpoints[2] == Points[1]) || (endpoints[2] == Points[2])

  ));
}
;

/** Checks whether three given \a *Points coincide with triangle's endpoints.
 * \param *Points[3] pointer to BoundaryPointSet
 * \return true - is the very triangle, false - is not
 */
bool BoundaryTriangleSet::IsPresentTupel(const BoundaryTriangleSet * const T) const
{
  //Info FunctionInfo(__func__);
  return (((endpoints[0] == T->endpoints[0]) || (endpoints[0] == T->endpoints[1]) || (endpoints[0] == T->endpoints[2])) && ((endpoints[1] == T->endpoints[0]) || (endpoints[1] == T->endpoints[1]) || (endpoints[1] == T->endpoints[2])) && ((endpoints[2] == T->endpoints[0]) || (endpoints[2] == T->endpoints[1]) || (endpoints[2] == T->endpoints[2])

  ));
}
;

/** Checks whether a given point is inside the plane of the triangle and inside the
 * bounds defined by its BoundaryLineSet's.
 *
 * @param point point to check
 * @return true - point is inside place and inside all BoundaryLine's
 */
bool BoundaryTriangleSet::IsInsideTriangle(const Vector &point) const
{
  Info FunctionInfo(__func__);

  // check if it's inside the plane
  try {
    Plane trianglePlane(
        endpoints[0]->node->getPosition(),
        endpoints[1]->node->getPosition(),
        endpoints[2]->node->getPosition());
    if (!trianglePlane.isContained(point)) {
      LOG(1, "INFO: Point " << point << " is not inside plane " << trianglePlane << " by "
          << trianglePlane.distance(point) << ".");
      return false;
    }
  } catch(LinearDependenceException) {
    // triangle is degenerated, it's just a line (i.e. one endpoint is right in between two others
    for (size_t i = 0; i < NDIM; ++i) {
      try {
        Line l = makeLineThrough(
            lines[i]->endpoints[0]->node->getPosition(),
            lines[i]->endpoints[1]->node->getPosition());
        if (l.isContained(GetThirdEndpoint(lines[i])->node->getPosition())) {
          // we have the largest of the three lines
          LOG(1, "INFO: Linear-dependent case where point " << point << " is on line " << l << ".");
          return (l.isContained(point));
        }
      } catch(ZeroVectorException) {
        // two points actually coincide
        try {
          Line l = makeLineThrough(
              lines[i]->endpoints[0]->node->getPosition(),
              GetThirdEndpoint(lines[i])->node->getPosition());
          LOG(1, "INFO: Degenerated case where point " << point << " is on line " << l << ".");
          return (l.isContained(point));
        } catch(ZeroVectorException) {
          // all three points coincide
          if (point.DistanceSquared(lines[i]->endpoints[0]->node->getPosition()) < MYEPSILON) {
            LOG(1, "INFO: Full-Degenerated case where point " << point << " is on three endpoints "
                << lines[i]->endpoints[0]->node->getPosition() << ".");
            return true;
          }
          else return false;
        }
      }
    }
  }

  // check whether it lies on the correct side as given by third endpoint for
  // each BoundaryLine.
  // NOTE: we assume here that endpoints are linear independent, as the case
  // has been caught before already extensively
  for (size_t i = 0; i < NDIM; ++i) {
    Line l = makeLineThrough(
        lines[i]->endpoints[0]->node->getPosition(),
        lines[i]->endpoints[1]->node->getPosition());
    Vector onLine( l.getClosestPoint(point) );
    LOG(1, "INFO: Closest point on boundary line is " << onLine << ".");
    Vector inTriangleDirection( GetThirdEndpoint(lines[i])->node->getPosition() - onLine );
    Vector inPointDirection(point - onLine);
    if ((inTriangleDirection.NormSquared() > MYEPSILON) && (inPointDirection.NormSquared() > MYEPSILON))
      if (inTriangleDirection.ScalarProduct(inPointDirection) < -MYEPSILON)
        return false;
  }

  return true;
}


/** Returns the endpoint which is not contained in the given \a *line.
 * \param *line baseline defining two endpoints
 * \return pointer third endpoint or NULL if line does not belong to triangle.
 */
class BoundaryPointSet *BoundaryTriangleSet::GetThirdEndpoint(const BoundaryLineSet * const line) const
{
  //Info FunctionInfo(__func__);
  // sanity check
  if (!ContainsBoundaryLine(line))
    return NULL;
  for (int i = 0; i < 3; i++)
    if (!line->ContainsBoundaryPoint(endpoints[i]))
      return endpoints[i];
  // actually, that' impossible :)
  return NULL;
}
;

/** Returns the baseline which does not contain the given boundary point \a *point.
 * \param *point endpoint which is neither endpoint of the desired line
 * \return pointer to desired third baseline
 */
class BoundaryLineSet *BoundaryTriangleSet::GetThirdLine(const BoundaryPointSet * const point) const
{
  //Info FunctionInfo(__func__);
  // sanity check
  if (!ContainsBoundaryPoint(point))
    return NULL;
  for (int i = 0; i < 3; i++)
    if (!lines[i]->ContainsBoundaryPoint(point))
      return lines[i];
  // actually, that' impossible :)
  return NULL;
}
;

/** Calculates the center point of the triangle.
 * Is third of the sum of all endpoints.
 * \param *center central point on return.
 */
void BoundaryTriangleSet::GetCenter(Vector & center) const
{
  //Info FunctionInfo(__func__);
  center.Zero();
  for (int i = 0; i < 3; i++)
    (center) += (endpoints[i]->node->getPosition());
  center.Scale(1. / 3.);
  LOG(4, "DEBUG: Center of BoundaryTriangleSet is at " << center << ".");
}

/**
 * gets the Plane defined by the three triangle Basepoints
 */
Plane BoundaryTriangleSet::getPlane() const{
  ASSERT(endpoints[0] && endpoints[1] && endpoints[2], "Triangle not fully defined");

  return Plane(endpoints[0]->node->getPosition(),
               endpoints[1]->node->getPosition(),
               endpoints[2]->node->getPosition());
}

Vector BoundaryTriangleSet::getEndpoint(int i) const{
  ASSERT(i>=0 && i<3,"Index of Endpoint out of Range");

  return endpoints[i]->node->getPosition();
}

string BoundaryTriangleSet::getEndpointName(int i) const{
  ASSERT(i>=0 && i<3,"Index of Endpoint out of Range");

  return endpoints[i]->node->getName();
}

/** output operator for BoundaryTriangleSet.
 * \param &ost output stream
 * \param &a boundary triangle
 */
ostream &operator <<(ostream &ost, const BoundaryTriangleSet &a)
{
  ost << "[" << a.Nr << "|" << a.getEndpointName(0) << "," << a.getEndpointName(1) << "," << a.getEndpointName(2) << "]";
  //  ost << "[" << a.Nr << "|" << a.endpoints[0]->node->Name << " at " << *a.endpoints[0]->node->node << ","
  //      << a.endpoints[1]->node->Name << " at " << *a.endpoints[1]->node->node << "," << a.endpoints[2]->node->Name << " at " << *a.endpoints[2]->node->node << "]";
  return ost;
}
;