/*
 * Line.cpp
 *
 *  Created on: Apr 30, 2010
 *      Author: crueger
 */

#include "Helpers/MemDebug.hpp"

#include "Line.hpp"

#include <cmath>

#include "vector.hpp"
#include "log.hpp"
#include "verbose.hpp"
#include "gslmatrix.hpp"
#include "info.hpp"
#include "Exceptions/LinearDependenceException.hpp"
#include "Exceptions/SkewException.hpp"
#include "Plane.hpp"

using namespace std;

Line::Line(const Vector &_origin, const Vector &_direction) :
  direction(new Vector(_direction))
{
  direction->Normalize();
  origin.reset(new Vector(_origin.partition(*direction).second));
}

Line::Line(const Line &src) :
  origin(new Vector(*src.origin)),
  direction(new Vector(*src.direction))
{}

Line::~Line()
{}


double Line::distance(const Vector &point) const{
  // get any vector from line to point
  Vector helper = point - *origin;
  // partition this vector along direction
  // the residue points from the line to the point
  return helper.partition(*direction).second.Norm();
}

Vector Line::getClosestPoint(const Vector &point) const{
  // get any vector from line to point
  Vector helper = point - *origin;
  // partition this vector along direction
  // add only the part along the direction
  return *origin + helper.partition(*direction).first;
}

Vector Line::getDirection() const{
  return *direction;
}

Vector Line::getOrigin() const{
  return *origin;
}

vector<Vector> Line::getPointsOnLine() const{
  vector<Vector> res;
  res.reserve(2);
  res.push_back(*origin);
  res.push_back(*origin+*direction);
  return res;
}

/** Calculates the intersection of the two lines that are both on the same plane.
 * This is taken from Weisstein, Eric W. "Line-Line Intersection." From MathWorld--A Wolfram Web Resource. http://mathworld.wolfram.com/Line-LineIntersection.html
 * \param *out output stream for debugging
 * \param *Line1a first vector of first line
 * \param *Line1b second vector of first line
 * \param *Line2a first vector of second line
 * \param *Line2b second vector of second line
 * \return true - \a this will contain the intersection on return, false - lines are parallel
 */
