// Cutmesh - Copyright (C) 2010-2018 T. Mouton, E. Bechet 
//
// See the LICENSE file for license information and contributions.
// bugs and problems to <thibaud.mouton@gmail.com>.

#include "LTetra.h"

LTetra::LTetra ( LVertex *v0, LVertex *v1, LVertex *v2, LVertex *v3, LTetra *parent )
{
  assert ( v0 != NULL );
  assert ( v1 != NULL );
  assert ( v2 != NULL );
  assert ( v3 != NULL );
  _v[0] = v0;
  v0->setTetra ( this );
  _v[1] = v1;
  v1->setTetra ( this );
  _v[2] = v2;
  v2->setTetra ( this );
  _v[3] = v3;
  v3->setTetra ( this );
  _a[0] = _a[1] = _a[2] = _a[3] = NULL;
  _o[0] = _o[1] = _o[2] = _o[3] = -1;
  _p = parent;
  id = -1;
  index = -1;
  flag = -1;
  _vol = -1;
  _oldvol = -1;
  _is_parent = 0;
  _tf = NULL;
//     if (parent != NULL)
//         parent->add_child(this);
}

void LTetra::print()
{
  printf ( "LTetra %d :\n", getIndex() );
  printf ( "v0 --> %lf %lf %lf\n", getVertex ( 0 )->x(), getVertex ( 0 )->y(), getVertex ( 0 )->z() );
  getVertex ( 0 )->dumpSurf();
  printf ( "v1 --> %lf %lf %lf\n", getVertex ( 1 )->x(), getVertex ( 1 )->y(), getVertex ( 1 )->z() );
  getVertex ( 1 )->dumpSurf();
  printf ( "v2 --> %lf %lf %lf\n", getVertex ( 2 )->x(), getVertex ( 2 )->y(), getVertex ( 2 )->z() );
  getVertex ( 2 )->dumpSurf();
  printf ( "v3 --> %lf %lf %lf\n", getVertex ( 3 )->x(), getVertex ( 3 )->y(), getVertex ( 3 )->z() );
  getVertex ( 3 )->dumpSurf();
}

LVertex LTetra::normal ( int face )
{
  LVertex *p1 = getFaceVertex ( face, 0 );
  LVertex *p2 = getFaceVertex ( face, 1 );
  LVertex *p3 = getFaceVertex ( face, 2 );
  LVertex uu, vv, n, ve;
  uu.setVector ( p1, p2 );
  vv.setVector ( p1, p3 );
  uu.crossproduct ( &vv, &n );
  n.normalize();
  return n;
}

int LTetra::barycentric ( int face, LVertex *v, double b[3] )
{
  LVertex *p1 = getFaceVertex ( face, 0 );
  LVertex *p2 = getFaceVertex ( face, 1 );
  LVertex *p3 = getFaceVertex ( face, 2 );
  LVertex uu, vv, pl;
  uu.setVector ( p1, p2 );
  vv.setVector ( p1, p3 );
  uu.crossproduct ( &vv, &pl );
  double tot = pl.norm();
  if ( tot == 0.0 )
    return 0;
  uu.setVector ( p2, p3 );
  vv.setVector ( p2, v );
  uu.crossproduct ( &vv, &pl );
  b[0] = pl.norm() /tot;
  uu.setVector ( p3, p1 );
  vv.setVector ( p3, v );
  uu.crossproduct ( &vv, &pl );
  b[1] = pl.norm() /tot;
  uu.setVector ( p1, p2 );
  vv.setVector ( p1, v );
  uu.crossproduct ( &vv, &pl );
  b[2] = pl.norm() /tot;
  return 1;
}

int LTetra::barycentric ( LVertex *v, double b[4] )
{
  LVertex *p1 = getVertex ( 0 );
  LVertex *p2 = getVertex ( 1 );
  LVertex *p3 = getVertex ( 2 );
  LVertex *p4 = getVertex ( 3 );
  LVertex uu, vv, ww, pl;
  uu.setVector ( p1, p2 );
  vv.setVector ( p1, p3 );
  ww.setVector ( p1, p4 );
  uu.crossproduct ( &vv, &pl );
  double tot = pl.dotproduct ( &ww );
  if ( tot == 0.0 )
    return 0;
  uu.setVector ( p2, p4 );
  vv.setVector ( p2, p3 );
  ww.setVector ( p2, v );
  uu.crossproduct ( &vv, &pl );
  b[0] = pl.dotproduct ( &ww ) /tot;
  uu.setVector ( p3, p4 );
  vv.setVector ( p3, p1 );
  ww.setVector ( p3, v );
  uu.crossproduct ( &vv, &pl );
  b[1] = pl.dotproduct ( &ww ) /tot;
  uu.setVector ( p1, p4 );
  vv.setVector ( p1, p2 );
  ww.setVector ( p1, v );
  uu.crossproduct ( &vv, &pl );
  b[2] = pl.dotproduct ( &ww ) /tot;
  uu.setVector ( p1, p2 );
  vv.setVector ( p1, p3 );
  ww.setVector ( p1, v );
  uu.crossproduct ( &vv, &pl );
  b[3] = pl.dotproduct ( &ww ) /tot;
  return 1;
}

