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

#include "Helpers/MemDebug.hpp"

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

/**
 * 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.
 */
Plane::Plane(const Vector &y1, const Vector &y2, double _offset) throw(ZeroVectorException,LinearDependenceException) :
    normalVector(new Vector()),
    offset(_offset)
{
  Vector x1 = y1;
  Vector x2 = y2;
  if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON)) {
    throw ZeroVectorException(__FILE__,__LINE__);
  }

  if((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);
}

/**
 * copy constructor
 */
Plane::Plane(const Plane& plane) :
  normalVector(new Vector(*plane.normalVector)),
  offset(plane.offset)
{}


Plane::~Plane()
{}


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

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

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

vector<Vector> Plane::getPointsOnPlane() const{
  std::vector<Vector> res;
  res.reserve(3);
  // first point on the plane
  res.push_back(getOffsetVector());
  // get a vector that has direction of plane
  Vector direction;
  direction.GetOneNormalVector(getNormal());
  res.push_back(res[0]+direction);
  // get an orthogonal vector to direction and normal (has direction of plane)
  direction.VectorProduct(getNormal());
  direction.Normalize();
  res.push_back(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 Line& line) const
{
  Info FunctionInfo(__func__);
  Vector res;

  double factor1 = getNormal().ScalarProduct(line.getDirection());
  if(fabs(factor1)<MYEPSILON){
    // the plane is parallel... under all circumstances this is bad luck
    // we no have either no or infinite solutions
    if(isContained(line.getOrigin())){
      throw MultipleSolutionsException<Vector>(__FILE__,__LINE__,line.getOrigin());
    }
    else{
      throw LinearDependenceException(__FILE__,__LINE__);
    }
  }

  double factor2 = getNormal().ScalarProduct(line.getOrigin());
  double scaleFactor = (offset-factor2)/factor1;

  res = line.getOrigin() + scaleFactor * line.getDirection();

  // tests to make sure the resulting vector really is on plane and line
  ASSERT(isContained(res),"Calculated line-Plane intersection does not lie on plane.");
  ASSERT(line.isContained(res),"Calculated line-Plane intersection does not lie on line.");
  return res;
};

Vector Plane::mirrorVector(const Vector &rhs) const {
  Vector helper = getVectorToPoint(rhs);
  // substract twice the Vector to the plane
  return rhs+2*helper;
}

Line Plane::getOrthogonalLine(const Vector &origin) const{
  return Line(origin,getNormal());
}

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

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

Vector Plane::getClosestPoint(const Vector &point) const{
  double factor = point.ScalarProduct(*normalVector)-offset;
  if(fabs(factor) < MYEPSILON){
    // the point itself lies on the plane
    return point;
  }
  Vector difference = factor * (*normalVector);
  return (point - difference);
}

// Operators

ostream &operator << (ostream &ost,const Plane &p){
  ost << "<" << p.getNormal() << ";x> - " << p.getOffset() << "=0";
  return ost;
}