Vector Line::getIntersection(const Line& otherLine) const{
  Info FunctionInfo(__func__);

  pointset line1Points = getPointsOnLine();

  Vector Line1a = line1Points[0];
  Vector Line1b = line1Points[1];

  pointset line2Points = otherLine.getPointsOnLine();

  Vector Line2a = line2Points[0];
  Vector Line2b = line2Points[1];

  Vector res;

  auto_ptr<GSLMatrix> M = auto_ptr<GSLMatrix>(new GSLMatrix(4,4));

  M->SetAll(1.);
  for (int i=0;i<3;i++) {
    M->Set(0, i, Line1a[i]);
    M->Set(1, i, Line1b[i]);
    M->Set(2, i, Line2a[i]);
    M->Set(3, i, Line2b[i]);
  }

  //Log() << Verbose(1) << "Coefficent matrix is:" << endl;
  //for (int i=0;i<4;i++) {
  //  for (int j=0;j<4;j++)
  //    cout << "\t" << M->Get(i,j);
  //  cout << endl;
  //}
  if (fabs(M->Determinant()) > MYEPSILON) {
    Log() << Verbose(1) << "Determinant of coefficient matrix is NOT zero." << endl;
    throw SkewException(__FILE__,__LINE__);
  }

  Log() << Verbose(1) << "INFO: Line1a = " << Line1a << ", Line1b = " << Line1b << ", Line2a = " << Line2a << ", Line2b = " << Line2b << "." << endl;


  // constuct a,b,c
  Vector a = Line1b - Line1a;
  Vector b = Line2b - Line2a;
  Vector c = Line2a - Line1a;
  Vector d = Line2b - Line1b;
  Log() << Verbose(1) << "INFO: a = " << a << ", b = " << b << ", c = " << c << "." << endl;
  if ((a.NormSquared() < MYEPSILON) || (b.NormSquared() < MYEPSILON)) {
   res.Zero();
   Log() << Verbose(1) << "At least one of the lines is ill-defined, i.e. offset equals second vector." << endl;
   throw LinearDependenceException(__FILE__,__LINE__);
  }

  // check for parallelity
  Vector parallel;
  double factor = 0.;
  if (fabs(a.ScalarProduct(b)*a.ScalarProduct(b)/a.NormSquared()/b.NormSquared() - 1.) < MYEPSILON) {
    parallel = Line1a - Line2a;
    factor = parallel.ScalarProduct(a)/a.Norm();
    if ((factor >= -MYEPSILON) && (factor - 1. < MYEPSILON)) {
      res = Line2a;
      Log() << Verbose(1) << "Lines conincide." << endl;
      return res;
    } else {
      parallel = Line1a - Line2b;
      factor = parallel.ScalarProduct(a)/a.Norm();
      if ((factor >= -MYEPSILON) && (factor - 1. < MYEPSILON)) {
        res = Line2b;
        Log() << Verbose(1) << "Lines conincide." << endl;
        return res;
      }
    }
    Log() << Verbose(1) << "Lines are parallel." << endl;
    res.Zero();
    throw LinearDependenceException(__FILE__,__LINE__);
  }

  // obtain s
  double s;
  Vector temp1, temp2;
  temp1 = c;
  temp1.VectorProduct(b);
  temp2 = a;
  temp2.VectorProduct(b);
  Log() << Verbose(1) << "INFO: temp1 = " << temp1 << ", temp2 = " << temp2 << "." << endl;
  if (fabs(temp2.NormSquared()) > MYEPSILON)
    s = temp1.ScalarProduct(temp2)/temp2.NormSquared();
  else
    s = 0.;
  Log() << Verbose(1) << "Factor s is " << temp1.ScalarProduct(temp2) << "/" << temp2.NormSquared() << " = " << s << "." << endl;

  // construct intersection
  res = a;
  res.Scale(s);
  res += Line1a;
  Log() << Verbose(1) << "Intersection is at " << res << "." << endl;

  return res;
}

/** Rotates the vector by an angle of \a alpha around this line.
 * \param rhs Vector to rotate
 * \param alpha rotation angle in radian
 */
Vector Line::rotateVector(const Vector &rhs, double alpha) const{
  Vector helper = rhs;

  // translate the coordinate system so that the line goes through (0,0,0)
  helper -= *origin;

  // partition the vector into a part that gets rotated and a part that lies along the line
  pair<Vector,Vector> parts = helper.partition(*direction);

  // we just keep anything that is along the axis
  Vector res = parts.first;

  // the rest has to be rotated
  Vector a = parts.second;
  // we only have to do the rest, if we actually could partition the vector
  if(!a.IsZero()){
    // construct a vector that is orthogonal to a and direction and has length |a|
    Vector y = a;
    // direction is normalized, so the result has length |a|
    y.VectorProduct(*direction);

    res += cos(alpha) * a + sin(alpha) * y;
  }

  // translate the coordinate system back
  res += *origin;
  return res;
}

Plane Line::getOrthogonalPlane(const Vector &origin) const{
  return Plane(getDirection(),origin);
}

std::vector<Vector> Line::getSphereIntersections() const{
  std::vector<Vector> res;

  // line is kept in normalized form, so we can skip a lot of calculations
  double discriminant = 1-origin->NormSquared();
  // we might have 2, 1 or 0 solutions, depending on discriminant
  if(discriminant>=0){
    if(discriminant==0){
      res.push_back(*origin);
    }
    else{
      Vector helper = sqrt(discriminant)*(*direction);
      res.push_back(*origin+helper);
      res.push_back(*origin-helper);
    }
  }
  return res;
}

Line makeLineThrough(const Vector &x1, const Vector &x2){
  if(x1==x2){
    throw LinearDependenceException(__FILE__,__LINE__);
  }
  return Line(x1,x1-x2);
}