#ifndef TRIANGLE3D_H
#define TRIANGLE3D_H

#include <line.h>

/*!
\brief A triangle in 3D with ways to calculate many 
       triangle properties.

The class is deliberately designed without virtual functions
 or inheritance for efficiency.
*/
template< typename PT, typename PD >
class triangle3D 
{
public:

  typedef PT PTtype;
  typedef PD PDtype;

  /** The three points of the triangle. */
  PT pi[3];

  /** Unconstructed triangle. */
  triangle3D() {}
  /** Construct from anti clockwise point ordering. */ 
  triangle3D
  (
    PT const & p0_,
    PT const & p1_,
    PT const & p2_
  ); 
  /** Construct from anti clockwise point ordering. */ 
  void construct
  (
    PT const & p0_,
    PT const & p1_,
    PT const & p2_
  );

  /** From vertex i extend a line bisecting the angle in half.
      Return where this line intersects the triangles edge. */
  void bisectangle(PT & x, uintc i) const;

  /** Calculate the Centroid. */
  void centroid(PT & x) const;

  /** Calculate the Circumcenter. */
  void circumcenter(PT & x) const;

  /** Get the point forming an equilateral triangle with
      the opposite side. */
  void equilaterali(PT & x, uint i) const;

  /** From the edge construct an equilateral triangle and 
      intersect its extreme vertexes with the opposite point. */
  void fermatpoint(PT & x) const;

  /** Intersection of bisectors with opposite points. */
  void gergonnepoint(PT & x) const;

  /** Calculate the Incenter. */
  void incenter(PT & x) const;
  /** Calculate the point of the inner circle on side i. */
  void incenteri(PT & x, uint i) const;

  /** Find the circle inside the triangle touching all sides. */
  //template< typename U >
  void innercircle
  ( 
    PD & radius, 
    PT & center, 
    PT & circlenormal 
  ) const;

  /** Calculate the midpoint opposite the vertex i. */
  void midpoint(PT& x, uintc i) const;

  /** Find the centroids of equilateral triangles constructed 
      on edges.  Intersect lines from centroid to opposite 
      point on triangle. */
  void napoleanpoint(PT & x) const;

  /** Find the normal of the side opposite the i'th vertex. */
  void normali(PT & x, uintc i) const;

  /** From the indexed point i construct a line to the 
      opposite side intersecting at right angles, 
      calculate this point. */
  void orthocenteri(PT & x, uintc i) const;
  /** Calculate the Orthocenter. */
  void orthocenter(PT & x) const;

  /** Normal in 3D space. */
  template< typename U >
  void normal(U & w) const
    { crossproduct::evalxyz(w, pi[1]-pi[0], pi[2]-pi[0]); }

  /** Find the circle passing though all the points. */
  template< typename U >
  void outercircle 
  ( 
    PD & radius, 
    U & center,
    U & circlenormal
  ) const;

  /** No testing, recommend test at construction context. */
  boolc valid() const;



};

//template< typename PT, typename PD >
//typedef void triangle3D<PT,PD>::pcenterfn(PT &) const;


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


