#ifndef TETRAHEDRON_H
#define TETRAHEDRON_H

#include <cassert>

#include <halfspaceD3.h>
#include <gausselim.h>
#include <mathlib.h>
#include <point.h>
#include <triangle.h>
#include <triangle3D.h>
#include <typedefs.h>

/*!  
\brief Tetrahedron properties. 

PT is the point type.
PD is the data type comprising the point.
*/
template< typename PT, typename PD >
class tetrahedron 
{
public:

  typedef PT PTtype;
  typedef PD PDtype;

  /** The four points of the tetrahedron. */
  PT pi[4];

  /** Unconstructed tetrahedron. */
  tetrahedron() {}
  /** The first three points form the inner base with a 
      clockwise winding. */
  tetrahedron
  (
    PT const & p0_,
    PT const & p1_,
    PT const & p2_,
    PT const & p3_
  );

  /** The first three points form the inner base with a 
      clockwise winding. */
  void construct
  (
    PT const & p0_,
    PT const & p1_,
    PT const & p2_,
    PT const & p3_
  );

  /** A tetrahedron is formed, but there is no guarantee of
      point order as they are swapped to get a correctly
      ordered tetrahedron. */
  void constructUnordered
  (
    PT const & p0_,
    PT const & p1_,
    PT const & p2_,
    PT const & p3_
  );

  /** The average of the four points. */
  void centroid(PT & x) const;

  /** Generalizing the equilateral triangle to a tetrahedron,
      i indexes the face opposite point i, construct a point
      which has all sides of the new tetrahedron with the same 
      triangle. */
  void equilaterali(PT & x, uint i) const;

  /** Center of sphere. ie distances to verticies are all equal. */
  void circumcenter(PT & x) const;
  /** Center of triangle opposite vertex i. */
  void circumcenter(PT & x, uintc i) const;

  void verticiespoint(PT & x, uintc i) const;

  /** Calculate the altitude to point on a triangle face. */
  void triangleverticiespoint(PT & x, uintc i) const;

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


  /** The triangle opposite the point i with anticlockwise 
      point ordering facing outside the tetrahedron, 
      arbitary orientation. */
  void trianglei
  ( 
    triangle3D<PT,PD> & x, 
    uintc i
  ) const;

  

  /** If the point representation is invalid and NDEBUG is not 
      definded the function asserts. */
  boolc valid() const;


  typedef void (triangle3D<PT,PD>::*Fptr)(PT&) const;

  /** Apply a function to generate a point from each side
      of the tetrahedron from the 3D triangle, the four
      points form another tetrahedron. */
  void applytoself( tetrahedron<PT,PD> & tet, Fptr fn ) const;
  void applytoself( tetrahedron<PT,PD> & tet, Fptr fn, uintc n ) const;
/*
  {
    triangle3D<PT,PD> ti[4];
    PT qi[4];
    for (uint i=0; i<4; ++i)
    {
      trianglei(ti[i],i);
      (ti[i].*fn)(qi[i]);
    }
    tet.constructUnordered(qi[0],qi[1],qi[2],qi[3]);  
  }
*/
  

};


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

template< typename PT, typename PD >
void tetrahedron<PT,PD>::applytoself
( 
  tetrahedron<PT,PD> & tet, 
  tetrahedron<PT,PD>::Fptr fn,
  uintc n 
) const
{
  if (n==0)
    return;

  applytoself(tet,fn);
  
  for (uint i=1; i<n; ++i)
  {
    tetrahedron<PT,PD> tet2(tet);
    tet2.applytoself(tet,fn);
  }
}


template< typename PT, typename PD >
void tetrahedron<PT,PD>::applytoself
( 
  tetrahedron<PT,PD> & tet, 
  tetrahedron<PT,PD>::Fptr fn 
) const
{
  triangle3D<PT,PD> ti[4];
  PT qi[4];
  for (uint i=0; i<4; ++i)
  {
    trianglei(ti[i],i);
    (ti[i].*fn)(qi[i]);
  }
  tet.constructUnordered(qi[0],qi[1],qi[2],qi[3]);  
}

