#ifndef TRIANGLE_H
#define TRIANGLE_H

#include <cassert>
using namespace std;

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

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

Many geometric properties such as the orthocenter, centroid 
 and circumcenter can be calculated.

This is a monolith design.  Each property could be made its 
 own class but I want efficiency.

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

  typedef PT PTtype;
  typedef PD PDtype;

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

  /** Unconstructed triangle. */
  triangle() {}
  /** Construct from anti clockwise point ordering. */ 
  triangle 
  (
    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
  );

  /** No guarantee of point order because of swapping. */
  void constructUnordered
  (
    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;

  /** Gets the gradient opposite the vertex i. */
  //PT const gradline(uintc i) const;

  /** Gets invards pointing normal opposite the vertex i. */
  //PT const normal(uint i) const;

  /** Get the point forming an equilateral triangle with
      the opposite side. */
  void equilaterali(PT & x, uintc 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, 
    U & center,
    U & circlenormal
  ) const;

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

  /** Normal in 3D space. */
  template< typename U >
  void normal(U & circlenormal) const;

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

  /** Calculate the Orthocenter. */
  void orthocenter(PT & x) 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;

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

  /** Find the circle passing though all the points. */
  void outercircle 
  ( 
    PD & radius, 
    PT & center
  ) const;

  /** Assert if the triangle is invalid. */
  boolc valid() const;
};


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



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

  if (valid()==false)
  {
    pi[1] = p2;
    pi[2] = p1;

    assert(valid());
  }
}

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

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

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

template< typename PT, typename PD >
void triangle<PT,PD>::napoleanpoint(PT & x) const
{
  PT c0;
  equilaterali(c0,0);
  c0 += pi[1];
  c0 += pi[2];
  c0 /= 3.0;
  PT c1;
  equilaterali(c1,1);
  c1 += pi[0];
  c1 += pi[2];
  c1 /= 3.0;

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

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

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

  PT t;
  d2matsolve(t,pi[0]-e0,e1-pi[1],e1-e0);

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

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

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

  x = pi[0]+(b0-pi[0])*v.x;
}
template< typename PT, typename PD >
template< typename U >
void triangle<PT,PD>::normal(U & circlenormal) const
{
  circlenormal.x = 0;
  circlenormal.y = 0;
  circlenormal.z = 1;
}


template< typename PT, typename PD >
template< typename U >
void triangle<PT,PD>::innercircle
( 
  PD & radius, 
  U & center, 
  U & circlenormal 
) const
{
  PT c;
  incenter(c);
  center.x = c.x;
  center.y = c.y;

  PT w;
  line<PT,PD>::nearestpointonline(w,c,pi[0],pi[1]-pi[0]);
  w -= c;

  radius = w.distance();
  normal(circlenormal);
}

template< typename PT, typename PD >
void triangle<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 >
template< typename U >
void triangle<PT,PD>::outercircle
(
  PD & radius, 
  U & center, 
  U & circlenormal
) const
{
  PT c;
  circumcenter(c);
  center.x = c.x;
  center.y = c.y;
  radius = (c-pi[0]).distance();
  normal(circlenormal);
}

template< typename PT, typename PD >
void triangle<PT,PD>::outercircle
(
  PD & radius, 
  PT & center 
) const
{
  PT c;
  circumcenter(c);
  center.x = c.x;
  center.y = c.y;
  radius = (c-pi[0]).distance();
}


template< typename PT, typename PD >
void triangle<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 triangle<PT,PD>::orthocenter(PT & x) const
{
  PT p0w;
  orthocenteri(p0w,0);
  PT p1w;
  orthocenteri(p1w,1);
  PT t;

  d2matsolve(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 triangle<PT,PD>::centroid(PT & x) const
{
  x =  pi[0];
  x += pi[1];
  x += pi[2];
  x /= 3.0;
}

template< typename PT, typename PD >
void triangle<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 triangle<PT,PD>::equilaterali(PT & x, uintc 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);
  }

  // Outwards normal.
  PT n2(n1.y,-n1.x);
  n2.normalize();

  x = g+n2*sqrt(3.0)*0.5*n1.distance();
}

//template< typename PT, typename PD >
//PT const triangle<PT,PD>::normal(uintc i) const
//{
//  PT g(gradline(i));
//  PT g2(-g.y,g.x);
//  return g2;
//}

/*
template< typename PT, typename PD >
PT const triangle<PT,PD>::gradline(uintc i) const
{
  PT g;

  switch(i)
  {
    case 0:
      g = pi[2] - pi[1];
      break;
  
    case 1:
      g = pi[0] - pi[2];
      break;

    case 2:
      g = pi[1] - pi[0];
      break;

    default:
      assert(false);
  }

  return g;
}
*/
 
template< typename PT, typename PD >
void triangle<PT,PD>::orthocenteri(PT & x, uintc i) const
{
  PT z;
  PT m0;
  PT w0;

  switch(i)
  {
    case 0:
      w0 = pi[1];
      m0 = pi[2] - pi[1];

      z = pi[0];
      break;

    case 1:
      w0 = pi[2];
      m0 = pi[0] - pi[2];

      z = pi[1];
      break;

    case 2:
      w0 = pi[0];
      m0 = pi[1] - pi[0];

      z = pi[2];
      break;
  
    default:
      assert(false);
  }

  PT m1(-m0.y,m0.x);
  PT t;
  d2matsolve(t,m0,m1*-1.0,z-w0);

  x = z + m1*t.y;
}

template< typename PT, typename PD >
void triangle<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 triangle<PT,PD>::construct
(
  PT const & _p0,
  PT const & _p1,
  PT const & _p2
)
{
  pi[0] = _p0;
  pi[1] = _p1;
  pi[2] = _p2;

  assert(valid());
}

template< typename PT, typename PD >
triangle<PT,PD>::triangle
(
  PT const & _p0,
  PT const & _p1,
  PT const & _p2
)
{
  construct(_p0,_p1,_p2);
}

template< typename PT, typename PD >
boolc triangle<PT,PD>::valid() const
{
  PT a0(pi[1]-pi[0]);
  PT a1;
  a1.x=-a0.y;
  a1.y=a0.x;
  PT q(pi[2]-pi[0]); 
  bool res = (a1.x*q.x+a1.y*q.y>0);

  return res;
}







#endif