/*
 * Plane.cpp
 *
 *  Created on: Apr 7, 2010
 *      Author: crueger
 */

#include "Plane.hpp"
#include "vector.hpp"
#include "defs.hpp"
#include "info.hpp"
#include "log.hpp"
#include "verbose.hpp"
#include "Helpers/Assert.hpp"
#include <cmath>

/**
 * generates a plane from three given vectors defining three points in space
 */
Plane::Plane(const Vector &y1, const Vector &y2, const Vector &y3) throw(LinearDependenceException) :
  normalVector(new Vector())
{
  Vector x1 = y1 -y2;
  Vector x2 = y3 -y2;
  if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(x2)) < MYEPSILON)) {
    throw LinearDependenceException(__FILE__,__LINE__);
  }
//  Log() << Verbose(4) << "relative, first plane coordinates:";
//  x1.Output((ofstream *)&cout);
//  Log() << Verbose(0) << endl;
//  Log() << Verbose(4) << "second plane coordinates:";
//  x2.Output((ofstream *)&cout);
//  Log() << Verbose(0) << endl;

  normalVector->at(0) = (x1[1]*x2[2] - x1[2]*x2[1]);
  normalVector->at(1) = (x1[2]*x2[0] - x1[0]*x2[2]);
  normalVector->at(2) = (x1[0]*x2[1] - x1[1]*x2[0]);
  normalVector->Normalize();

  offset=normalVector->ScalarProduct(y1);
}
/**
 * Constructs a plane from two direction vectors and a offset.
 * If no offset is given a plane through origin is assumed
 */
Plane::Plane(const Vector &y1, const Vector &y2, double _offset) throw(LinearDependenceException) :
    normalVector(new Vector()),
    offset(_offset)
{
  Vector x1 = y1;
  Vector x2 = y2;
  if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(x2)) < MYEPSILON)) {
    throw LinearDependenceException(__FILE__,__LINE__);
  }
//  Log() << Verbose(4) << "relative, first plane coordinates:";
//  x1.Output((ofstream *)&cout);
//  Log() << Verbose(0) << endl;
//  Log() << Verbose(4) << "second plane coordinates:";
//  x2.Output((ofstream *)&cout);
//  Log() << Verbose(0) << endl;

  normalVector->at(0) = (x1[1]*x2[2] - x1[2]*x2[1]);
  normalVector->at(1) = (x1[2]*x2[0] - x1[0]*x2[2]);
  normalVector->at(2) = (x1[0]*x2[1] - x1[1]*x2[0]);
  normalVector->Normalize();
}

Plane::Plane(const Vector &_normalVector, double _offset) throw(ZeroVectorException):
  normalVector(new Vector(_normalVector)),
  offset(_offset)
{
  if(normalVector->IsZero())
    throw ZeroVectorException(__FILE__,__LINE__);
  double factor = 1/normalVector->Norm();
  // normalize the plane parameters
  (*normalVector)*=factor;
  offset*=factor;
}

Plane::Plane(const Vector &_normalVector, const Vector &_offsetVector) throw(ZeroVectorException):
    normalVector(new Vector(_normalVector))
{
  if(normalVector->IsZero()){
    throw ZeroVectorException(__FILE__,__LINE__);
  }
  normalVector->Normalize();
  offset = normalVector->ScalarProduct(_offsetVector);
}

Plane::~Plane()
{}


Vector Plane::getNormal(){
  return *normalVector;
}

double Plane::getOffset(){
  return offset;
}

Vector Plane::getOffsetVector() {
  return getOffset()*getNormal();
}

vector<Vector> Plane::getPointsOnPlane(){
  std::vector<Vector> res;
  // first point on the plane
  res[0] = getOffsetVector();
  // first is orthogonal to the plane...
  // an orthogonal vector to this one lies on the plane
  Vector direction;
  direction.GetOneNormalVector(res[0]);
  res[1] = res[0]+direction;
  // get an orthogonal vector to direction and offset (lies on the plane)
  direction.VectorProduct(res[0]);
  direction.Normalize();
  res[2] = res[0] +direction;
  return res;
}