template< typename PT, typename PD >
void tetrahedron<PT,PD>::equilaterali(PT & x, uint i) const
{
  triangle3D<PT,PD> w;
  trianglei(w,i);
  PT a;
  w.centroid(a);
  PT b;
  w.normal(b);
  PT a2(a-w.pi[0]);
  // The distance between point 1 and 2, squared.
  PD l0 = (w.pi[1]-w.pi[2]).dot();

  // Currently must solve in doubles.
  PD t0;
  PD t1;
  //double t0;
  //double t1;
  solvequadratic(t0,t1,b.dot(),a2.dot(b)*2.0,a2.dot()-l0);
  PD t=t0;
  if (t<0)
    t=t1;
  assert(t>0);

  x = a+b*t;
}

template< typename PT, typename PD >
void tetrahedron<PT,PD>::trianglei
( 
  triangle3D<PT,PD> & x, 
  uintc i
) const
{
  PT q;
  switch (i)
  {
    case 0: x.construct(pi[3],pi[2],pi[1]); break;
    case 1: x.construct(pi[0],pi[2],pi[3]); break;
    case 2: x.construct(pi[1],pi[0],pi[3]); break;
    case 3: x.construct(pi[0],pi[1],pi[2]); break;
    default: assert(false); return;
  }

#ifndef NDEBUG
  halfspaceD3<PT,PD> h(x.pi[2],x.pi[1],x.pi[0]);
  assert( h.isInside(pi[i]) );
#endif
}
  
template< typename PT, typename PD >
void tetrahedron<PT,PD>::centroid(PT & x) const
{
  x = pi[0];
  x += pi[1];
  x += pi[2];
  x += pi[3];
  x *= 0.25;
}

template< typename PT, typename PD >
boolc tetrahedron<PT,PD>::valid() const
{
  PT a0(pi[1]-pi[0]);
  PT a1(pi[2]-pi[0]);
  PT a2;
  crossproduct::evalxyz(a2,a1,a0);

  PT q(pi[3]-pi[0]);
  bool res = (q.x*a2.x+q.y*a2.y+q.z*a2.z>0);

  return res;
}

template< typename PT, typename PD >
void tetrahedron<PT,PD>::circumcenter(PT & x) const
{
  // Translate the points : p[0] is at the origin.
  PT p[4];
  for (uint i=0; i<4; ++i)
  {
    p[i] = pi[i];
    p[i] -= pi[0];
  };

  PD e[] = 
  {
    p[1].x, p[1].y, p[1].z, p[1].dot()*0.5,
    p[2].x, p[2].y, p[2].z, p[2].dot()*0.5,
    p[3].x, p[3].y, p[3].z, p[3].dot()*0.5
  };

  //gausselim<PD,3> g(e,1E-15);
  gausselim<PD> g(e,3);
//cout << "Initial system" << endl;
//  g.print();
  g.eval();
//cout << "Solve system." << endl;
//g.print();

  PD s[3];
  g.columnC(s);

  x = pi[0];
  x.x += s[0];
  x.y += s[1];
  x.z += s[2];
  
//  return pi[0]+(PT)(s[0],s[1],s[2]);
}


template< typename PT, typename PD >
void tetrahedron<PT,PD>::circumcenter(PT & x, uintc i ) const
{
assert(false);
/*
// <TODO>
//  T N3(hi[i].nml);

  T a(pi[(i+1)%4]);
  T b(pi[(i+2)%4]);
  T c(pi[(i+3)%4]);

  crossprod< T > cross;
  T N1,N2;
  cross( N1, a-b, N3 );
  cross( N2, c-b, N3 );

  T mba = (b+a)*0.5;
  T mbc = (b+c)*0.5;

  T t0, t1;

  d2matsolve
  (
    t0, t1,
    N1.x, -N2.x, 
    N1.y, -N2.y, 
    mbc.x - mba.x,
    mbc.y - mba.y
  );

  return N1*t0 + mba;
*/
}


/*
template< typename PT, typename PD >
PT const tetrahedron<PT,PD>::bisectangle(uintc i) const
{
  PT A,B,C;
  PT X;

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

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

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

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

    default:
      assert(false);
  }

  PD a,b,c;
  a = sqrt((A - X).dot());
  b = sqrt((B - X).dot());
  c = sqrt((C - X).dot());

//cout << SHOW(A) << endl;
//cout << SHOW(B) << endl;
//cout << SHOW(C) << endl;
//cout << SHOW(X) << endl;

//cout << SHOW(a) << " " SHOW(b) << " " << SHOW(c) << " " << endl;
assert(false);

  PD den = 1.0 / ( (a + b + c ) * 2.0 );
//cout << SHOW(den);
PT Z = (A*(b+c)+B*(a+c)+C*(a+b))*den;
//cout << SHOW(b+c) << " " << SHOW(a+c) << " " << SHOW(a+b);
//cout << SHOW((a+b+c)*2.0) << endl;
//cout << SHOW(Z) << endl;
  
  return Z; 
}
*/