int LTetra::contain ( LVertex *v )
{
  static double b[4] = {INFINITY, INFINITY, INFINITY, INFINITY};
  barycentric ( v, b );
  for ( int i = 0; i < 4; i++ )
    if ( b[i] < -THRESHOLD )
      return 0;
  return 1;
}


double LTetra::volume()
{
  LVertex *p1 = getVertex ( 0 );
  LVertex *p2 = getVertex ( 1 );
  LVertex *p3 = getVertex ( 2 );
  LVertex *p4 = getVertex ( 3 );
  LVertex uu, vv, ww, pl;
  uu.setVector ( p1, p2 );
  vv.setVector ( p1, p3 );
  ww.setVector ( p1, p4 );
  uu.crossproduct ( &vv, &pl );
  return pl.dotproduct ( &ww ) /6.0;
}

double LTetra::volume ( int face, LVertex *v )
{
  LVertex *p1 = getFaceVertex ( face, 0 );
  LVertex *p2 = getFaceVertex ( face, 1 );
  LVertex *p3 = getFaceVertex ( face, 2 );
  LVertex *p4 = v;
  LVertex uu, vv, ww, pl;
  uu.setVector ( p1, p2 );
  vv.setVector ( p1, p3 );
  ww.setVector ( p1, p4 );
  uu.crossproduct ( &vv, &pl );
  return pl.dotproduct ( &ww ) /6.0;
}

int LTetra::visibility ( int face, LVertex *v, double b[4], LVertex **inter, int *inside )
{
  static int vt[3][3] = {{-1, 2, 1},{2, -1, 0},{1, 0, -1}};
  int idfv[3];
  LVertex *p1 = getFaceVertex ( face, 0 );
  LVertex *p2 = getFaceVertex ( face, 1 );
  LVertex *p3 = getFaceVertex ( face, 2 );
  idfv[0] = getFaceVertexIndex ( face, 0 );
  idfv[1] = getFaceVertexIndex ( face, 1 );
  idfv[2] = getFaceVertexIndex ( face, 2 );
  b[0] = b[1] = b[2] = b[3] = INFINITY;
  ( *inter ) = NULL;
  ( *inside ) = 0;
  barycentric ( v, b );
  int visible = 0;
  int nz = 0;
  int zero[3];
  for ( int i = 0; i < 3; i++ )
  {
    if ( b[idfv[i]] > -THRESHOLD_BARY_FACE )
      visible++;
    /*else */
    if ( fabs ( b[idfv[i]] ) < THRESHOLD_BARY_FACE )
      zero[nz++] = idfv[i];
  }
  if ( ( visible == 3 ) && ( b[face] > -THRESHOLD_BARY_FACE ) )
  {
    *inside = ( b[idfv[0]] > -THRESHOLD_BARY_FACE ) && ( b[idfv[1]] > -THRESHOLD_BARY_FACE ) && ( b[idfv[2]] > -THRESHOLD_BARY_FACE );
    return 0;
  }
  if ( visible == 3 )
  {
    if ( nz == 2 )
    {
      int vfi = idfv[vt[zero[0]][zero[1]]];
      *inter = getFaceVertex ( face, vfi );
    }
    else
    {
      double sum = b[idfv[0]]+b[idfv[1]]+b[idfv[2]];
      double xx = ( b[idfv[0]]*p1->x() +b[idfv[1]]*p2->x() +b[idfv[2]]*p3->x() ) /sum;
      double yy = ( b[idfv[0]]*p1->y() +b[idfv[1]]*p2->y() +b[idfv[2]]*p3->y() ) /sum;
      double zz = ( b[idfv[0]]*p1->z() +b[idfv[1]]*p2->z() +b[idfv[2]]*p3->z() ) /sum;
      *inter = new LVertex ( xx, yy, zz );
    }
    return 1;
  }
  return 0;
}

LTetraRefineField::LTetraRefineField ( LTetra *t )
{
  _owner = t;
  _state = NOT_REFINED;
  refdepth = 0;
  generation = 0;
  rl = 0.0;
  _father = NULL;
  bit_edge = 0;
  for ( int i = 0; i < 8; i++ )
    child[i] = NULL;
  for ( int i = 0; i < 6; i++ )
  {
    LVertex ab;
    ab.setVector ( _owner->getEdgeVertex ( i, 0 ), _owner->getEdgeVertex ( i, 1 ) );
    double el = ab.norm();
    if ( el > rl )
      rl = el;
  }
}

