/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2010 University of Bonn. All rights reserved.
 * Please see the LICENSE file or "Copyright notice" in builder.cpp for details.
 */

/*
 * 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 "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(0, "New triangle " << Nr << " with end points: ");
  for (PointMap::iterator runner = OrderMap.begin(); runner != OrderMap.end(); runner++) {
    endpoints[Counter] = runner->second;
    LOG(0, " " << *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(0, "Triangle Nr." << Nr << " erased in line " << *lines[i] << ".");
      }
      if (lines[i]->triangles.empty()) {
        //LOG(0, *lines[i] << " is no more attached to any triangle, erasing.");
        delete (lines[i]);
        lines[i] = NULL;
      }
    }
  }
  //LOG(0, "Erasing triangle Nr." << Nr << " itself.");
}
;

/** 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(1, "Normal Vector 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(1, "INFO: Triangle is " << *this << ".");
    LOG(1, "INFO: Line is from " << MolCenter << " to " << x << ".");
    LOG(1, "INFO: Intersection is " << Intersection << ".");

    if (Intersection.DistanceSquared(endpoints[0]->node->getPosition()) < MYEPSILON) {
      LOG(1, "Intersection coindices with first endpoint.");
      return true;
    }   else if (Intersection.DistanceSquared(endpoints[1]->node->getPosition()) < MYEPSILON) {
      LOG(1, "Intersection coindices with second endpoint.");
      return true;
    }   else if (Intersection.DistanceSquared(endpoints[2]->node->getPosition()) < MYEPSILON) {
      LOG(1, "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(1, "INFO: Factor s is " << s << ".");
      if ((s < -MYEPSILON) || ((s-1.) > MYEPSILON)) {
        LOG(1, "INFO: Crosspoint " << CrossPoint << "outside of triangle.");
        return false;
      }
      i++;
    } while (i < 3);
    LOG(1, "INFO: 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;
  }
}
;

/** 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(1, "INFO: Looking for closest point of triangle " << *this << " to " << x << ".");
  GetCenter(Direction);
  try {
    Line l = makeLineThrough(x, Direction);
    ClosestPoint = Plane(NormalVector, (endpoints[0]->node->getPosition())).GetIntersection(l);
  }
  catch (LinearAlgebraException &excp) {
    (ClosestPoint) = (x);
  }

  // 2. Calculate in plane part of line (x, intersection)
  Vector InPlane = (x) - (ClosestPoint); // points from plane intersection to straight-down point
  InPlane.ProjectOntoPlane(NormalVector);
  InPlane += ClosestPoint;

  LOG(2, "INFO: Triangle is " << *this << ".");
  LOG(2, "INFO: Line is from " << Direction << " to " << x << ".");
  LOG(2, "INFO: In-plane part is " << InPlane << ".");

  // Calculate cross point between one baseline and the desired point such that distance is shortest
  double ShortestDistance = -1.;
  bool InsideFlag = false;
  Vector CrossDirection[3];
  Vector CrossPoint[3];
  Vector helper;
  for (int i = 0; i < 3; i++) {
    // treat direction of line as normal of a (cut)plane and the desired point x as the plane offset, the intersect line with point
    Direction = (endpoints[(i+1)%3]->node->getPosition()) - (endpoints[i%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] = Plane(Direction, InPlane).GetIntersection(l);
    CrossDirection[i] = CrossPoint[i] - InPlane;
    CrossPoint[i] -= (endpoints[i%3]->node->getPosition());  // cross point was returned as absolute vector
    const double s = CrossPoint[i].ScalarProduct(Direction)/Direction.NormSquared();
    LOG(2, "INFO: Factor s is " << s << ".");
    if ((s >= -MYEPSILON) && ((s-1.) <= MYEPSILON)) {
          CrossPoint[i] += (endpoints[i%3]->node->getPosition());  // make cross point absolute again
      LOG(2, "INFO: Crosspoint is " << CrossPoint[i] << ", intersecting BoundaryLine between " << endpoints[i % 3]->node->getPosition() << " and " << endpoints[(i + 1) % 3]->node->getPosition() << ".");
      const double distance = CrossPoint[i].DistanceSquared(x);
      if ((ShortestDistance < 0.) || (ShortestDistance > distance)) {
        ShortestDistance = distance;
        (ClosestPoint) = CrossPoint[i];
      }
    } else
      CrossPoint[i].Zero();
  }
  InsideFlag = true;
  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;
  }
  if (InsideFlag) {
    (ClosestPoint) = InPlane;
    ShortestDistance = InPlane.DistanceSquared(x);
  } else { // also check endnodes
    for (int i = 0; i < 3; i++) {
      const double distance = x.DistanceSquared(endpoints[i]->node->getPosition());
      if ((ShortestDistance < 0.) || (ShortestDistance > distance)) {
        ShortestDistance = distance;
        (ClosestPoint) = (endpoints[i]->node->getPosition());
      }
    }
  }
  LOG(1, "INFO: 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])

  ));
}
;

/** 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(1, "INFO: Center 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;
}
;