/** Calculates the intersection point between a line defined by \a *LineVector and \a *LineVector2 and a plane defined by \a *Normal and \a *PlaneOffset.
 * According to [Bronstein] the vectorial plane equation is:
 *   -# \f$\stackrel{r}{\rightarrow} \cdot \stackrel{N}{\rightarrow} + D = 0\f$,
 * where \f$\stackrel{r}{\rightarrow}\f$ is the vector to be testet, \f$\stackrel{N}{\rightarrow}\f$ is the plane's normal vector and
 * \f$D = - \stackrel{a}{\rightarrow} \stackrel{N}{\rightarrow}\f$, the offset with respect to origin, if \f$\stackrel{a}{\rightarrow}\f$,
 * is an offset vector onto the plane. The line is parametrized by \f$\stackrel{x}{\rightarrow} + k \stackrel{t}{\rightarrow}\f$, where
 * \f$\stackrel{x}{\rightarrow}\f$ is the offset and \f$\stackrel{t}{\rightarrow}\f$ the directional vector (NOTE: No need to normalize
 * the latter). Inserting the parametrized form into the plane equation and solving for \f$k\f$, which we insert then into the parametrization
 * of the line yields the intersection point on the plane.
 * \param *Origin first vector of line
 * \param *LineVector second vector of line
 * \return true -  \a this contains intersection point on return, false - line is parallel to plane (even if in-plane)
 */
Vector Plane::GetIntersection(const Vector &Origin, const Vector &LineVector)
{
  Info FunctionInfo(__func__);
  Vector res;

  // find intersection of a line defined by Offset and Direction with a  plane defined by triangle
  Vector Direction = LineVector - Origin;
  Direction.Normalize();
  Log() << Verbose(1) << "INFO: Direction is " << Direction << "." << endl;
  //Log() << Verbose(1) << "INFO: PlaneNormal is " << *PlaneNormal << " and PlaneOffset is " << *PlaneOffset << "." << endl;
  double factor1 = Direction.ScalarProduct(*normalVector.get());
  if (fabs(factor1) < MYEPSILON) { // Uniqueness: line parallel to plane?
    Log() << Verbose(1) << "BAD: Line is parallel to plane, no intersection." << endl;
    throw LinearDependenceException(__FILE__,__LINE__);
  }

  double factor2 = Origin.ScalarProduct(*normalVector.get());
  if (fabs(factor2-offset) < MYEPSILON) { // Origin is in-plane
    Log() << Verbose(1) << "GOOD: Origin of line is in-plane." << endl;
    res = Origin;
    return res;
  }

  double scaleFactor = (offset-factor2)/factor1;

  //factor = Origin->ScalarProduct(PlaneNormal)*(-PlaneOffset->ScalarProduct(PlaneNormal))/(Direction.ScalarProduct(PlaneNormal));
  Direction.Scale(scaleFactor);
  res = Origin + Direction;
  Log() << Verbose(1) << "INFO: Scaled direction is " << Direction << "." << endl;

  // test whether resulting vector really is on plane
  ASSERT(fabs(res.ScalarProduct(*normalVector) - offset) < MYEPSILON,
         "Calculated line-Plane intersection does not lie on plane.");
  return res;
};

/************ Methods inherited from Space ****************/

double Plane::distance(Vector &point){
  double res = point.ScalarProduct(*normalVector)-offset;
  return fabs(res);
}

Vector Plane::getClosestPoint(Vector &point){
  Vector difference = distance(point) * (*normalVector);
  if(difference.IsZero()){
    // the point itself lies on the plane
    return point;
  }
  // get the direction this vector is pointing
  double sign = difference.ScalarProduct(*normalVector);
  // sign cannot be zero, since normalVector and difference are both != zero
  sign = sign/fabs(sign);
  return (point - (sign * difference));
}

bool Plane::isContained(Vector &point){
  return (fabs(point.ScalarProduct(*normalVector) - offset)) < MYEPSILON;
}