void LTetraRefineField::setFather ( LTetra* p )
{
  _father = p;
  LTetraRefineField *ptf = p->getRefineField();
  setGen ( ptf->getGen() +1 );
  setRefDepth ( ptf->getRefDepth()-1 );
}

void LTetraRefineField::resetFather()
{
  _father = NULL;
  setGen ( 0 );
  setRefDepth ( 0 );
}

void LTetraRefineField::split_edge ( int edge )
{
  assert ( edge>= 0 && edge <6 );
  bit_edge |= ( 1U << edge );
}

int LTetraRefineField::edge_state ( int edge )
{
  assert ( edge>= 0 && edge <6 );
  return ( bit_edge & ( 1U << edge ) ) != 0;
}

int LTetraRefineField::edge_split_count()
{
  int count = 0;
  for ( int i = 0; i < 6; i++ )
    count += edge_state ( i );
  return count;
}

void LTetra::split_edge ( int edge, LVertex *newv, LTetra **t1, LTetra **t2 )
{
  int nt[6][2][3] =
  {
    {{0, 2, 3}, {1, 3, 2}},
    {{0, 3, 1}, {2, 1, 3}},
    {{0, 1, 2}, {3, 2, 1}},
    {{1, 0, 3}, {2, 3, 0}},
    {{1, 2, 0}, {3, 0, 2}},
    {{2, 0, 1}, {3, 1, 0}}
  };
  *t1 = new LTetra ( getVertex ( nt[edge][0][0] ), getVertex ( nt[edge][0][1] ), getVertex ( nt[edge][0][2] ), newv, this );
  *t2 = new LTetra ( getVertex ( nt[edge][1][0] ), getVertex ( nt[edge][1][1] ), getVertex ( nt[edge][1][2] ), newv, this );
  ( *t1 )->setMutualAdj ( 0, *t2, 0 );
  if ( getAdj ( getEdgeVertexIndex ( edge, 1 ) ) != NULL )
    ( *t1 )->setMutualAdj ( 3, getAdj ( getEdgeVertexIndex ( edge, 1 ) ), getOpp ( getEdgeVertexIndex ( edge, 1 ) ) );
  if ( getAdj ( getEdgeVertexIndex ( edge, 0 ) ) != NULL )
    ( *t2 )->setMutualAdj ( 3, getAdj ( getEdgeVertexIndex ( edge, 0 ) ), getOpp ( getEdgeVertexIndex ( edge, 0 ) ) );
}

void LTetra::split_face ( int face, LVertex *newv, LTetra **t1, LTetra **t2, LTetra **t3 )
{
  int nt[4][3][3] =
  {
    {{0, 1, 2}, {0, 2, 3}, {0, 3, 1}},
    {{1, 0, 3}, {1, 3, 2}, {1, 2, 0}},
    {{2, 0, 1}, {2, 1, 3}, {2, 3, 0}},
    {{3, 0, 2}, {3, 2, 1}, {3, 1, 0}}
  };
  int adj[4][3] =
  {
    {3, 1, 2},
    {2, 0, 3},
    {3, 0, 1},
    {1, 0, 2}
  };
  *t1 = new LTetra ( getVertex ( nt[face][0][0] ), getVertex ( nt[face][0][1] ), getVertex ( nt[face][0][2] ), newv, this );
//     printf("tetra 1 --> %d %d %d %p\n", (*t1)->getVertex(0)->getIndex(), (*t1)->getVertex(1)->getIndex(), (*t1)->getVertex(2)->getIndex(), (*t1)->getVertex(3));
  *t2 = new LTetra ( getVertex ( nt[face][1][0] ), getVertex ( nt[face][1][1] ), getVertex ( nt[face][1][2] ), newv, this );
//     printf("tetra 2 --> %d %d %d %p\n", (*t2)->getVertex(0)->getIndex(), (*t2)->getVertex(1)->getIndex(), (*t2)->getVertex(2)->getIndex(), (*t2)->getVertex(3));
  *t3 = new LTetra ( getVertex ( nt[face][2][0] ), getVertex ( nt[face][2][1] ), getVertex ( nt[face][2][2] ), newv, this );
//     printf("tetra 3 --> %d %d %d %p\n", (*t3)->getVertex(0)->getIndex(), (*t3)->getVertex(1)->getIndex(), (*t3)->getVertex(2)->getIndex(), (*t3)->getVertex(3));
  ( *t1 )->setMutualAdj ( 1, *t2, 2 );
  ( *t2 )->setMutualAdj ( 1, *t3, 2 );
  ( *t3 )->setMutualAdj ( 1, *t1, 2 );

  if ( getAdj ( adj[face][0] ) != NULL )
    ( *t1 )->setMutualAdj ( 3, getAdj ( adj[face][0] ), getOpp ( adj[face][0] ) );
  if ( getAdj ( adj[face][1] ) != NULL )
    ( *t2 )->setMutualAdj ( 3, getAdj ( adj[face][1] ), getOpp ( adj[face][1] ) );
  if ( getAdj ( adj[face][2] ) != NULL )
    ( *t3 )->setMutualAdj ( 3, getAdj ( adj[face][2] ), getOpp ( adj[face][2] ) );
}

