#include <iomanip>
#include <vector>
#include <sstream>
using namespace std;

#include <d3fan.h>
#include <d3fan2.h>
#include <d3tess.h>
#include <message.h>
#include <print.h>
#include <simplexD2linked.h>
#include <simplexface.h>
#include <trianglespace.h>



void d3tess::splitmidpoint1D(uintc s, uintc p1)
{
  assert(s<vi.size());
  assert(p1<pt.size());

  simplexD2linked & S(vi[s]);

  assert(S.piInverseHas(p1)==true);

  uintc j = S.piInverse(p1);

  uintc a = S.pi[(j+1)%3];
  uintc b = S.pi[(j+2)%3];

  uintc n = pt.size();
  pt.push_back( (pt[a]+pt[b])*0.5 );

  split1D(s,p1,n);
}

void d3tess::removenullsimplexes()
{    
  // Remove any null simplexes at the end of the 
  //   tessellation so the last simplex is non null 
  //   or there is no tessellation.
  if (vi[vi.size()-1].isnull())
  {
    for ( ; vi[vi.size()-1].isnull() && (vi.size()>1) ; )
      vi.pop_back();
  }

  // Swap null simplexes with non null simplexes
  //   and delete them.
  for (uint i=1; i<vi.size(); ++i)
  {
    if (vi[i].isnull()==false)
      continue;

    simplexswap(i,vi.size()-1);
    for ( ; vi[vi.size()-1].isnull() && (vi.size()>1) ; )
      vi.pop_back();
  }

  debugcheck();
}

void d3tess::simplexswap(uintc s0, uintc s1)
{
  assert(s0<vi.size());
  assert(s1<vi.size());

  if (s0==s1)
    return;

  uint a[3];
  uint b[3];

  // Copy the neighbor links of both simplexes.
  uint i;

  simplexD2linked & A(vi[s0]);
  for (i=0; i<3; ++i)
    a[i] = A.ni[i];

  simplexD2linked & B(vi[s1]);
  for (i=0; i<3; ++i)
    b[i] = B.ni[i];

  // Change the links in the actual mesh.
  for (i=0; i<3; ++i)
  {
    if (a[i]==0)
      continue;

    simplexD2linked & x(vi[a[i]]);
    if (x.niInverseHas(s1)==false)
      x.changelink(s0,s1);
    else
    {
      // The case where the neighbor has both simplexes
      //   on its boundary.
      uint c = x.niInverse(s1);
      x.ni[ x.niInverse(s0) ] = s1;  
      x.ni[ c ] = s0;
    }
  }

  for (i=0; i<3; ++i)
  {
    if (b[i]==0)
      continue;
    simplexD2linked & x(vi[b[i]]);
    if (x.niInverseHas(s0)==false)
      x.changelink(s1,s0);
  }

  // Swap the simplexes s0 and s1 in memory
  uint c;
  for (i=0; i<3; ++i)
  {
    c = A.pi[i];
    A.pi[i] = B.pi[i];
    B.pi[i] = c;

    c = A.ni[i];
    A.ni[i] = B.ni[i];
    B.ni[i] = c;
  }
}

  

void d3tess::splitwithline
( 
  uintc s, 
  uintc p0, 
  uintc w0, 
  uintc p1, 
  uintc w1 
)
{
  uintc n0 = vi.size();
  assert(s<n0);

  // Neighboring triangle opposite p0
  uintc neib = vi[s].nifrom(p0);

  // If the first splitting point has no neighbor on the given 
  //   boundary then handle this case. 
  if (neib==0)
  {
    split1D(s,p0,w0);
    if ( vi[s].piInverseHas(p1) )
      split1D(n0,w0,w1);
    else
      split1D(s,w0,w1);

    return;
  }

  // The first split will create 4 new triangles and delete two.

  split1D(s,p0,w0);

  //  The second triangle to be split contains the point p0 but
  //    does not contain the point p1 as one of its points.

  uint k[4];
  k[0] = s;
  k[1] = neib;
  k[2] = n0;
  k[3] = n0+1;

  simplexD2linked *t;
  uint ki;
  for ( uint i=0; i<4; ++i )
  {
    ki=k[i];
    t = & vi[ki];
    if ( t->piInverseHas(p1) == true )
      continue;
    if ( t->piInverseHas(p0) == false )
      continue;

    split1D(ki,w0,w1);

    return;
  }

  assert(false);  // The case should have been handled.  
}