template< typename PT, typename PD >
void tetrahedron<PT,PD>::verticiespoint(PT & x, uintc i) const
{
  PT a,b,c,d;

  // Let d be the opposite point.
  // Choose a as vector vertex.

  switch(i)
  {
    case 0:
      a=pi[3]; b=pi[2]; c=pi[1]; d=pi[0];
      break;

    case 1:
      a=pi[0]; b=pi[2]; c=pi[3]; d=pi[1];
      break;

    case 2:
      a=pi[0]; b=pi[1]; c=pi[3]; d=pi[2];
//cout << SHOW(a) << endl;
//cout << SHOW(b) << endl;
//cout << SHOW(c) << endl;
      break;

    case 3:
      a=pi[0]; b=pi[1]; c=pi[2]; d=pi[3];
      break;
   
    default:
      assert(false);
  }

  // ab
  PT A(b-a);
  // ac
  PT B(c-a);

  PT N;
  crossproduct::eval(N,A,B);
  //crossprod< T >()(N,A,B);
  assert(N.dot()!=0.0);
  
  x = N*( N.dot(a-d) / N.dot() )+ d;
}
  

  





template< typename PT, typename PD >
void tetrahedron<PT,PD>::triangleverticiespoint(PT & x, uintc i) const
{
  PT a,b,c;

  switch(i)
  {
    case 0:
      a=pi[1]; b=pi[2]; c=pi[3];
      break;

    case 1:
      a=pi[0]; b=pi[2]; c=pi[3];
      break;

    case 2:
      a=pi[0]; b=pi[1]; c=pi[3];
//cout << SHOW(a) << endl;
//cout << SHOW(b) << endl;
//cout << SHOW(c) << endl;
      break;

    case 3:
      a=pi[0]; b=pi[1]; c=pi[2];
      break;
   
    default:
      assert(false);
  }

  PT ab = b - a;
  PT ac = c - a;

//cout << SHOW(ab) << endl;
//cout << SHOW(ac) << endl;

  PT w;
  crossproduct::eval(w,ab,ac);
  //crossprod< T > cross;
  //cross(w,ab,ac);

  PT act;
  PT abt;
  crossproduct::eval(act,w,ac);
  crossproduct::eval(abt,w,ab);
  //cross(act,w,ac);
  //cross(abt,w,ab);

  PT t0, t1;

  // Usually only one solving is necessary.
  // However if the determinant is zero, try
  // all the other combinations, one should be ok.

  // Assuming that the object is a tetrahedron: all the 
  // four points are unique.

  bool test = 
  d2matsolve
  (
    t0, t1,
    abt.x, -act.x,
    abt.y, -act.y,
    b.x - c.x,
    b.y - c.y
  );

  if (!test)
  {
    test = 
    d2matsolve
    (
      t0, t1,
      abt.x, -act.x,
       abt.z, -act.z,
      b.x - c.x,
      b.z - c.z
    );
  }

  if (!test)
  {
    test = 
    d2matsolve
    (
      t0, t1,
      abt.y, -act.y,
       abt.z, -act.z,
      b.y - c.y,
      b.z - c.z
    );
  }

  x = c + abt * t0;
}


template< typename PT, typename PD >
tetrahedron<PT,PD>::tetrahedron
(
  PT const & p0,
  PT const & p1,
  PT const & p2,
  PT const & p3
)
{
  construct(p0,p1,p2,p3);
}

template< typename PT, typename PD >
void tetrahedron<PT,PD>::construct
(
  PT const & p0,
  PT const & p1,
  PT const & p2,
  PT const & p3
)
{
  pi[0] = p0;
  pi[1] = p1;
  pi[2] = p2;
  pi[3] = p3;

  assert(valid());
}

template< typename PT, typename PD >
void tetrahedron<PT,PD>::constructUnordered
(
  PT const & p0,
  PT const & p1,
  PT const & p2,
  PT const & p3
)
{
  pi[0] = p0;
  pi[1] = p1;
  pi[2] = p2;
  pi[3] = p3;

  if (valid()==false)
  {
    pi[0] = p2;
    pi[2] = p0;

    assert(valid());
  }
}


#endif