#ifndef LINE_H
#define LINE_H

#include <mathlib.h>
#include <typedefs.h>
#include <zero.h>

/*!
\brief Line class for 2 and 3 dimensional lines.

Optimization Warning: 
 Use this class with caution when optimizing code. 
 This is because the inlining can have a significant impact 
 on performance in a negative way when not done correctly,
 which is easy to do. 

Logic for nml==0 is not supported but this may change.
 
*/
template< typename PT, typename PD >
class line
{
public:

  /** The lines gradient. */
  PT nml;

  /** The start of the line. */
  PT pos;

  /** Construct a line from the gradient and an initial point. */
  line(PT const & nml_, PT const & pos_);

  /** Construct a line from the end points. */ 
  line(PT const & p0, PT const & p1, boolc endpoints);

  /** Construc the line pos+nml*t. */
  void construct(PT const & nml_,PT const & pos_); 
  /** Given two points construct the line (p0,p1). */
  void constructfromendpoints(PT const & p0, PT const & p1)
    { pos=p0; nml=(p1-p0); }

  /** Normalize the gradient. */
  void normalize();

  /** Normalize the gradient to the given length. */
  void normalize(PD const len);
    
  /** Line as a parametric equation. */
  PT const operator () (PD const t) const
    { return pos + nml*t; }

  /** Give a 2D point on the line find its position. */
  boolc tD2(PD & t, PT const & p) const;

  /** Give a 3D point on the line find its position. */
  boolc tD3(PD & t, PT const & p) const;
  
  /** Serialize this object by writing it out as a string. */
  operator stringc () const;

  /** Test for normal before using functions that may divide by zero. */
  boolc isnormalzero() const
    { return zero<PD>::test(nml.dot()); }

  /** Find the nearest point on the line to p, return the 
      t value.  Assumes the gradient nml is not zero. 
      The line points t=0,1 <--> (pos,pos+nml). 
  */
  void nearestpoint(PD & t, PT const & p) const;

  /** Find the nearest point to the line segment.
      The line points t=0,1 <--> (pos,pos+nml). 
  */
  void nearestpointcapped(PD & t, PT const & p) const;
  
  /** Find the nearest point q on the line to p. 
      Assumes the gradient nml is not zero. */
  void nearestpoint(PT & q, PT const & p) const;

  /**Minimize the distance to point p from point x on line a+b*t.*/
  static void nearestpointonline
  (
    PT & x,
    PT const & p,
    PT const & a,
    PT const & b
  );

  /** Minimize two points on lines, t1 is this line and 
      t2 is line L2. */
  boolc lineD3minimized
  (
    PD & t1, 
    PD & t2, 
    line<PT,PD> const & L2
  ) const;


  /** Find the two lines intersection. */
  boolc intersects
  (
    PD & t0,
    PD & t1,
    line<PT,PD> const & L2
  );

};


//---------------------------------------------------------
// Implementation

template< typename PT, typename PD >
boolc line<PT,PD>::intersects
(
  PD & t0,
  PD & t1,
  line<PT,PD> const & L2
)
{
  return solver<PD>::d2linearequ
  (
    t0,
    t1,
    nml.x,
    L2.nml.x*((PD)-1.0),
    nml.y,
    L2.nml.y*((PD)-1.0),
    L2.pos.x-pos.x,
    L2.pos.y-pos.y
  );
}

template< typename PT, typename PD >
line<PT,PD>:: operator stringc () const
{
  return (stringc)nml + " " + (stringc)pos;
}

template< typename PT, typename PD >
boolc line<PT,PD>::lineD3minimized
(
  PD & t1, 
  PD & t2, 
  line<PT,PD> const & L2
) const
{
  PT C(pos-L2.pos);
  PT E1(nml);
  PT E2(L2.nml*(-1));

  bool res;
  res = solver<PD>::d2linearequ
  (
    t1,t2,
    E1.dot(),E1.dot(E2),
    E1.dot(E2),E2.dot(),
    C.dot(E1)*(-1),
    C.dot(E2)*(-1)
  );

  return res;
}

template< typename PT, typename PD >
void line<PT,PD>::construct(PT const & nml_,PT const & pos_) 
{ 
  nml = nml_; 
  pos=pos_; 
}

template< typename PT, typename PD >
line<PT,PD>::line(PT const & p0, PT const & p1, boolc endpoints)
  : nml(p1-p0), pos(p0) 
{ 
  assert(endpoints); 
}

template< typename PT, typename PD >
line<PT,PD>::line(PT const & nml_, PT const & pos_)
  :nml(nml_), pos(pos_) 
{
}

template< typename PT, typename PD >
void line<PT,PD>::normalize()
{ 
  nml.normalize(); 
}

template< typename PT, typename PD >
void line<PT,PD>::normalize(PD const len)
{ 
  nml.normalize(); 
  nml *= len; 
}

template< typename PT, typename PD >
boolc line<PT,PD>::tD2(PD & t, PT const & p) const
{
  if (zero<PD>::test(nml.x))
  {
    if (zero<PD>::test(nml.y))
      return false;

    t = (p.y-pos.y)/nml.y;
  }
  else
    t = (p.x-pos.x)/nml.x;

  assert( zero<PD>::test( (operator()(t)-p).dot()) );
  return true;
}

template< typename PT, typename PD >
boolc line<PT,PD>::tD3(PD & t, PT const & p) const
{
  if (zero<PD>::test(nml.x))
  {
    if (zero<PD>::test(nml.y))
    {
      if (zero<PD>::test(nml.z))
        return false;

      t = (p.z-pos.z)/nml.z;
    }
    else
      t = (p.y-pos.y)/nml.y;
  }
  else
    t = (p.x-pos.x)/nml.x;

  assert( zero<PD>::test( (operator()(t)-p).dot()) );
  return true;
}

template< typename PT, typename PD >
void line<PT,PD>::nearestpoint(PD & t, PT const & p) const
{
  assert(zero<PD>::test(nml.dot())==false);

  t=(p-pos).dot(nml)/nml.dot();
}

template< typename PT, typename PD >
void line<PT,PD>::nearestpointcapped(PD & t, PT const & p) const
{
  nearestpoint(t,p);
  if (t<((PD)0.0))
    t = (PD)0.0;
  if (t>((PD)1.0))
    t = (PD)1.0;
}

template< typename PT, typename PD >
void line<PT,PD>::nearestpoint(PT & q, PT const & p) const
{
  assert(zero<PD>::test(nml.dot())==false);

  q = pos;
  q += nml*(p-pos).dot(nml)/nml.dot();
}

template< typename PT, typename PD >
void line<PT,PD>::nearestpointonline
(
  PT & x,
  PT const & p,
  PT const & a,
  PT const & b
)
{  
  assert(zero<PD>::test(b.dot())==false);
  x = a + b*( (p-a).dot(b)/(b.dot(b)) );
}


#endif