void d3tess::splitonboundarynoneighbor
( 
  uintc s,
  uintc j, 
  uintc w 
)
{
  // Two triangles produced from split.

  assert(w<pt.size());
  assert(w!=0);
  assert(s<vi.size());

  // Extract info from mesh.

  uint p[4];
  uint n[2];

  simplexD2linked & x(vi[s]);

  p[0] = x.pi[(j+1)%3];
  p[1] = x.pi[(j+2)%3];
  p[2] = x.pi[j];
  p[3] = w;

  assert(x.ni[j]==0);

  n[0] = x.ni[(j+1)%3];
  n[1] = x.ni[(j+2)%3];

  // Check the mesh before changing it.
  debugcheck();

  // Indexes to new triangles.
  uint k[2];
  k[0] = s;
  k[1] = vi.size();

  x =           simplexD2linked(p[1],p[2],p[3],k[1],0,n[0]);
  vi.push_back( simplexD2linked(p[3],p[2],p[0],n[1],0,k[0]) );

  if (n[1]!=0)
    vi[n[1]].changelink(s,k[1]);

  if (n[0]!=0)
    vi[n[0]].changelink(s,k[0]);

  debugcheck();
}

void d3tess::split1D( uintc s, uintc p0, uintc w0 )
{
  assert(s<vi.size());

  uintc j = vi[s].piInverse(p0); 

  if (vi[s].ni[j]==0)
  {
    splitonboundarynoneighbor(s,j,w0);
    return;
  }

  // General Splitting on Boundary,
  //  four triangles produced.

  assert(w0<pt.size());
  assert(w0!=0);

  // Extract info from mesh.

  uint p[5];
  uint n[4];

  simplexD2linked & x(vi[s]);
  uint b = x.ni[j];
  assert(b!=0);
  simplexD2linked & y(vi[b]);

  assert( isconnected(s,b) );  

  uintc j2 = y.niInverse(s);

  p[0] = x.pi[(j+1)%3];
  p[1] = y.pi[j2];
  p[2] = x.pi[(j+2)%3];
  p[3] = x.pi[j];
  p[4] = w0;

  n[0] = x.ni[(j+1)%3];
  n[1] = x.ni[(j+2)%3];
  n[2] = y.ni[(j2+1)%3];
  n[3] = y.ni[(j2+2)%3];

  // Check the mesh before changing it.
  debugcheck();

  // Indexes to new triangles.
  uint k[4];
  k[0] = s;
  k[1] = b;
  k[2] = vi.size();
  k[3] = k[2]+1;

  x =           simplexD2linked(p[4],p[2],p[3],n[0],k[1],k[3]);
  y =           simplexD2linked(p[4],p[3],p[0],n[1],k[2],k[0]);
  vi.push_back( simplexD2linked(p[4],p[0],p[1],n[2],k[3],k[1]) );
  vi.push_back( simplexD2linked(p[4],p[1],p[2],n[3],k[0],k[2]) );

  if (n[0]!=0)
    vi[n[0]].changelink(s,k[0]);

  if (n[1]!=0)
    vi[n[1]].changelink(s,k[1]);

  if (n[2]!=0)
    vi[n[2]].changelink(b,k[2]);

  if (n[3]!=0)
    vi[n[3]].changelink(b,k[3]);

  debugcheck();
}

bool const d3tess::simplexdelete(uintc s)
{
  if (s>=vi.size())
    return false;

  simplexD2linked & t(vi[s]);

  // If already deleted return true.
  if (t.isnull())
    return true;

  uint nb;

  // Visit each neighbor and unlink.
  for (int i=0; i<3; ++i)
  {
    nb = t.ni[i];
    if (nb!=0)
      vi[nb].changelink(s,0);
  }

  t.setnull();

  return true;
}