void LTetra::split ( LVertex *newv, LTetra **t1, LTetra **t2, LTetra **t3, LTetra **t4 )
{
//     printf("split tetra %d --> v %d %d %d %d --> adj %d %d %d %d\n", getIndex(), getVertex(0)->getIndex(), getVertex(1)->getIndex(), getVertex(2)->getIndex(), getVertex(3)->getIndex(),
//     getAdj(0)?getAdj(0)->getIndex():-1, getAdj(1)?getAdj(1)->getIndex():-1, getAdj(2)?getAdj(2)->getIndex():-1, getAdj(3)?getAdj(3)->getIndex():-1);
  *t1 = new LTetra ( getFaceVertex ( 0, 0 ), getFaceVertex ( 0, 1 ), getFaceVertex ( 0, 2 ), newv, this );
  *t2 = new LTetra ( getFaceVertex ( 1, 0 ), getFaceVertex ( 1, 1 ), getFaceVertex ( 1, 2 ), newv, this );
  *t3 = new LTetra ( getFaceVertex ( 2, 0 ), getFaceVertex ( 2, 1 ), getFaceVertex ( 2, 2 ), newv, this );
  *t4 = new LTetra ( getFaceVertex ( 3, 0 ), getFaceVertex ( 3, 1 ), getFaceVertex ( 3, 2 ), newv, this );
  ( *t1 )->setMutualAdj ( 0, *t2, 0 );
  ( *t1 )->setMutualAdj ( 1, *t4, 0 );
  ( *t1 )->setMutualAdj ( 2, *t3, 0 );
  ( *t2 )->setMutualAdj ( 1, *t3, 2 );
  ( *t2 )->setMutualAdj ( 2, *t4, 1 );
  ( *t3 )->setMutualAdj ( 1, *t4, 2 );
  if ( getAdj ( 0 ) != NULL )
    ( *t1 )->setMutualAdj ( 3, getAdj ( 0 ), getOpp ( 0 ) );
//     printf("tetra %p --> adj %d %d %d %d\n", *t1, (*t1)->getAdj(0)?(*t1)->getAdj(0)->getIndex():-1, (*t1)->getAdj(1)?(*t1)->getAdj(1)->getIndex():-1, (*t1)->getAdj(2)?(*t1)->getAdj(2)->getIndex():-1, (*t1)->getAdj(3)?(*t1)->getAdj(3)->getIndex():-1);
  if ( getAdj ( 1 ) != NULL )
    ( *t2 )->setMutualAdj ( 3, getAdj ( 1 ), getOpp ( 1 ) );
//     printf("tetra %p --> adj %d %d %d %d\n", *t2, (*t2)->getAdj(0)?(*t2)->getAdj(0)->getIndex():-1, (*t2)->getAdj(1)?(*t2)->getAdj(1)->getIndex():-1, (*t2)->getAdj(2)?(*t2)->getAdj(2)->getIndex():-1, (*t2)->getAdj(3)?(*t2)->getAdj(3)->getIndex():-1);
  if ( getAdj ( 2 ) != NULL )
    ( *t3 )->setMutualAdj ( 3, getAdj ( 2 ), getOpp ( 2 ) );
//     printf("tetra %p --> adj %d %d %d %d\n", *t3, (*t3)->getAdj(0)?(*t3)->getAdj(0)->getIndex():-1, (*t3)->getAdj(1)?(*t3)->getAdj(1)->getIndex():-1, (*t3)->getAdj(2)?(*t3)->getAdj(2)->getIndex():-1, (*t3)->getAdj(3)?(*t3)->getAdj(3)->getIndex():-1);
  if ( getAdj ( 3 ) != NULL )
    ( *t4 )->setMutualAdj ( 3, getAdj ( 3 ), getOpp ( 3 ) );
//     printf("tetra %p --> adj %d %d %d %d\n", *t4, (*t4)->getAdj(0)?(*t4)->getAdj(0)->getIndex():-1, (*t4)->getAdj(1)?(*t4)->getAdj(1)->getIndex():-1, (*t4)->getAdj(2)?(*t4)->getAdj(2)->getIndex():-1, (*t4)->getAdj(3)?(*t4)->getAdj(3)->getIndex():-1);

}