#include <mathlib.h>
#include <trianglespace.h>
#include <d3meshpartition.h>

typedef point3<double> pt3;
typedef point3<double> const pt3c;

bool const d3meshpartition::isInside(pt3c & x) const
{
  vector<d3tri> const & vi(meshB.vi);
  vector<pt3> const & pt(meshB.pt);
  uintc sz = vi.size();
  for (uint i=1; i<sz; ++i)
  {
    d3tri const & y(vi[i]);
     
    trianglespace< pt3, double > t
    ( 
      pt[ y.pi[0] ],
      pt[ y.pi[1] ],
      pt[ y.pi[2] ]
    );

    if (!t.isInside(x))
      return true;
  }

  return false;
}



bool const d3meshpartition::intersection(pt3 & x, pt3c & w, pt3c & b) const
{

  vector<d3tri> const & vi(meshB.vi);
  vector<pt3> const & pt(meshB.pt);

  static vector<pt3> solutions;
  solutions.clear();

  double t0,t1;

  uintc sz = vi.size();
  uint k;
  for (uint i=1; i<sz; ++i)
  {
    d3tri const & y(vi[i]);

    if (y.isonboundary()==false)
      continue;

    for (k=0; k<3; ++k)
    {
      if (y.ni[k]!=0)
        continue;

      pt3c & c(pt[ y.pi[(k+1)%3] ]);
      pt3c & d(pt[ y.pi[(k+2)%3] ]);
      if (lineintersection(t0,t1,w,b,c,d))
      {
        if ((t1>=0.0)&&(t1<=1.0))
          solutions.push_back(c+(d-c)*t1);
      }
    }
  }

  assert(solutions.empty()==false);

  uintc solsz = solutions.size();
  if (solsz==1)
  {
    x = solutions[0];
    return true;
  }

  uint di = 0;
  double dmin = (w-solutions[0]).dot();
  double d;
 
  for (uint i=1; i<solsz; ++i)
  {
    d = (w-solutions[i]).dot();
    if (d<dmin)
    {
      dmin = d;
      di = i;
    }
  }

  x = solutions[di];
  return true;
}

bool const d3meshpartition::isOnBoundary(pt3c & w, double const zero) const
{
  // Find the simplex with the point w.

  vector<d3tri> const & vi(meshB.vi);
  vector<pt3> const & pt(meshB.pt);
  uintc sz = vi.size();
  for (uint i=1; i<sz; ++i)
  {
    d3tri const & y(vi[i]);
     
    // Inefficent code. Just get working, optimize later.
    trianglespace< pt3, double > t
    ( 
      pt[ y.pi[0] ],
      pt[ y.pi[1] ],
      pt[ y.pi[2] ]
    );

//    if (!t.isInside(w))
    if (t.isOnBoundary(w,zero))
      return true;
  }

  return false;
}