d3tess::~d3tess()
{
  delete minimizer;
  minimizer=0;
}

d3tess::d3tess( uintc arrayMax )
  : 
  fan(*this), 
  minimizer( new d3minoperator(*this) ),
  debugenable(true)
//  enablesplit(true),
//  enablefan(true)
{ 
  pt.reserve(arrayMax);
  vi.reserve(arrayMax);

  reset();
}

void d3tess::initialize()
{
  assert(pt.size()>3);
  assert(vi.size()==1);

  pt3c & p0(pt[1]);
  pt3c & p1(pt[2]);
  pt3c & p2(pt[3]);

  halfspaceD2< pt3, double > h(p1,p0);
  if (h.isInside(p2)==false)
    vi.push_back( simplexD2linked(1,2,3, 0,0,0) );
  else
    vi.push_back( simplexD2linked(3,2,1, 0,0,0) );
}

void d3tess::reset()
{
  pt.clear();
  vi.clear();

  pt.push_back( pt3(0.0,0.0,0.0) );
  vi.push_back( simplexD2linked() );

  cp=1;
}

void d3tess::minimizerSet( d3minoperator* m)
{
  assert(m!=0);

  delete minimizer;
  minimizer = m;
}

bool const d3tess::surfaceviewable(uintc k) const
{
  simplexD2linked const & x(vi[cp]);
  halfspaceD2< pt3, double > h(pt[x.pi[vs.v[1]]],pt[x.pi[vs.v[0]]]);
  return h.isInside(pt[k]);
}
  
  


bool const d3tess::isconnected(uintc a, uintc b) const 
{
#ifndef NDEBUG
  uintc sz(vi.size());
  assert(a<sz);
  assert(b<sz);
#endif

  //simplexD2linked const & A(vi[a]);
  //simplexD2linked const & B(vi[b]);

  if (vi[a].niInverseHas(b)==false)
    return false;

  if (vi[b].niInverseHas(a)==false)
    return false;

/*
  bool res;

  A.niInverse(res,b);
  if (res==false)
    return false;

  B.niInverse(res,a);
  if (res==false)
    return false;
*/

  return true;
}


 
bool const d3tess::isconvex(uintc a, uintc b) const
{
#ifndef NDEBUG
  uintc sz(vi.size());
  assert(a<sz);
  assert(b<sz);
#endif

  simplexD2linked const & A(vi[a]);
  simplexD2linked const & B(vi[b]);

  bool res;

  uint ai = A.niInverse(res,b);
  if (res==false)
    return false;

  uint bi = B.niInverse(res,a);
  if (res==false)
    return false;

  pt3 const & P0(pt[A.pi[ai]]);
  pt3 const & P1(pt[A.pi[(ai+1)%3]]);
  pt3 const & P2(pt[A.pi[(ai+2)%3]]);

//cout << SHOW(P0) << " " << SHOW(P1) << " " << SHOW(P2) << endl;

  halfspaceD2< pt3, double > h1(P0,P1);
  halfspaceD2< pt3, double > h2(P2,P0);

  pt3 const & P3(pt[B.pi[bi]]);

  if (h1.isInside(P3)==false)
    return false;
  
  if (h2.isInside(P3)==false)
    return false;

  return true;
}


bool const d3tess::moveleft()
{
  vs.clockwise();

  if (movedown()==false)
  {
    vs.anticlockwise();
    return false;
  }

  return true;
}

bool const d3tess::moveright()
{
  vs.anticlockwise();

  if (movedown()==false)
  {
    vs.clockwise();
    return false;
  }

  return true;
}

bool const d3tess::movedown()
{
  simplexD2linked const & x(vi[cp]);

  uint cp2 = x.ni[ vs.v[2] ];
  if (cp2==0)
    return false;

  simplexD2linked const &  y(vi[cp2]);

  uint yi = y.niInverse(cp);
  cp = cp2;
  vs.set(yi);

  return true;
}