template< typename PT, typename PD >
template< typename U >
void triangle3D<PT,PD>::outercircle
(
  PD & radius, 
  U & center, 
  U & circlenormal
) const
{
  circumcenter(center);
  radius = (center-pi[0]).distance();
  normal(circlenormal);
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::normali(PT & x, uintc i) const
{
  switch(i)
  {
    case 0: 
      crossproduct::evalxyz(x, pi[2]-pi[1], pi[0]-pi[2]); break;

    case 1: 
      crossproduct::evalxyz(x, pi[0]-pi[2], pi[1]-pi[0]); break;

    case 2: 
      crossproduct::evalxyz(x, pi[1]-pi[0], pi[2]-pi[1] ); break;

    default:
      assert(false);
  }
} 

template< typename PT, typename PD >
void triangle3D<PT,PD>::orthocenteri(PT & x, uintc i) const
{
  assert(i<3);

  PT w0(pi[(i+1)%3]);
  PT w(pi[(i+2)%3]-w0);
  PT nml;
  normal(nml);
//cout << SHOW(nml) << endl;
  PT m1;
  crossproduct::evalxyz(m1, w, nml);
//cout << SHOW(m1) << endl;

  PT t;
  asserteval(d2matsolve3D(t,m1,w*-1.0,w0-pi[i]));

  x = pi[i] + m1*t.x;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::centroid(PT & x) const
{
  x =  pi[0];
  x += pi[1];
  x += pi[2];
  x /= 3.0;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::circumcenter(PT & x) const
{
  //x =(G*3.0-H)*0.5;
  centroid(x);
  x *= 3.0;
  PT H;
  orthocenter(H);
  x -= H;
  x *= 0.5;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::construct
(
  PT const & p0_,
  PT const & p1_,
  PT const & p2_
)
{
  pi[0] = p0_;
  pi[1] = p1_;
  pi[2] = p2_;

  valid();
}

template< typename PT, typename PD >
triangle3D<PT,PD>::triangle3D
(
  PT const & p0_,
  PT const & p1_,
  PT const & p2_
)
{
  construct(p0_,p1_,p2_);
}

template< typename PT, typename PD >
boolc triangle3D<PT,PD>::valid() const
{
  return true;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::equilaterali(PT & x, uint i) const
{
  PT g;
  PT n1;
  switch(i)
  {
    case 0:
      g = (pi[2]+pi[1])*0.5;
      n1=pi[2]-pi[1];
      break;
    case 1:
      g = (pi[2]+pi[0])*0.5;
      n1=pi[0]-pi[2];
      break;
    case 2:
      g = (pi[0]+pi[1])*0.5;
      n1=pi[1]-pi[0];
      break;
    default:
      assert(false);
  }
  PT nml;
  normal(nml);
  PT n2;
  crossproduct::evalxyz(n2,n1,nml);
  assert(n2.distance()!=0);
  n2 /= n2.distance();
  x = g+n2*sqrt(3.0)*0.5*n1.distance();
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::orthocenter(PT & x) const
{
  PT t;
  PT p0w;
  orthocenteri(p0w,0);
//cout << SHOW(p0w) << endl;
  PT p1w;
  orthocenteri(p1w,1);
//cout << SHOW(p1w) << endl;

  d2matsolve3D(t, p0w-pi[0], pi[1]-p1w, pi[1]-pi[0]);

  x = pi[0] + (p0w-pi[0])*t.x;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::napoleanpoint(PT & x) const
{
  PT e0;
  equilaterali(e0,0);
  PT e1;
  equilaterali(e1,1);

  PD a(1.0);
  a /= PD(3.0);

  PT c0(e0);
  c0 += pi[1];
  c0 += pi[2];
  c0 *= a;

  PT c1(e1);
  c1 += pi[0];
  c1 += pi[2];
  c1 *= a;

  PT t;
  d2matsolve3D(t, pi[1]-c1, c0-pi[0], c0-c1);

  x = c1 + (pi[1]-c1)*t.x;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::fermatpoint(PT & x) const
{
  PT c0;
  equilaterali(c0,0);
  PT c1;
  equilaterali(c1,1);

  PT t;
  d2matsolve3D(t, pi[1]-c1, c0-pi[0], c0-c1);

  x = c1 + (pi[1]-c1)*t.x;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::bisectangle(PT & x, uintc i) const
{
  PT A,B;
  PT X;

  switch(i)
  {
    case 0:
      A = pi[1];
      B = pi[2];
      X = pi[0];
      break;

    case 1:
      A = pi[0];
      B = pi[2];
      X = pi[1];
      break;

    case 2:
      A = pi[1];
      B = pi[0];
      X = pi[2];
      break;

    default:
      assert(false);
  }

  PD a = (A-X).distance();
  PD b = (B-X).distance();

  x = (A*b+B*a)/(a+b);
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::midpoint(PT & x, uintc i) const
{
  switch(i)
  {
    case 0:
      x=pi[1]; x += pi[2]; 
      break;

    case 1:
      x=pi[0]; x += pi[2]; 
      break;

    case 2:
      x=pi[1]; x += pi[0];
      break;
      
    default:
      assert(false);
  }

  x *= 0.5;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::gergonnepoint(PT & x) const
{
  PT c0; 
  incenteri(c0,0);
  PT c1; 
  incenteri(c1,1);

  PT t;
  d2matsolve3D(t,pi[0]-c0,c1-pi[1],c1-c0);

  x = c0 + (pi[0]-c0)*t.x;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::incenter(PT & x) const
{
  PT b0;
  bisectangle(b0,0);
  PT b1;
  bisectangle(b1,1);

  PT v;  // The two variables. 
  d2matsolve3D
  (
    v,
    b0 - pi[0], pi[1] - b1,
    pi[1] - pi[0]
  );

  x = pi[0]+(b0-pi[0])*v.x;
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::incenteri(PT & x, uintc i) const
{
  PT q;
  incenter(q);

  switch(i)
  {
    case 0: line<PT,PD>::nearestpointonline(x,q,pi[1],pi[2]-pi[1]); break;
    case 1: line<PT,PD>::nearestpointonline(x,q,pi[2],pi[0]-pi[2]); break;
    case 2: line<PT,PD>::nearestpointonline(x,q,pi[0],pi[1]-pi[0]); break;

    default: assert(false);
  }
}

template< typename PT, typename PD >
void triangle3D<PT,PD>::innercircle
( 
  PD & radius, 
  PT & center, 
  PT & circlenormal 
) const
{
  incenter(center);

  PT w;
  line<PT,PD>::nearestpointonline(w,center,pi[0],pi[1]-pi[0]);
//cout << SHOW(w) << endl;
  w -= center;

  radius = w.distance();

  normal(circlenormal);
/*
cout << SHOW(pi[0]) << " ";
cout << SHOW(pi[1]) << " ";
cout << SHOW(pi[2]) << " ";
cout << endl;
cout << SHOW(radius) << endl;
cout << SHOW(center) << endl;
cout << SHOW(circlenormal) << endl;
*/
}

#endif