bool const d3tess::boundaryorient()
{
  simplexD2linked const & x(vi[cp]);
  
  for (uint i=0; i<3; ++i)
    if(x.ni[0]==0)
    {
      vs.set(i);
      return true;
    }

  return false;
}

simplexface const d3tess::cpsimplexfaceget() const
{ 
  assert(vs.validstate()); 
  return simplexface(cp,vs.v[2]); 
}

void d3tess::cpsimplexfaceset( simplexface const & sf)
{
  cp = sf.id;
  vs.set(sf.face);
}

double const d3tess::cpbasemeasure() const
{
  simplexD2linked const & x(vi[cp]);

  pt3 p0( pt[x.pi[vs.v[0]]]);
  pt3 p1( pt[x.pi[vs.v[1]]]);
  p0 -= p1;

  return p0.distance();
}




void d3tess::split2D( uintc w )
{
  assert(w<pt.size());
  assert(w!=0);

  // Extract info from mesh.

  uint p[4];
  uint n[3];

//cout << SHOW(pt[w]) << "  ";
//cout << cpsimplex() << endl;

  simplexD2linked & x(vi[cp]);
  for (uint i=0; i<3; ++i)
  {
    p[i] = x.pi[i];
    n[i] = x.ni[i];
  }
  p[3] = w;

  debugcheck();

  // Indexes to new triangles.
  uint k[3];
  k[0] = cp;
  k[1] = vi.size();
  k[2] = k[1]+1;

  x =           simplexD2linked(p[3],p[1],p[2],n[0],k[1],k[2]);
  vi.push_back( simplexD2linked(p[3],p[2],p[0],n[1],k[2],k[0]) );
  vi.push_back( simplexD2linked(p[3],p[0],p[1],n[2],k[0],k[1]) );

  if (n[1]!=0)
    vi[n[1]].changelink(cp,k[1]);

  if (n[2]!=0)
    vi[n[2]].changelink(cp,k[2]);

  debugcheck();

  for (uint i=0; i<3; ++i)
  {
    if (n[i]==0)
      continue;

    assert(isconnected(n[i],k[i]));
  }

  for (uint i=0; i<3; ++i)
  {
    if (n[i]==0)
      continue;

    minimizer->eval(n[i],k[i]);
  }

  debugcheck();
}




void d3tess::addpoint(uintc k)
{

  bool found = searchminimizetowardspoint(k);
  if (found)
  {

//cout << "*1: " << SHOW(enablesplit) << endl;
//    if (enablesplit==false)
//      return;

//cout << "*2" << endl;
    split2D(k);

    return;
  }


//cout << "*3: " << SHOW(enablefan) << endl;
//  if (enablefan==false);
//    return;

//cout << "*4" << endl;

  fan.eval(k);
}

bool const d3tess::searchminimizetowardspoint( uintc p )
{
  // Statistics only.
  movecounter = 0;

  bool notfound = false;
  bool isinside = false;
  uint viewableface;
  for ( ; !notfound; )
    notfound = move_terminated(isinside,viewableface,p);

  if (isinside==false)
  {
    if (vs.v[2]!=viewableface)
      vs.anticlockwise();
    else
      return isinside;

    if (vs.v[2]!=viewableface)
      vs.anticlockwise();
    else
      return isinside;

    if (vs.v[2]!=viewableface)
      assert(false);
  }
  
  return isinside;
}


bool const d3tess::move_terminated
(
  bool & insidetriangle,
  uint & viewableface,
  uintc p
)
{
  //For measuring statistics only.
  ++movecounter;

  simplexD2linked & x(vi[cp]);

  assert(p<pt.size());
  point3<double> const & target(pt[p]);

  trianglespace<pt3,double> t
  ( 
    pt[ x.pi[0] ],
    pt[ x.pi[1] ],
    pt[ x.pi[2] ]
  );

  // Is the point inside the triangle?
  if (!t.isInside(target))
  {
    insidetriangle=true;

    return true;
  }

  for ( uint i=0; i<3; ++i )
  {
    // Is the target viewable?
    if (t.hi[i].isInside(target))
    {
      if(x.ni[i]==0)
      {
        viewableface=i;
        return true;
      }

      // Move
      cp = x.ni[i];

      return false;
    }
  }

  assert(false);

  return false;
}

bool const d3tess::valid(uintc k) const
{
  if (k>=vi.size())
    return false;

  if (vi[k].isnull())
    return true;

  // Look at each neighbor for links.
  for (uint i=0, nb; i<3; ++i)
  {
    nb = vi[k].ni[i];
    if (nb==0)
      continue;

    bool res;

    // Each point bordering neighbor must also be one
    // of the neighbors own points.
    for (uint w=0; w<3; ++w)
    {
      if (w==i)
        continue;

      vi[nb].piInverse( res, vi[k].pi[w] );
      if (res==false)
        return false;
    }

    // The neighbor must have a pointer back to k.

    if (vi[nb].niInverseHas(k)==false)
      return false;

/*
    vi[nb].niInverse( res, k );
    if (res==false)
      return false;
*/
  }

  return true;
}

istream & d3tess::serializeInverse(istream & is)
{
  pt.clear();
  vi.clear();
  cp=1;

  pt3 p;

  uint sz;
  is >> sz;

  uint k;

  for (uint i=0; i<sz; ++i)
  {
    is >> k;
    is >> p;

    pt.push_back(p);
  }

  simplexD2linked t;

  is >> sz;
  for (uint i=0; i<sz; ++i)
  {
    is >> k;
    is >> t;

    vi.push_back(t);
  }

  return is;
}

istream & operator >> (istream & is, d3tess & x)
{
  return x.serializeInverse(is);
}

void d3tess::print(string & s) const
{
  s="";
  uintc ptsz = pt.size();
  { stringstream ss; ss << ptsz; s+=(ss.str()+'\n'); }

  for (uint i=0; i<ptsz; ++i)
  {
    if (i<10)
      s += " ";
    if (i<100)
      s += " ";
    { stringstream ss; ss << i; s+=ss.str(); }
    s += ("  " + (string)(pt[i]) + '\n'); 
  }

  uintc visz = vi.size();
  { stringstream ss; ss << visz; s+=(ss.str()+'\n'); }
  for (uint i=0; i<visz; ++i)
  {
    if (i<10)
      s += " ";
    if (i<100)
      s += " ";
    { stringstream ss; ss << i; s+=ss.str(); }
    s += ("  " + (string)(vi[i]) + '\n');
  }
}

ostream & d3tess::print(ostream & os) const
{
  uintc ptsz = pt.size();
  os << ptsz << endl;

  for (uint i=0; i<ptsz; ++i)
  {
    os << setw(2) << i;
    os << "  " << pt[i] << endl;
  }

  uintc visz = vi.size();
  os << visz << endl;
  for (uint i=0; i<visz; ++i)
  {
    os << setw(2) << i;
    os << "  " << vi[i] << endl;
  }

  return os;
}

ostream & operator << (ostream& os, d3tess const & x)
{
  return x.print(os);
}


void d3tess::surfaceright()
{
  assert( vi[cp].ni[ vs.v[2] ]==0 );

  if( vi[cp].ni[ vs.v[0] ]==0 )
  {
    vs.anticlockwise();
    return;
  }
  
  for ( ; moveright(); );

  vs.anticlockwise();
}

void d3tess::surfaceleft()
{
  assert( vi[cp].ni[ vs.v[2] ]==0 );

  if( vi[cp].ni[ vs.v[1] ]==0 )
  {
    vs.clockwise();
    return;
  }
  
  for ( ; moveleft(); );

  vs.clockwise();
}




bool const d3tess::debugcheck() const 
{

  if (debugenable==false)
    return true;

// This function checks for correct links and
// duplicate traingles.
//
// Another test that could be added is to check the
// triangles winding. ie line(pi[0],pi[1]) to creat a halfplane h
// left and including the line.  h sees p[2]. Include degenerate case
// where all the points lie on the same line.

#ifndef NDEBUG

//cout << "DEBUG ON" << endl;

  bool res(true);
  uintc visz = vi.size();

  // Are the triangles unique.
  for (uint i=1; i<visz; ++i )
  {
    if ( vi[i].isnull() )
      continue;

    for (uint k=i+1; k<visz; ++k)
    {
      if (vi[i]==vi[k])
      {
        res=false;

        messageglobal() << "\n";
        messageglobal() << "error: triangles with the same points.\n";
        messageglobal() << "  check " << i << " " << k << "\n";
        messageglobal() << "vi[i]=" << vi[i] << "\n";
        messageglobal() << "vk[i]=" << vi[k] << "\n";
        messageglobal() << "\n";
        messageglobal() << *this;
      }
    }
  }
  assert(res==true);

  for (uint i=1; i<visz; ++i)
  {
    if ( vi[i].isnull() )
      continue;

    if(valid(i)==false)
    {
      res=false;
      messageglobal() << "error:  ";
      //messageglobal() << setw(2) << i;
      messageglobal() << "  " << vi[i] << "\n";
      messageglobal() << "Printing out tess." << "\n";
      messageglobal() << *this << "\n";
    }
  }

  assert(res==true);

  return res;

#else
  return true;
#endif

}


bool const d3tess::flip()
{
  simplexD2linked & x(vi[cp]);
  uint b = x.ni[vs.v[2]];

  return flip(cp,b);
}

bool const d3tess::flip(uintc a, uintc b)
{
//cout << a << " " << b << " " << isconvex(a,b) << " | ";
  if (isconvex(a,b)==false)
    return false;

  simplexD2linked const & A(vi[a]);
  simplexD2linked const & B(vi[b]);

  // Have vs base bordering triangle b.
  uint ai = A.niInverse(b);
  cp=a;
  vs.set(ai);

  // Extract point and neighbor information
  // from the mesh.

  uint p[4];

  for (uint i=0; i<3; ++i)
    p[i] = A.pi[ vs.v[i] ];
  p[3] = B.pi[B.niInverse(a)];

  uint n[4];
  n[0] = A.ni[ vs.v[0] ];
  n[1] = A.ni[ vs.v[1] ];
  n[2] = B.ni[ B.piInverse(p[0]) ];
  n[3] = B.ni[ B.piInverse(p[1]) ];

//for (uint i=0; i<4; ++i)
//  cout << "p[" << i << "]=" << p[i] << " ";
//cout << endl;

//for (uint i=0; i<4; ++i)
//  cout << "n[" << i << "]=" << n[i] << " ";
//cout << endl;

  // Overwrite the previous triangles with new ones.

  uint k[2];
  k[0] = a;
  k[1] = b;

  vi[k[0]] = simplexD2linked(p[3],p[2],p[0],n[1],n[3],k[1]);
  vi[k[1]] = simplexD2linked(p[2],p[3],p[1],n[2],n[0],k[0]);

  // Relink neighbors to new triangles.
  //   Test if the neighbor exists before resetting.
  if (n[3]!=0)
    vi[n[3]].changelink(b,a);
  if (n[0]!=0)
    vi[n[0]].changelink(a,b);

  // Have vs base bordering b.
  ai = A.niInverse(b);
  cp=a;
  vs.set(ai);

  debugcheck();

  return true; 
}



void d3tess::viadd
( 
  uintc v0, uintc v1, uintc v2,
  uintc n0, uintc n1, uintc n2
)
{ 
  vi.push_back( simplexD2linked(v0,v1,v2,n0,n1,n2) ); 
}


void d3tess::cppreserve(uint & cp0, virtualtriangle & vs0) const
{
  cp0 = cp;
  vs0 = vs;
}

void d3tess::cppreserveInverse
(
   uint const & cp0, 
   virtualtriangle const & vs0
)
{
  cp = cp0;
  vs = vs0;
}


d3tesspreserve::d3tesspreserve( d3tess & _tess)
  : tess(_tess)
{  
  tess.cppreserve(cp0,vs0); 
}

d3tesspreserve::~d3tesspreserve()
{
  tess.cppreserveInverse(cp0,vs0); 
}