// 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 "mesh.h"
#include "LVertex.h"
#include "discreteSurface.h"

void Shell::build()
{
  int count = 0;
  LTetra *tn = _t[0];
  int opp = tn->getOppEdgeVertexIndex ( _e[0], 0 ); // a surveiller
  int edge = tn->getEdgeIndex ( _v[0], _v[1] );
  LVertex *co = tn->getOppEdgeVertex ( _e[0], 1 );
  while ( tn->getAdj ( opp ) != _t[0] )
  {
    tn->getAdjOpp ( opp, &tn, &opp );
    if ( tn == NULL )
    {
      _o.push_back ( co );
      int ipos = _t.size();
      tn = _t[0];
      opp = tn->getOppEdgeVertexIndex ( _e[0], 1 );
      co = tn->getOppEdgeVertex ( _e[0], 0 );
      while ( tn->getAdj ( opp ) != _t[0] )
      {
        tn->getAdjOpp ( opp, &tn, &opp );
        if ( tn == NULL )
        {
          _o.push_back ( co );
          std::reverse ( _t.begin() +ipos, _t.end() );
          std::reverse ( _e.begin() +ipos, _e.end() );
          std::reverse ( _o.begin() +ipos, _o.end() );
          return;
        }
        edge = tn->getEdgeIndex ( _v[0], _v[1] );
        co = tn->getVertex ( opp );
        opp = tn->getOppEdgeOppVertexIndex ( edge, opp );
        assert ( tn->getIndex() != -1 );
        _t.push_back ( tn );
        _e.push_back ( edge );
        _o.push_back ( tn->getVertex ( opp ) );
        assert ( count++ < 500 );
      }
    }
    edge = tn->getEdgeIndex ( _v[0], _v[1] );
    co = tn->getVertex ( opp );
    opp = tn->getOppEdgeOppVertexIndex ( edge, opp );
    assert ( tn->getIndex() != -1 );
    _t.push_back ( tn );
    _e.push_back ( edge );
    _o.push_back ( tn->getVertex ( opp ) );
    assert ( count++ < 500 );
  }
}


void Shell::set_edge ( LTetra *t, int edge )
{
  assert ( !locked() );
  assert ( t != NULL );
  assert ( t->getIndex() != -1 );
  _t.clear();
  _e.clear();
  _o.clear();
  _v[0] = t->getEdgeVertex ( edge, 0 );
  _v[1] = t->getEdgeVertex ( edge, 1 );
  _t.push_back ( t );
  _e.push_back ( edge );
}

void Shell::get_tetra ( int i, LTetra **t, int *edge )
{
  assert ( i<_t.size() );
  *t = _t[i];
  *edge = _e[i];
}

LVertex *Shell::get_vertex ( int i )
{
  assert ( i<_o.size() );
  return _o[i];
}

void Shell::dump()
{
  printf ( "## Current shell ##\n" );
  for ( int i = 0; i < _t.size(); i++ )
    printf ( "t: %p (%d) -- e: %d\n", _t[i], _t[i]->getIndex(), _e[i] );
}

void Shell::dump_vtk()
{
  char filename[100];
  std::map<LVertex *, int> map;
  std::map<LVertex *, int>::iterator it;
  int count = 0;

  sprintf ( filename, "shell_%d_%d.vtk", _v[0]->getIndex(), _v[1]->getIndex() );
  FILE *fp = fopen ( filename, "w" );
  fprintf ( fp, "# vtk DataFile Version 2.0\n" );
  fprintf ( fp, "Really cool data\n" );
  fprintf ( fp, "ASCII\n" );
  fprintf ( fp, "DATASET UNSTRUCTURED_GRID\n" );

  for ( int i = 0; i < size(); i++ )
  {
    LTetra *tb;
    int eb;
    get_tetra ( i, &tb, &eb );
    for ( int j = 0; j < 4; j++ )
    {
      LVertex *vb = tb->getVertex ( j );
      it = map.find ( vb );
      if ( it == map.end() )
        map.insert ( std::pair<LVertex*, int> ( vb, count++ ) );
    }
  }

  fprintf ( fp, "POINTS %d float\n", ( int ) map.size() );
  for ( it=map.begin() ; it != map.end(); it++ )
    fprintf ( fp, "%lf %lf %lf\n", ( *it ).first->x(), ( *it ).first->y(), ( *it ).first->z() );

  fprintf ( fp, "CELLS %d %d\n", size(), size() *5 );
  for ( int i = 0; i < size(); i++ )
  {
    LTetra *tb;
    int eb;
    get_tetra ( i, &tb, &eb );
    fprintf ( fp, "%d", 4 );
    for ( int j = 0; j < 4; j++ )
    {
      LVertex *vb = tb->getVertex ( j );
      it = map.find ( vb );
      fprintf ( fp, " %d", ( *it ).second );
    }
    fprintf ( fp, "\n" );
  }

  fprintf ( fp, "CELL_TYPES %d\n", size() );
  for ( int i = 0; i < size(); i++ )
    fprintf ( fp, "%d\n", 10 );

  fclose ( fp );
}

void Ball::set_vertex ( LVertex *v )
{
//     assert(!locked());
  assert ( v != NULL );
  assert ( v->getIndex() != -1 );
  assert ( v->getTetra() != NULL );
  assert ( v->getTetra()->getIndex() != -1 );
  lock();
  _t.clear();
  _o.clear();
  _v = v;
  _t.push_back ( _v->getTetra() );
  _t[0]->setFlag ( _v->getIndex() );
  for ( int i = 0; i < 4; i++ )
    if ( _t[0]->getVertex ( i ) == _v )
    {
      _o.push_back ( i );
      return;
    }
  assert ( 0 );
}

void Ball::build()
{
  int seek = 0;
  while ( seek < _t.size() )
  {
    LTetra *t = _t[seek++];
    assert ( t->getIndex() != -1 );
    for ( int i = 0; i < 4; i++ )
      if ( t->getVertex ( i ) != _v )
      {
        LTetra *adj = t->getAdj ( i );
        if ( adj != NULL && adj->getFlag() != _v->getIndex() )
        {
          assert ( adj->getIndex() != -1 );
          adj->setFlag ( _v->getIndex() );
          _t.push_back ( adj );
          for ( int j = 0; j < 4; j++ )
            if ( adj->getVertex ( j ) == _v )
            {
              _o.push_back ( j );
              break;
            }
          assert ( _t[_t.size()-1]->getVertex ( _o[_o.size()-1] ) == _v );
        }
      }
  }
  for ( int i = 0; i < _t.size(); i++ )
    _t[i]->resetFlag();
//     printf("Ball size --> %d\n", size());
}

void Ball::get_tetra ( int i, LTetra **t, int *opp )
{
  assert ( i<_t.size() );
  *t = _t[i];
  *opp = _o[i];
}

void Ball::dump()
{
  printf ( "## Current ball ##\n" );
  for ( int i = 0; i < _t.size(); i++ )
    printf ( "t: %p (%d) -- opp: %d\n", _t[i], _t[i]->getIndex(), _o[i] );
}

void Ball::dump_vtk()
{
  char filename[100];
  std::map<LVertex *, int> map;
  std::map<LVertex *, int>::iterator it;
  int count = 0;

  sprintf ( filename, "ball_%d.vtk", _v->getIndex() );
  FILE *fp = fopen ( filename, "w" );
  fprintf ( fp, "# vtk DataFile Version 2.0\n" );
  fprintf ( fp, "Really cool data\n" );
  fprintf ( fp, "ASCII\n" );
  fprintf ( fp, "DATASET UNSTRUCTURED_GRID\n" );

  for ( int i = 0; i < size(); i++ )
  {
    LTetra *tb;
    int eb;
    get_tetra ( i, &tb, &eb );
    for ( int j = 0; j < 4; j++ )
    {
      LVertex *vb = tb->getVertex ( j );
      it = map.find ( vb );
      if ( it == map.end() )
        map.insert ( std::pair<LVertex*, int> ( vb, count++ ) );
    }
  }

  fprintf ( fp, "POINTS %d float\n", ( int ) map.size() );
  for ( it=map.begin() ; it != map.end(); it++ )
    fprintf ( fp, "%lf %lf %lf\n", ( *it ).first->x(), ( *it ).first->y(), ( *it ).first->z() );

  fprintf ( fp, "CELLS %d %d\n", size(), size() *5 );
  for ( int i = 0; i < size(); i++ )
  {
    LTetra *tb;
    int eb;
    get_tetra ( i, &tb, &eb );
    fprintf ( fp, "%d", 4 );
    for ( int j = 0; j < 4; j++ )
    {
      LVertex *vb = tb->getVertex ( j );
      it = map.find ( vb );
      fprintf ( fp, " %d", ( *it ).second );
    }
    fprintf ( fp, "\n" );
  }

  fprintf ( fp, "CELL_TYPES %d\n", size() );
  for ( int i = 0; i < size(); i++ )
    fprintf ( fp, "%d\n", 10 );

  fclose ( fp );
}

Mesh::Mesh ( options *opt )
{
  global_tetra_index = 0;
  global_vertex_index = 0;
  _hfs = new hashFaceStruct;
  _hes = new hashEdgeStruct;
  _em = new EntityManager;
  _sot = new Shell;
  _bot = new Ball;
  _opts = opt;
  _le = 0.0;
  _start_index = 0;
}

Mesh::~Mesh()
{
  delete _hfs;
  delete _hes;
  delete _em;
  delete _sot;
  delete _bot;
}

void Mesh::set_tetra ( int i, LTetra *t )
{
  if ( i >= tetra_size() )
    _t.resize ( i+1 );
  t->setIndex ( i );
  _t[i] = t;
}

void Mesh::add_tetra ( LTetra *t )
{
  if ( !free_t.empty() )
  {
    int index = *free_t.begin();
    _t[index] = t;
    t->setIndex ( index );
    free_t.erase ( free_t.begin() );
  }
  else
  {
    t->setIndex ( _t.size() );
    _t.push_back ( t );
  }
//     printf("add tetra --> %d\n", t->getIndex());
  assert ( t->getIndex() != -1 );
  assert ( t->getIndex() < tetra_size() );
}

void Mesh::add_tetra ( LTetra *t, int id )
{
  if ( !free_t.empty() )
  {
    int index = *free_t.begin();
    _t[index] = t;
    t->setIndex ( index );
    free_t.erase ( free_t.begin() );
    _lookup_tetra_id[id] = index;
  }
  else
  {
    t->setIndex ( _t.size() );
    _lookup_tetra_id[id] = _t.size();
    _t.push_back ( t );
  }

//     printf("add tetra --> %d\n", t->getIndex());
  assert ( t->getIndex() != -1 );
  assert ( t->getIndex() < tetra_size() );
}

void Mesh::set_vertex ( int i, LVertex *v )
{
  if ( i >= vertex_size() )
    _v.resize ( i+1 );
  v->setIndex ( i );
  _v[i] = v;
}

void Mesh::add_vertex ( LVertex *v )
{
  if ( !free_v.empty() )
  {
    int index = *free_v.begin();
    _v[index] = v;
    v->setIndex ( index );
    free_v.erase ( free_v.begin() );
  }
  else
  {
    v->setIndex ( _v.size() );
    _v.push_back ( v );
  }
//     printf("add vertex --> %d\n", v->getIndex());
  assert ( v->getIndex() != -1 );
  assert ( v->getIndex() < vertex_size() );
}

void Mesh::add_vertex ( LVertex *v, int id )
{
  if ( !free_v.empty() )
  {
    int index = *free_v.begin();
    _v[index] = v;
    v->setIndex ( index );
    _lookup_vertex_id[id] = index;
    free_v.erase ( free_v.begin() );
  }
  else
  {
    v->setIndex ( _v.size() );
    _lookup_vertex_id[id] = _v.size();
    _v.push_back ( v );
  }
//     printf("add vertex --> %d\n", v->getIndex());
  assert ( v->getIndex() != -1 );
  assert ( v->getIndex() < vertex_size() );
}

void Mesh::remove_tetra ( LTetra *t )
{
//     printf("remove tetra --> %d (%p)\n", t->getIndex(), t);
  assert ( t->getIndex() != -1 );
  int index = t->getIndex();
  _t[index] = NULL;
  free_t.insert ( index );
  t->setIndex ( -1 );
}

void Mesh::remove_vertex ( LVertex *v )
{
//     printf("remove vertex --> %d (%p)\n", v->getIndex(), v);
  assert ( v->getIndex() != -1 );
  int index = v->getIndex();
  _v[index] = NULL;
  free_v.insert ( index );
  v->setIndex ( -1 );
}

void Mesh::shrink_tetra()
{
//     printf("Shrinking LTetra ...\n");
  while ( !free_t.empty() )
  {
    int index = *free_t.begin();
//         printf("process tetra index --> %d\n", index);
    free_t.erase ( free_t.begin() );
    std::vector<LTetra*>::iterator end = _t.end();
    while ( * ( --end ) == NULL )
      _t.erase ( end );
    if ( index < tetra_size() )
    {
      _t[index] = _t.back();
      _t[index]->setIndex ( index );
      _t.erase ( end );
    }
  }
}

void Mesh::shrink_vertex()
{
//     printf("Shrinking LVertex ...\n");
  while ( !free_v.empty() )
  {
    int index = *free_v.begin();
//         printf("process vertex index --> %d\n", index);
    free_v.erase ( free_v.begin() );
    std::vector<LVertex*>::iterator end = _v.end();
    while ( * ( --end ) == NULL )
      _v.erase ( end );
    if ( index < vertex_size() )
    {
      _v[index] = _v.back();
      _v[index]->setIndex ( index );
      _v.erase ( end );
    }
  }
}

void Mesh::readMeshFile ( const char *name )
{
  int error = 0;
  char filename[100];
  sprintf ( filename, "%s.mesh", name );
  tprintf ( "Reading mesh file %s ....\n", filename );
  FILE *fp = fopen ( filename, "r" );
  int nb_vx;
  if ( fscanf ( fp, "%d", &nb_vx ) != 1 )
    error++;
  tprintf ( "--> %d vertices\n", nb_vx );
  float x, y, z;
  for ( int i = 0; i < nb_vx; i++ )
  {
    if ( fscanf ( fp, "%f %f %f", &x, &y, &z ) != 3 )
      error++;
    LVertex *v = new_vertex ( x,y,z );
    add_vertex ( v, i );
  }
  int nb_tet;
  if ( fscanf ( fp, "%d", &nb_tet ) != 1 )
    error++;
  tprintf ( "--> %d tetrahedra\n", nb_tet );
  int v0, v1, v2, v3;
  for ( int i = 0; i < nb_tet; i++ )
  {
    if ( fscanf ( fp, "%d %d %d %d", &v0, &v1, &v2, &v3 ) != 4 )
      error++;
    LTetra *t = new_tetra ( get_vertex ( v0 ), get_vertex ( v1 ), get_vertex ( v2 ), get_vertex ( v3 ), NULL );
    add_tetra ( t, i );
    for ( int j = 0; j < 4; j++ )
      _hfs->addAdjFace ( t, j );
  }
  fclose ( fp );

  assert ( !error );

  _hfs->clear();

#if 0
  //initialisation du bucket
  LVertex min, max;
  min.setPosition ( INFINITY, INFINITY, INFINITY );
  max.setPosition ( -INFINITY, -INFINITY, -INFINITY );
  for ( int i = 0; i < vertex_size(); i++ )
  {
    LVertex *v = get_vertex ( i );
    if ( v->x() < min.x() )
      min.setPosition ( v->x(), min.y(), min.z() );
    if ( v->y() < min.y() )
      min.setPosition ( min.x(), v->y(), min.z() );
    if ( v->z() < min.z() )
      min.setPosition ( min.x(), min.y(), v->z() );
    if ( v->x() > max.x() )
      max.setPosition ( v->x(), max.y(), max.z() );
    if ( v->y() > max.y() )
      max.setPosition ( max.x(), v->y(), max.z() );
    if ( v->z() > max.z() )
      max.setPosition ( max.x(), max.y(), v->z() );
  }
  _bs = new Bucket ( min, max, 0.1, 50 );
  for ( int i = 0; i < vertex_size(); i++ )
    _bs->add_point ( get_vertex ( i ) );
#endif

#if 0
  check_adjacence();
#endif

}

void Mesh::readLevelsetType0 ( FILE *fp )
{
  int error = 0;
  int idsurf;
  int local_id;
  if ( fscanf ( fp, "%d", &idsurf ) != 1 )
    error++;
  if ( idsurf < 0 )
  {
    int s1, s2;
    if ( fscanf ( fp, "%d %d", &s1, &s2 ) != 2 )
      error++;
    local_id = get_entity_manager()->add_cut_face_id ( idsurf, s1, s2 );
    get_entity_manager()->set_virtual_face ( local_id );
  }
  else
  {
    local_id = get_entity_manager()->add_face_id ( idsurf );
    get_entity_manager()->set_real_face ( local_id );
  }
  int nbelem;
  if ( fscanf ( fp, "%d", &nbelem ) != 1 )
    error++;
  for ( int j = 0; j<nbelem; j++ )
  {
    int elem;
    if ( fscanf ( fp, "%d", &elem ) != 1 )
      error++;
    int vx[4];
    double dist[4];
    for ( int k = 0; k<4; k++ )
    {
      if ( fscanf ( fp, "%d %lf", &vx[k], &dist[k] ) != 2 )
        error++;
      dist[k] *= -1.0; // WARNING --> hack
    }

    LTetra *t = get_tetra_from_lookup ( elem );

    if ( t != NULL )
    {
      for ( int k = 0; k<4; k++ )
        assert ( t->getVertex ( k ) == get_vertex_from_lookup ( vx[k] ) );
      addLevelset ( local_id, t, dist );
    }

  }
  writeSupportVTK ( local_id );
}

void Mesh::readLevelsetType1 ( FILE *fp )
{
  int error = 0;
  int idsurf;
  int local_id;
  if ( fscanf ( fp, "%d", &idsurf ) != 1 )
    error++;
  if ( idsurf < 0 )
  {
    int s1, s2;
    if ( fscanf ( fp, "%d %d", &s1, &s2 ) != 2 )
      error++;
    local_id = get_entity_manager()->add_cut_face_id ( idsurf, s1, s2 );
    get_entity_manager()->set_virtual_face ( local_id );
  }
  else
  {
    local_id = get_entity_manager()->add_face_id ( idsurf );
    get_entity_manager()->set_real_face ( local_id );
  }
  int nbvx;
  if ( fscanf ( fp, "%d", &nbvx ) != 1 )
    error++;
  for ( int j = 0; j<nbvx; j++ )
  {
    int vx;
    double dist;
    if ( fscanf ( fp, "%d", &vx ) != 1 )
      error++;
    LVertex *v = get_vertex ( vx );
    if ( fscanf ( fp, "%lf", &dist ) !=1 )
      error++;
    dist *= -1.0; // WARNING --> hack
    addLevelset ( local_id, v, dist );
//      if (!v->checkSurf(local_id))
//      {
//          v->addSurf(local_id, dist);
//          if (fabs(dist) < TOL)
//              v->addSurfZero(local_id);
//      }
  }
  writeSupportVTK ( local_id );
}

void Mesh::addLevelset ( int ls, LTetra *t, double dist[4] )
{
  if ( t->child_size() )
  {
    printf ( "has child --> return\n" );
    return;
  }

  for ( int i = 0; i < 4; i++ )
  {
    addLevelset ( ls, t->getVertex ( i ), dist[i] );
//      LVertex *v = get_tetra(elem)->getVertex(i);
//      if (!v->checkSurf(ls))
//      {
//          v->addSurf(ls, dist[i]);
//          if (fabs(dist[i]) < TOL)
//              v->addSurfZero(ls);
//      }
  }
}

void Mesh::addLevelset ( int ls, LVertex *v, double dist )
{
  if ( !v->checkSurf ( ls ) )
  {
    v->addSurf ( ls, dist );
    if ( fabs ( dist ) < TOL )
      v->addSurfZero ( ls );
  }
}

void Mesh::readEdges ( FILE *fp )
{
  if (_kdtree==NULL) bucketize();

  int error = 0;
  ////////////////////////////////////////
  // edge insertion --> en stand by !!  //
  ////////////////////////////////////////
  // sommets topologiques
  std::vector<LVertex *> tvx;
  int nbvertex;
  if ( fscanf ( fp, "%d", &nbvertex ) != 1 )
    error++;
  tvx.resize ( nbvertex );
  tprintf ( "--> %d vertices ...\n", nbvertex );
  for ( int i = 0; i<nbvertex; i++ )
  {
    int idvx;
    double x, y, z;
    if ( fscanf ( fp, "%d %lf %lf %lf", &idvx, &x, &y, &z ) != 4 )
      error++;
    LVertex *vx = new_vertex ( x, y, z );
    tvx[idvx] = vx;
  }

  // aretes topologiques
  std::vector<std::vector<int> > tedge;
  std::vector<std::vector<int> > tedge_surf;
  int nbedge;
  if ( fscanf ( fp, "%d", &nbedge ) != 1 )
    error++;
  tprintf ( "--> %d edges ...\n", nbedge );
  tedge.resize ( nbedge );
  tedge_surf.resize ( nbedge );
  for ( int i = 0; i<nbedge; i++ )
  {
    int idedge, nbv, idv;
    if ( fscanf ( fp, "%d %d", &idedge, &nbv ) != 2 )
      error++;

//     local_id = get_entity_manager()->add_edge_id ( idedge );
//     get_entity_manager()->set_real_edge ( local_id );
    
    for ( int j = 0; j<nbv; j++ )
    {
      if ( fscanf ( fp, "%d", &idv ) != 1 )
        error++;
      tedge[i].push_back ( idv );
    }
//     int nbsurf, surf;
//     if ( fscanf ( fp, "%d", &nbsurf ) != 1 )
//       error++;
//     for ( int j = 0; j<nbsurf; j++ )
//     {
//       if ( fscanf ( fp, "%d", &surf ) != 1 )
//         error++;
//       tedge_surf[i].push_back ( surf );
//       for ( int k = 0; k < tedge[i].size(); k++ )
//       {
//         LVertex *vx = tvx[tedge[i][k]];
//         if ( !vx->checkSurf ( surf ) )
//         {
//           vx->addSurf ( surf, 0.0 );
//           vx->addSurfZero ( surf );
//         }
//       }
//     }
    int idface=idedge, local_id_surf;
    assert(idface-idface%(1000000)==1000000); // check the implicite edge id for identification in mesh_cutter->build_GModel_New
    local_id_surf = get_entity_manager()->add_face_id ( idface );
    get_entity_manager()->set_real_face ( local_id_surf );

    tedge_surf[i].push_back ( local_id_surf );
    for ( int k = 0; k < tedge[i].size(); k++ )
    {
      LVertex *vx = tvx[tedge[i][k]];
      if ( !vx->checkSurf ( local_id_surf ) )
      {
        vx->addSurf ( local_id_surf, 0.0 );
        vx->addSurfZero ( local_id_surf );
      }
    }
  }
//   fclose ( fp );

#if 0
  //////////////////////////////////////////////////////////////////
  if ( tvx.size() != 0 )
  {
    tprintf ( "Writing %s ....\n", "debug_forced_edges.vtk" );
    fp = fopen ( "debug_forced_edges.vtk", "w" );
    fprintf ( fp, "# vtk DataFile Version 2.0\n" );
    fprintf ( fp, "Really cool data\n" );
    fprintf ( fp, "ASCII\n" );
    fprintf ( fp, "DATASET UNSTRUCTURED_GRID\n" );

    fprintf ( fp, "POINTS %d float\n", ( int ) tvx.size() );
    for ( int i = 0; i < tvx.size(); i++ )
    {
      fprintf ( fp, "%lf %lf %lf\n", tvx[i]->x(), tvx[i]->y(), tvx[i]->z() );
//  tvx[i]->dumpSurfZero();
    }
    int ee = 0;
    for ( int i = 0; i < tedge.size(); i++ )
      ee += tedge[i].size()-1;
    fprintf ( fp, "CELLS %d %d\n", ee, ee*3 );
    for ( int i = 0; i < tedge.size(); i++ )
      for ( int j = 0; j < tedge[i].size()-1; j++ )
        fprintf ( fp, "%d %d %d\n", 2, tedge[i][j], tedge[i][j+1] );
    fprintf ( fp, "CELL_TYPES %d\n", ee );
    for ( int i = 0; i < ee; i++ )
      fprintf ( fp, "%d\n", 3 );
    fprintf ( fp, "CELL_DATA %d\n", ee );
    fprintf ( fp, "SCALARS edge_color int 1\n" );
    fprintf ( fp, "LOOKUP_TABLE default\n" );
    for ( int i = 0; i < ee; i++ )
      fprintf ( fp, "%d\n", i );
    fclose ( fp );
  }
  //////////////////////////////////////////////////////////////////
#endif

  // forcage des entitees topologiques
  tprintf ( "Edges insertion START\n" );
  for ( int i = 0; i < tedge.size(); i++ )
  {
//         printf("Inserting edge %d --> %d segments\n", i, (int)tedge[i].size()-1);
    double b[4];
    LVertex *v0, *v1;
    v0 = tvx[tedge[i][0]];
    LTetra *t = find_tetra_containing ( v0 );
    assert ( t != NULL );
    for ( int j = 0; j < tedge[i].size()-1; j++ )
    {
//      printf("segment %d...\n", j);
      v0 = tvx[tedge[i][j]];
      v1 = tvx[tedge[i][j+1]];
      t = insert_topo_edge ( v0, v1, t, tedge_surf[i] );
    }
    t->barycentric ( v1, b );
    LVertex *inserted = insert_topo_vertex ( v1, t, b, tedge_surf[i] );
//  char temp_name[100];
//  sprintf(temp_name, "temp_mesh_%d", i);
//  writeMesh(temp_name);
  }
//     check_adjacence();
  _hfs->clear();
  _hes->clear();
  tprintf ( "Edges insertion END\n" );
}

void Mesh::readLevelsetFile ( const char *name )
{
  // variables debut
  double dotmin_merge_surface = 0.99;
  double distmin_merge_surface = 1e-1;
  if ( _opts->mesh_dotmin_merge_surface >= 0.0 )
    dotmin_merge_surface = _opts->mesh_dotmin_merge_surface;
  if ( _opts->mesh_distmin_merge_surface >= 0.0 )
    distmin_merge_surface = _opts->mesh_distmin_merge_surface;
  // variables fin

  int error = 0;
  char filename[100];
  sprintf ( filename, "%s.ls", name );
  tprintf ( "Reading levelsets file %s ....\n", filename );
  FILE *fp = fopen ( filename, "r" );
  int version;
  if ( fscanf ( fp, "%d", &version ) != 1 )
    error++;

  // lecture de la premiere partie du fichier --> les valeurs des levelset surfaciques
  if ( version == 0 )
  {
    int nbsurf = 0;
    if ( fscanf ( fp, "%d", &nbsurf ) != 1 )
      error++;
    tprintf ( "--> %d surfaces ...\n", nbsurf );
    for ( int i = 0; i<nbsurf; i++ )
      readLevelsetType0 ( fp );
  }
  if ( version == 1 )
  {
    int nbsurf = 0;
    if ( fscanf ( fp, "%d", &nbsurf ) != 1 )
      error++;
    tprintf ( "--> %d surfaces ...\n", nbsurf );
    for ( int i = 0; i<nbsurf; i++ )
      readLevelsetType1 ( fp );
  }

#if 1
  // on lit les surfaces orthogonales materialisant les aretes tangentes
  tprintf ( "Reading cutting levelsets ....\n" );

  if ( version == 0 )
  {
    int nbsurf = 0;
    if ( fscanf ( fp, "%d", &nbsurf ) != 1 )
      error++;
    tprintf ( "--> %d tangent edges ...\n", nbsurf );
    for ( int i = 0; i<nbsurf; i++ )
      readLevelsetType0 ( fp );

  }
  if ( version == 1 )
  {
    int nbsurf = 0;
    if ( fscanf ( fp, "%d", &nbsurf ) != 1 )
      error++;
    tprintf ( "--> %d tangent edges ...\n", nbsurf );
    for ( int i = 0; i<nbsurf; i++ )
      readLevelsetType1 ( fp );
  }

#endif

  // on merge alors les surfaces tangentes
  if ( _opts->brep_topo )
    surface_merging ( dotmin_merge_surface, distmin_merge_surface );

  if ( _opts->read_edges )
    readEdges ( fp );

  fclose ( fp );

  assert ( !error );

  collect_edges();

  writeMeshVTK ( "initial" );
}

void Mesh::collect_edges()
{
  _hes->set_stack_number ( get_entity_manager()->face_size() );

  // remplissage des aretes intersectees
  for ( int i = 0; i<tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    assert ( !t->isParent() );
    for ( int j = 0; j < 6; j++ )
    {
      int s = crossing_surf ( t, j );
      if ( s != -1 )
      {
        hashEdgeKey *k = _hes->addEdge ( t, j, s );
      }
    }
  }

  clear_all_tetra_child();
}

void Mesh::bucketize()
{
#if defined(HAVE_ANN)
  ANNpointArray nodes = annAllocPts ( vertex_size(), 4 );
  for ( int i = 0; i < vertex_size(); i++ )
  {
    LVertex *pt = get_vertex ( i );
    nodes[i][0] = pt->x();
    nodes[i][1] = pt->y();
    nodes[i][2] = pt->z();
    nodes[i][3] = ( double ) ( pt->getTetra()->getIndex() );
  }
  _kdtree = new ANNkd_tree ( nodes, vertex_size(), 3 );

#else
  assert ( 0 );
#endif
}

LTetra* Mesh::find_tetra_containing ( LVertex *pt )
{
#if defined(HAVE_ANN)
  ANNidxArray index = new ANNidx;
  ANNdistArray dist = new ANNdist;
  ANNpoint xyz = new ANNcoord[3];
  xyz[0] = pt->x();
  xyz[1] = pt->y();
  xyz[2] = pt->z();
  _kdtree->annkSearch ( xyz, 1, index, dist );
  ANNpointArray pts = _kdtree->thePoints();
  ANNpoint pcl = pts[index[0]];
  LVertex cl ( pcl[0], pcl[1], pcl[2] );
  int tetra_index = ( int ) pcl[3];
  LTetra *t = get_tetra ( tetra_index );

  srand ( 0 );
  while ( 1 )
  {
    if ( t->contain ( pt ) )
      return t;
    int dir = rand() %4;
    LTetra *adj = t->getAdj ( dir );
    int opp = t->getOpp ( dir );
    if ( adj != NULL )
      t = adj;
  }
#else
  assert ( 0 );
#endif
}

void Mesh::add_included ( DiscreteSurface *surf )
{
  tprintf ( "--> add vertices for surf %p\n", surf );

  progress_bar pbar ( surf->size() );
  pbar.start();
  for ( int i = 0; i < surf->size(); i++ )
  {
    pbar.progress ( i );
    LVertex *pt;
    LVertex *n;
    double curv;
    surf->get_point ( i, &pt, &n, &curv );

    if ( curv > 1e-5 )
    {
      LTetra *t = find_tetra_containing ( pt );
      pt->setCurvature ( curv );

//      if (pt->getCurvature())
//    printf("curv %lf\n", pt->getCurvature());

      t->getRefineField()->addInc ( pt, n );
    }
  }
  pbar.end();
}

void Mesh::process_edge()
{
  LTetra *ts;
  int es;
  _sot->get_tetra ( 0, &ts, &es );
  int s = crossing_surf ( ts, es );
  if ( s != -1 )
  {
    LVertex *newv = intersection ( ts, es, s );
    if ( newv != NULL )
    {
      add_vertex ( newv );
      split_edge ( newv, 1 );
    }
    else
    {
      s = crossing_surf ( ts, es );
      if ( s != -1 )
        _hes->addEdge ( ts, es, s );
    }
  }
  _sot->unlock();
}

int Mesh::cross ( LVertex *v0, LVertex *v1, int s )
{
  compute_common_surf ( v0, v1 );
  for ( int i = 0; i < common_surf_size(); i++ )
  {
    int cs = get_common_surf ( i );
    if ( cs == s && !v0->checkZero ( cs ) && !v1->checkZero ( cs ) )
    {
      double val[2];
      val[0] = v0->surfVal ( cs );
      val[1] = v1->surfVal ( cs );
      if ( val[0] * val[1] < 0.0 )
        return 1;
    }
  }
  return 0;    // return no surface
}

int Mesh::cross ( LTetra *t, int edge, int s )
{
  LVertex *v[2];
  v[0] = t->getEdgeVertex ( edge, 0 );
  v[1] = t->getEdgeVertex ( edge, 1 );
  return cross ( v[0], v[1], s );
}

int Mesh::crossing_surf ( LVertex *v0, LVertex *v1 )
{
  compute_common_surf ( v0, v1 );
  for ( int i = 0; i < common_surf_size(); i++ )
  {
    int cs = get_common_surf ( i );
//         printf("cs %d (%d %d)\n", cs, v0->checkZero(cs), v1->checkZero(cs));
    if ( !v0->checkZero ( cs ) && !v1->checkZero ( cs ) )
    {
      double val[2];
      val[0] = v0->surfVal ( cs );
      val[1] = v1->surfVal ( cs );
//      printf("%lf %lf)\n", val[0], val[1]);
      if ( val[0] * val[1] < 0.0 )
        return cs;
    }
  }
  return -1;    // return no surface
}

int Mesh::crossing_surf ( LTetra *t, int edge )
{
  LVertex *v[2];
  v[0] = t->getEdgeVertex ( edge, 0 );
  v[1] = t->getEdgeVertex ( edge, 1 );
  return crossing_surf ( v[0], v[1] );
}

void Mesh::compute_common_surf ( LVertex *v0, LVertex *v1 )
{
  _cs.clear();
  for ( int i = 0; i < v0->nbSurf(); i++ ) // should be efficient enough because of small size of array
    for ( int j = 0; j < v1->nbSurf(); j++ )
    {
//         printf("%d %d %d %d\n",i,v0->surfId(i),j,v1->surfId(j));
      if ( v0->surfId ( i ) == v1->surfId ( j ) )
      {
//                printf("idcommon=%d\n",v0->surfId(i));
        _cs.push_back ( v0->surfId ( i ) );
        break;
      }
    }
}

void Mesh::compute_common_zero_surf ( LVertex *v0, LVertex *v1 )
{
  _cs.clear();
  for ( int i = 0; i < v0->nbSurfZero(); i++ ) // should be efficient enough because of small size of array
    for ( int j = 0; j < v1->nbSurfZero(); j++ )
    {
      if ( v0->surfZeroId ( i ) == v1->surfZeroId ( j ) )
      {
        _cs.push_back ( v0->surfZeroId ( i ) );
        break;
      }
    }
}

void Mesh::compute_common_surf ( std::vector<LVertex*> &vx )
{
  _cs.clear();
  std::map<int, int> map;
  int index = 0;
  std::vector<int> cs;

  for ( int i = 0; i < vx.size(); i++ )
  {
    for ( int j = 0; j < vx[i]->nbSurf(); j++ )
    {
      int surf = vx[i]->surfId ( j );
      std::map<int, int >::iterator it = map.find ( surf );
      if ( it == map.end() )
      {
        index = cs.size();
        map.insert ( std::pair<int, int> ( surf, cs.size() ) );
        cs.push_back ( 0 );
      }
      else
      {
        index = it->second;
      }
      cs[index]++;
      if ( cs[index] == vx.size() )
      {
        _cs.push_back ( surf );
      }
    }
  }
}

void Mesh::compute_common_zero_surf ( std::vector<LVertex*> &vx )
{
  _cs.clear();
  std::map<int, int> map;
  int index = 0;
  std::vector<int> czs;

  for ( int i = 0; i < vx.size(); i++ )
  {
    for ( int j = 0; j < vx[i]->nbSurfZero(); j++ )
    {
      int surf = vx[i]->surfZeroId ( j );
      std::map<int, int >::iterator it = map.find ( surf );
      if ( it == map.end() )
      {
        index = czs.size();
        map.insert ( std::pair<int, int> ( surf, czs.size() ) );
        czs.push_back ( 0 );
      }
      else
      {
        index = it->second;
      }
      czs[index]++;
      if ( czs[index] == vx.size() )
      {
        _cs.push_back ( surf );
      }
    }
  }
}

void Mesh::compute_common_surf ( LTetra *t )
{
  std::vector<LVertex*> vx;
  vx.push_back ( t->getVertex ( 0 ) );
  vx.push_back ( t->getVertex ( 1 ) );
  vx.push_back ( t->getVertex ( 2 ) );
  vx.push_back ( t->getVertex ( 3 ) );
  compute_common_surf ( vx );
}

void Mesh::compute_common_zero_surf ( LTetra *t )
{
  std::vector<LVertex*> vx;
  vx.push_back ( t->getVertex ( 0 ) );
  vx.push_back ( t->getVertex ( 1 ) );
  vx.push_back ( t->getVertex ( 2 ) );
  vx.push_back ( t->getVertex ( 3 ) );
  compute_common_zero_surf ( vx );
}

int Mesh::same_surf_zero ( LVertex *v0, LVertex *v1 )
{
  LVertex *v[2];
  if ( v0->nbSurfZero() <= v1->nbSurfZero() )
  {
    v[0] = v0;
    v[1] = v1;
  }
  else
  {
    v[0] = v1;
    v[1] = v0;
  }

  for ( int i = 0; i < v0->nbSurfZero(); i++ )
  {
    int ok = 0;
    for ( int j = 0; j < v1->nbSurfZero(); j++ )
    {
      if ( v0->surfZeroId ( i ) == v1->surfZeroId ( j ) )
      {
        ok = 1;
        break;
      }
    }
    if ( !ok )
      return 0;
  }
  return 1;
}

int Mesh::same_root_parents ( LVertex *v0, LVertex *v1 )
{
  LVertex *v[2];
  if ( v0->getRootParentSize() <= v1->getRootParentSize() )
  {
    v[0] = v0;
    v[1] = v1;
  }
  else
  {
    v[0] = v1;
    v[1] = v0;
  }

  for ( int i = 0; i < v[0]->getRootParentSize(); i++ )
  {
    int ok = 0;
    for ( int j = 0; j < v[1]->getRootParentSize(); j++ )
    {
      if ( v[0]->getRootParent ( i ) == v[1]->getRootParent ( j ) )
      {
        ok = 1;
        break;
      }
    }
    if ( !ok )
      return 0;
  }
  return 1;
}

// int Mesh::same_surf_zero(std::vector<LVertex*> &vx)
// {
//     for (int i = 0; i < vx.size(); i++)
//         if (vx[0]->nbSurfZero() != vx[i]->nbSurfZero())
//             return 0;
//
//     std::set<int> rpset;
//     for (int i = 0; i < vx.size(); i++)
//         for (int j = 0; j < vx[i]->nbSurfZero(); j++)
//             rpset.insert(vx[i]->surfZeroId(j));
//
//     if (rpset.size() == 3) // humm ??
//         return 1;
//     else
//         return 0;
// }
//
// int Mesh::same_root_parents(std::vector<LVertex*> &vx)
// {
//     for (int i = 0; i < vx.size(); i++)
//         if (vx[0]->getRootParentSize() != vx[i]->getRootParentSize())
//             return 0;
//
//     std::set<LVertex*> rpset;
//     for (int i = 0; i < vx.size(); i++)
//         for (int j = 0; j < vx[i]->getRootParentSize(); j++)
//             rpset.insert(vx[i]->getRootParent(j));
//
//     if (rpset.size() == 3) // humm ??
//         return 1;
//     else
//         return 0;
// }


int Mesh::get_generation ( LTetra *t )
{
  int gen = 0;
  LTetra *tt = t;
  while ( tt->getParent() != NULL )
  {
    gen++;
    tt = tt->getParent();
  }
  return gen;
}

LVertex * Mesh::intersection ( LTetra *t, int edge, int s )
{
  LVertex *v[2];
  v[0] = t->getEdgeVertex ( edge, 0 );
  v[1] = t->getEdgeVertex ( edge, 1 );

  double surf[2];
  double val[2];
  val[0] = v[0]->surfVal ( s );
  val[1] = v[1]->surfVal ( s );

  if ( fabs ( val[0] ) < THRESHOLD )
  {
    v[0]->addSurfZero ( s );
    val[0] = 0.0;
  }
  if ( fabs ( val[1] ) < THRESHOLD )
  {
    v[1]->addSurfZero ( s );
    val[1] = 0.0;
  }
  if ( ( val[0] == 0.0 ) || ( val[1] == 0.0 ) )
    return NULL;

  double tt = -val[0]/ ( val[1]-val[0] );
  //     assert(tt > 0.0 && tt < 1.0);

//     double l = sqrt((v[1]->x()-v[0]->x())*(v[1]->x()-v[0]->x()) + (v[1]->y()-v[0]->y())*(v[1]->y()-v[0]->y()) + (v[1]->z()-v[0]->z())*(v[1]->z()-v[0]->z()));
//     if (fabs(tt*l) < 1e-2)
//     {
//       v[0]->addSurfZero(s);
//       return NULL;
//     }
//     else if (fabs((1.0-tt)*l) < 1e-2)
//     {
//       v[1]->addSurfZero(s);
//       return NULL;
//     }

  LVertex *newv = new_vertex ( v[0]->x() +tt* ( v[1]->x()-v[0]->x() ), v[0]->y() +tt* ( v[1]->y()-v[0]->y() ), v[0]->z() +tt* ( v[1]->z()-v[0]->z() ) );
  newv->setParent ( v[0], v[1] ); // utilisation ulterieur pour optimisation

  compute_common_surf ( v[0], v[1] );
  for ( int i = 0; i < common_surf_size(); i++ )
  {
    int cs = get_common_surf ( i );
    val[0] = v[0]->surfVal ( cs );
    val[1] = v[1]->surfVal ( cs );
    double newval = val[0]+tt* ( val[1]-val[0] );
    newv->addSurf ( cs, newval );
    if ( val[1] != val[0] )
    {
      double rel = newval/ ( val[1]-val[0] );
      if ( fabs ( rel ) <= THRESHOLD )
        newv->addSurfZero ( cs );
    }
  }
  assert ( newv->nbSurf() == common_surf_size() );

  compute_common_zero_surf ( v[0], v[1] );
  for ( int i = 0; i < common_surf_size(); i++ )
  {
    int cs = get_common_surf ( i );
    newv->addSurfZero ( cs );
  }

  return newv;
}

void Mesh::split_edge ( LVertex *newv, int update )
{
  LTetra *t;
  int edge;
  int surf;
  // boucle sur la coquille
  for ( int i = 0; i < _sot->size(); i++ )
  {
    _sot->get_tetra ( i, &t, &edge );
    LTetra *t1, *t2;
    // creation des tetras
    t->split_edge ( edge, newv, &t1, &t2 );
    t1->setId ( global_tetra_index++ );
    t2->setId ( global_tetra_index++ );
    t1->setOldVol ( t->getOldVol() );
    t2->setOldVol ( t->getOldVol() );
    t->setOldVol ( -1 );
    t->setParent();
    // ajout des faces a la structure d'adjacence
    _hfs->addAdjFace ( t1, 1 );
    _hfs->addAdjFace ( t2, 2 );
    _hfs->addAdjFace ( t1, 2 );
    _hfs->addAdjFace ( t2, 1 );
    // ajout/retrait de la structure mesh
    remove_tetra ( t );
    add_tetra ( t1 );
    add_tetra ( t2 );
#if 0
    for ( int j = 0; j < 4; j++ )
      if ( t1->getAdj ( j ) != NULL )
        assert ( t1->getAdj ( j )->getIndex() != -1 );
    for ( int j = 0; j < 4; j++ )
      if ( t2->getAdj ( j ) != NULL )
        assert ( t2->getAdj ( j )->getIndex() != -1 );
#endif
    if ( update )
    {
      // mise a jour des edges dans la structure de hashage des aretes
      static int eu[6][5] = {{1, 2, 4, 3, 5},{0, 2, 5, 3, 4},{0, 1, 5, 4, 3},{0, 4, 5, 1, 2},{0, 3, 5, 2, 1},{3, 1, 2, 4, 0}};
      _hes->updateEdge ( t, eu[edge][0], t1 );
      _hes->updateEdge ( t, eu[edge][1], t1 );
      _hes->updateEdge ( t, eu[edge][2], t2 );
      _hes->updateEdge ( t, eu[edge][3], t2 );
      _hes->updateEdge ( t, eu[edge][4], t1 );
      // ajout des aretes nouvellement crees dans la pile
      if ( i == 0 )
      {
        // les deux aretes issues de l'arete coupees
        surf = crossing_surf ( t1, 2 );
        if ( surf != -1 )
          _hes->addEdge ( t1, 2, surf );
        surf = crossing_surf ( t2, 2 );
        if ( surf != -1 )
          _hes->addEdge ( t2, 2, surf );
      }
      surf = crossing_surf ( t1, 4 );
      if ( surf != -1 )
        _hes->addEdge ( t1, 4, surf );
      surf = crossing_surf ( t2, 4 );
      if ( surf != -1 )
        _hes->addEdge ( t2, 4, surf );
    }
  }
  _hfs->clear();
}


void Mesh::split_face ( LVertex *newv, int update )
{
  LTetra *t;
  int face;
  int surf;
  for ( int i = 0; i < 2; i++ )
  {
    t = tetra_from_queue();
    face = face_from_queue();
    if ( t != NULL )
    {
//      printf("split tetra %d -- face %d --> v %d %d %d %d\n", t->getIndex(), face, t->getVertex(0)->getIndex(), t->getVertex(1)->getIndex(), t->getVertex(2)->getIndex(), t->getVertex(3)->getIndex());
      LTetra *t1, *t2, *t3;
      // creation des tetras
      t->split_face ( face, newv, &t1, &t2, &t3 );
      t1->setId ( global_tetra_index++ );
      t2->setId ( global_tetra_index++ );
      t3->setId ( global_tetra_index++ );
      // ajout des faces a la structure d'adjacence
      _hfs->addAdjFace ( t1, 0 );
      _hfs->addAdjFace ( t2, 0 );
      _hfs->addAdjFace ( t3, 0 );
      // ajout/retrait de la structure mesh
      remove_tetra ( t );
      add_tetra ( t1 );
      add_tetra ( t2 );
      add_tetra ( t3 );
#if 0
      for ( int j = 0; j < 4; j++ )
        if ( t1->getAdj ( j ) != NULL )
          assert ( t1->getAdj ( j )->getIndex() != -1 );
      for ( int j = 0; j < 4; j++ )
        if ( t2->getAdj ( i ) != NULL )
          assert ( t2->getAdj ( j )->getIndex() != -1 );
      for ( int j = 0; j < 4;j++ )
        if ( t3->getAdj ( j ) != NULL )
          assert ( t3->getAdj ( j )->getIndex() != -1 );
#endif
      if ( update )
      {
        // mise a jour des edges dans la structure de hashage des aretes
        static int fu[4][6] = {{0, 1, 2, 3, 4, 5},{0, 3, 4, 1, 5, 2},{1, 5, 3, 2, 4, 0},{2, 4, 5, 0, 3, 1}};
        _hes->updateEdge ( t, fu[face][0], t1 );
        _hes->updateEdge ( t, fu[face][1], t2 );
        _hes->updateEdge ( t, fu[face][2], t3 );
        _hes->updateEdge ( t, fu[face][3], t1 );
        _hes->updateEdge ( t, fu[face][4], t2 );
        _hes->updateEdge ( t, fu[face][5], t3 );
        // les aretes issues de la face coupee
        surf = crossing_surf ( t1, 2 );
        if ( surf != -1 )
          _hes->addEdge ( t1, 2, surf );
        surf = crossing_surf ( t2, 2 );
        if ( surf != -1 )
          _hes->addEdge ( t2, 2, surf );
        surf = crossing_surf ( t3, 2 );
        if ( surf != -1 )
          _hes->addEdge ( t3, 2, surf );
        // arete cree au milieu
        surf = crossing_surf ( t1, 5 );
        if ( surf != -1 )
          _hes->addEdge ( t1, 5, surf );
      }
    }
    tetra_dequeue();
    face_dequeue();
  }
  _hfs->clear();
}

void Mesh::split_tetra ( LVertex *newv, int update )
{
  LTetra *t = tetra_from_queue();
  LTetra *t1, *t2, *t3, *t4;
  int surf;
  // creation des tetras
  t->split ( newv, &t1, &t2, &t3, &t4 );
  t1->setId ( global_tetra_index++ );
  t2->setId ( global_tetra_index++ );
  t3->setId ( global_tetra_index++ );
  t4->setId ( global_tetra_index++ );
  // ajout/retrait de la structure mesh
  remove_tetra ( t );
  add_tetra ( t1 );
  add_tetra ( t2 );
  add_tetra ( t3 );
  add_tetra ( t4 );
#if 1
  for ( int i = 0; i < 4; i++ )
    if ( t1->getAdj ( i ) != NULL )
      assert ( t1->getAdj ( i )->getIndex() != -1 );
  for ( int i = 0; i < 4; i++ )
    if ( t2->getAdj ( i ) != NULL )
      assert ( t2->getAdj ( i )->getIndex() != -1 );
  for ( int i = 0; i < 4; i++ )
    if ( t3->getAdj ( i ) != NULL )
      assert ( t3->getAdj ( i )->getIndex() != -1 );
  for ( int i = 0; i < 4; i++ )
    if ( t4->getAdj ( i ) != NULL )
      assert ( t4->getAdj ( i )->getIndex() != -1 );
#endif
  if ( update )
  {
    // mise a jour des edges dans la structure de hashage des aretes
    _hes->updateEdge ( t, 3, t1 );
    _hes->updateEdge ( t, 4, t1 );
    _hes->updateEdge ( t, 5, t1 );
    _hes->updateEdge ( t, 1, t2 );
    _hes->updateEdge ( t, 2, t2 );
    _hes->updateEdge ( t, 0, t3 );
    // les aretes crees au milieu
    surf = crossing_surf ( t1, 2 );
    if ( surf != -1 )
      _hes->addEdge ( t1, 2, surf );
    surf = crossing_surf ( t1, 4 );
    if ( surf != -1 )
      _hes->addEdge ( t1, 4, surf );
    surf = crossing_surf ( t1, 5 );
    if ( surf != -1 )
      _hes->addEdge ( t1, 5, surf );
    surf = crossing_surf ( t2, 2 );
    if ( surf != -1 )
      _hes->addEdge ( t2, 2, surf );
  }
  tetra_dequeue();
}


void Mesh::cut_mesh()
{
  tprintf ( "Mesh cutting START\n" );
  clock_t init = clock ();
  set_all_tetra_flag ( -1 );
  set_all_vertex_flag ( -1 );
  _hfs->clear();
  int nb_tetra_init = tetra_size();
  tprintf ( "Edge struct size --> %d\n", _hes->size() );
  progress_bar pbar ( _hes->get_stack_number() );
  pbar.start();
  while ( _hes->size() )
  {
    LTetra *t;
    int edge;
    pbar.progress ( _hes->get_current_surf() );
    _hes->getEdge ( &t, &edge );
    _hes->deleteCurrent(); // tres important ici !!
    assert ( t != NULL );
    assert ( edge >=0 && edge < 6 );
    _sot->set_edge ( t, edge );
    _sot->build();
    process_edge();
    _hes->nextEdge();
  }
  _hfs->clear();
  pbar.end();

  writeMeshVTK ( "cut" );

  clock_t end = clock ();
  double cpu_t = ( end - init ) * 1e-6;
  int nb_tetra_end = tetra_size();
  tprintf ( "--> %d vertices\n", vertex_size() );
  tprintf ( "--> %d tetrahedra\n", tetra_size() );
//     tprintf("--> Mesh footprint > %.2lf Mb\n", memory()/1e6);
  tprintf ( " --> %d tetra / sec\n", ( int ) ( ( double ) ( nb_tetra_end - nb_tetra_init ) /cpu_t ) );
  tprintf ( "Mesh cutting END\n" );
}

LTetra *Mesh::locate_vertex ( LVertex *v, double b[4] )
{
  srand ( 0 );
  static int dir[6][4] = {{0, 1, 2, 3}, {0, 1, 3, 2}, {0, 2, 3, 1}, {0, 2, 1, 3}, {0, 3, 2, 1}, {0, 3, 1, 2}};
  LVertex *mv = _bs->get_closest ( v );
//     printf("closest --> %p\n", mv);
  std::vector<LTetra*> tf;
  LTetra *t = mv->getTetra();
  while ( 1 )
  {
    t->barycentric ( v, b );
    int init = rand() % 6;
    int dec = rand() % 4;
    t->setFlag ( 1 );
    tf.push_back ( t );
    int found = 1;
    for ( int i = 0; i < 4; i++ ) // determination de la direction de recherche
    {
      int ii = ( dir[init][i]+dec ) %4;
      if ( b[ii] < -THRESHOLD )
      {
        if ( t->getAdj ( ii ) != NULL )
        {
          if ( t->getAdj ( ii )->getFlag() == -1 )
          {
            t = t->getAdj ( ii );
            found = 0;
          }
          break;
        }
      }
    }
    if ( found )
    {
      for ( int i = 0; i < tf.size(); i++ )
        tf[i]->resetFlag();
      return t;
    }
  }
  assert ( 0 );
}

// insert a topologic vertex in the mesh giving the tetra containing it and the barycentric coordinates
LVertex *Mesh::insert_topo_vertex ( LVertex *newv, LTetra *t, double b[4], std::vector<int> surf )
{
  assert ( newv != NULL );
  assert ( t != NULL );
  assert ( t->getIndex() != -1 );
//     printf("Inserting vertex %d (tetra %d) --> %lf %lf %lf %lf\n", newv->getIndex(), t->getIndex(), b[0], b[1], b[2], b[3]);
//     t->print();
  int zero[4];
  int nz = 0;
  int plus[4];
  int np = 0;
  for ( int i = 0; i < 4; i++ )
  {
    if ( fabs ( b[i] ) < THRESHOLD_BARY_FACE )
      zero[nz++] = i;
    else
      plus[np++] = i;
  }
  // transfert des levelset existantes
  std::vector<LVertex*> vx;
  for ( int i = 0; i < np; i++ )
    vx.push_back ( t->getVertex ( plus[i] ) );
  compute_common_surf ( vx );
  for ( int i = 0; i < common_surf_size(); i++ )
  {
    std::vector<double> val;
    int cs = get_common_surf ( i );
    for ( int i = 0; i < np; i++ )
      val.push_back ( vx[i]->surfVal ( cs ) );
    double newval = 0.0;
    for ( int i = 0; i < np; i++ )
      newval += b[plus[i]]*val[i];
    if ( !newv->checkSurf ( cs ) )
      newv->addSurf ( cs, newval );
    if ( fabs ( newval ) <= THRESHOLD && !newv->checkZero ( cs ) )
      newv->addSurfZero ( cs );
  }
//   assert ( newv->nbSurf() == common_surf_size() );
  compute_common_zero_surf ( vx );
  for ( int i = 0; i < common_surf_size(); i++ )
  {
    int cs = get_common_surf ( i );
    if ( !newv->checkZero ( cs ) )
      newv->addSurfZero ( cs );
  }
  // insertion des nouvelles surf
  for ( int i = 0; i < surf.size(); i++ )
  {
    if ( !newv->checkSurf ( surf[i] ) )
    {
      newv->addSurf ( surf[i], 0.0 );
      newv->addSurfZero ( surf[i] );
    }
    else if ( !newv->checkZero ( surf[i] ) )
      newv->addSurfZero ( surf[i] );
  }
//     newv->dumpSurf();
  if ( nz == 1 ) // face
  {
//  printf("splitting tetra %d -- face %d\n", t->getIndex(), zero[0]);
    add_vertex ( newv );
    add_tetra_to_queue ( t );
    add_face_to_queue ( zero[0] );
    add_tetra_to_queue ( t->getAdj ( zero[0] ) );
    add_face_to_queue ( t->getOpp ( zero[0] ) );
    split_face ( newv, 0 );
    return newv;
  }
  else if ( nz == 2 ) // arete
  {
    int edge = t->getEdgeIndex ( plus[0], plus[1] );
//  printf("splitting tetra %d -- edge %d\n", t->getIndex(), edge);
    add_vertex ( newv );
    _sot->set_edge ( t, edge );
    _sot->build();
    split_edge ( newv, 0 );
    _sot->unlock();
    return newv;
  }
  else if ( nz == 3 ) // sommet
  {
//  printf("adding ls to vertex %d\n", t->getVertex(plus[0])->getIndex());
    // on ajoute les levelsets au sommet !
    LVertex *mv = t->getVertex ( plus[0] );
    mv->mergeSurf ( newv );
    return mv;
  }
  else // tetra
  {
//         printf("splitting tetra %d\n", t->getIndex());
    add_vertex ( newv );
    add_tetra_to_queue ( t );
    split_tetra ( newv, 0 );
    return newv;
  }
}

// insert edge, ie all intersections with a tetrahedron (but don't insert v0 nor v1)
LTetra *Mesh::insert_topo_edge ( LVertex *v0, LVertex *v1, LTetra *t, std::vector<int> surf )
{
  assert ( v0 != NULL );
  assert ( v1 != NULL );
  assert ( t != NULL );
  assert ( t->getIndex() != -1 );
//     printf("Inserting edge %d -- %d (tetra %d)\n", v0->getIndex(), v1->getIndex(), t->getIndex());
  std::vector<LTetra*> tf;
  LVertex *init = v0;
  LVertex *inter = NULL;
  LTetra *tb = NULL;
  int eb = -1;
  int inside = 0;
  LTetra *final = NULL;
  double b[4];

  t->barycentric ( init, b );
  init = insert_topo_vertex ( init, t, b, surf );
  inter = init;
  while ( inter != NULL )
  {
    _bot->set_vertex ( init );
    _bot->build();
    for ( int i = 0; i < _bot->size(); i++ )
    {
      _bot->get_tetra ( i, &tb, &eb );
      if ( tb->getFlag() == -1 )
      {
        tb->setFlag ( 1 );
        tf.push_back ( tb );
        if ( tb->visibility ( eb, v1, b, &inter, &inside ) )
        {
          init = inter;
          tb->barycentric ( init, b );
          init = insert_topo_vertex ( init, tb, b, surf );
//        if (init != inter) // --> memoryleak !
//          delete inter;
//        check_adjacence();
          break;
        }
        if ( inside )
        {
          final = tb;
          break;
        }
      }
    }
    _bot->unlock();
  }
  // restore flags
  for ( int i = 0; i < tf.size(); i++ )
    tf[i]->resetFlag();

  if ( final == NULL )
    _bot->dump_vtk();

  assert ( final != NULL );
  return final;
}

void Mesh::buildRootParents()
{
  tprintf ( "--> compute root parents ...\n" );
  for ( int i = 0; i < vertex_size(); i++ )
  {
    LVertex *v = get_vertex ( i );
    if ( !v->getRootParentSize() )
    {
      v->computeRootParents();
    }
  }
}

int Mesh::delete_degenerated_tetra ( LTetra *t, double threshold, std::vector<LTetra*> &modified )
{
  std::vector<adouble> edge_lenght;
  LVertex edge;
  for ( int j = 0; j < 6; j++ )
  {
    LVertex *v0 = t->getEdgeVertex ( j, 0 );
    LVertex *v1 = t->getEdgeVertex ( j, 1 );
    edge.setVector ( v0, v1 );
    edge_lenght.push_back ( adouble ( j, edge.norm() ) );
  }
  std::sort ( edge_lenght.begin(), edge_lenght.end() );
  std::vector<LVertex*> vx ( 2, ( LVertex * ) NULL );
  for ( int j = 0; j < 6; j++ )
  {
    int eid = edge_lenght[j].id();
    LVertex *v0 = t->getEdgeVertex ( eid, 0 );
    LVertex *v1 = t->getEdgeVertex ( eid, 1 );
    if ( same_root_parents ( v0, v1 ) )
    {
      vx[0] = v0;
      vx[1] = v1;
      if ( v0->getRootParentSize() == v1->getRootParentSize() )
      {
        if ( merge_vertices ( vx, vx[0], true, threshold, modified ) )
          return 1;
      }
      else if ( v0->getRootParentSize() < v1->getRootParentSize() )
      {
        if ( merge_vertices ( vx, vx[0], false, threshold, modified ) )
          return 1;
      }
      else if ( v0->getRootParentSize() > v1->getRootParentSize() )
      {
        if ( merge_vertices ( vx, vx[1], false, threshold, modified ) )
          return 1;
      }
    }
  }
  return 0;
}

int Mesh::delete_extra_tetra ( LTetra *t, double threshold, std::vector<LTetra*> &modified )
{
  std::vector<LVertex*> vx ( 2, ( LVertex * ) NULL );
  for ( int i = 0; i < 6; i++ )
  {
    LVertex *v0 = t->getEdgeVertex ( i, 0 );
    LVertex *v1 = t->getEdgeVertex ( i, 1 );
    if ( v0->nbSurfZero() && same_surf_zero ( v0, v1 ) )
    {
      if ( same_root_parents ( v0, v1 ) )
      {
        vx[0] = v0;
        vx[1] = v1;
        if ( v0->getRootParentSize() == v1->getRootParentSize() && v0->nbSurfZero() < 3 && v0->nbSurfZero() == v1->nbSurfZero() )
        {
          if ( merge_vertices ( vx, vx[0], true, threshold, modified ) )
            return 1;
        }
        else if ( v0->getRootParentSize() < v1->getRootParentSize() && v0->nbSurfZero() > v1->nbSurfZero() )
        {
          if ( merge_vertices ( vx, vx[0], false, threshold, modified ) )
            return 1;
        }
        else if ( v0->getRootParentSize() > v1->getRootParentSize() && v0->nbSurfZero() < v1->nbSurfZero() )
        {
          if ( merge_vertices ( vx, vx[1], false, threshold, modified ) )
            return 1;
        }
      }
    }
  }
  return 0;
}

void Mesh::clean_invalid_tetras ( double threshold )
{
  tprintf ( "--> remove invalid tetras ...\n" );

  std::queue<LTetra*> checklist;
  std::vector<LTetra*> modified;
  LTetra *t = NULL;

  // construction de la liste initiale
  for ( int i = 0; i < tetra_size(); i++ )
  {
    t = get_tetra ( i );
    if ( t != NULL && get_tetra ( i )->getIndex() != -1 )
    {
      compute_common_zero_surf ( t );
      if ( common_surf_size() )
        checklist.push ( t );
    }
  }
  tprintf ( "--> found %d invalid tetras ...\n", ( int ) checklist.size() );

  // vide la structure de hashage
  _hfs->clear();

  int merged = 0;
  int failed = 0;

  while ( !checklist.empty() )
  {
    t = checklist.front();
    checklist.pop();;
    if ( t != NULL && t->getIndex() != -1 )
    {
      compute_common_zero_surf ( t );
      if ( common_surf_size() )
      {
        if ( delete_degenerated_tetra ( t, threshold, modified ) )
        {
          for ( int i = 0; i < modified.size(); i++ )
            checklist.push ( modified[i] );
          merged++;
        }
        else
          failed++;
      }
    }
  }
  tprintf ( "--> remain %d invalid tetras\n", failed );
}

void Mesh::remove_extra_tetras ( double threshold )
{
  tprintf ( "--> remove extra tetras ...\n" );

  std::vector<LTetra*> modified;
  std::vector<LVertex*> vx ( 2, ( LVertex * ) NULL );
  LTetra *t = NULL;
  int count;

//     do
//     {
  // vide la structure de hashage
  _hfs->clear();

  count = 0;
  for ( int i = 0; i < tetra_size(); i++ )
  {
    t = get_tetra ( i );
    if ( t != NULL && get_tetra ( i )->getIndex() != -1 )
    {
      if ( delete_extra_tetra ( t, threshold, modified ) )
        count++;
    }
  }
  tprintf ( "--> merged %d extra tetras\n", count );
//     }
//     while (count);
}

void Mesh::optimize()
{
  tprintf ( "Mesh optimizing START\n" );

//     writeMesh("regular_mesh");

  // determine les parents racine
  buildRootParents();

  // elimine les tetraedres planaires
  clean_invalid_tetras ( -THRESHOLD_VOLUME/*0.0*/ );

  // supprime un maximum de tetraedre
//     remove_extra_tetras(THRESHOLD_VOLUME);

  int old_tetra_size = tetra_size();
  int old_vertex_size = vertex_size();

  shrink_vertex();
  shrink_tetra();

  check_indexing();

  tprintf ( "Removed %d vertices (- %.2lf %%)\n", old_vertex_size-vertex_size(), ( ( double ) old_vertex_size- ( double ) vertex_size() ) *100.0/ ( double ) vertex_size() );
  tprintf ( "Removed %d tetra (- %.2lf %%)\n", old_tetra_size-tetra_size(), ( ( double ) old_tetra_size- ( double ) tetra_size() ) *100.0/ ( double ) tetra_size() );

//     writeMesh("optimized_mesh");
  tprintf ( "Mesh optimizing END\n" );
}


int Mesh::merge_vertices ( std::vector<LVertex*> list, LVertex *mv, int move_allowed, double threshold, std::vector<LTetra*> &modified ) // merge list to mv and change mv position
{
  // check
  assert ( ( list.size() >= 2 ) && ( list.size() <= 4 ) );
  for ( int i = 0; i < list.size(); i++ )
    assert ( list[i]->getIndex() != -1 );

  // calcul de la position du nouveau point
  LVertex merge ( 0.0, 0.0, 0.0 );
  if ( move_allowed )
  {
    LVertex merge ( 0.0, 0.0, 0.0 );
    for ( int i = 0; i < list.size(); i++ )
      merge.setPosition ( merge.x() +list[i]->x(), merge.y() +list[i]->y(), merge.z() +list[i]->z() );
    merge.setPosition ( merge.x() /list.size(), merge.y() /list.size(), merge.z() /list.size() );
  }
  else
  {
    merge.setPosition ( mv->x(), mv->y(), mv->z() );
  }

  std::map<LTetra*, int> map;
  std::map<LTetra*, int>::iterator it;
  std::vector<LTetra*> tetlist;
  std::vector<int> opplist;

  for ( int i = 0; i < list.size(); i++ )
  {
    _bot->set_vertex ( list[i] );
    _bot->build();
    LTetra *tb;
    int eb;
    for ( int j = 0; j < _bot->size(); j++ )
    {
      _bot->get_tetra ( j, &tb, &eb );
      assert ( tb->getIndex() != -1 );
      assert ( tb->getVertex ( eb ) == list[i] );
      // verification de la validite
      double vol = tb->volume ( eb, &merge );
      if ( vol <= threshold )
        return 0;

      it = map.find ( tb );
      if ( it == map.end() )
        map.insert ( std::pair<LTetra*, int> ( tb, 1 ) );
      else
        it->second++;

      tetlist.push_back ( tb );
      opplist.push_back ( eb );
    }
    _bot->unlock();
  }

  // merge des surfaces --> a verifier
  for ( int i = 0; i < list.size(); i++ )
    if ( mv != list[i] )
      mv->mergeSurf ( list[i] );

  mv->setPosition ( merge.x(), merge.y(), merge.z() );

  for ( int i = 0; i < tetlist.size(); i++ )
    tetlist[i]->setVertex ( opplist[i], mv );

  std::vector<LTetra*> rmtetlist;
  std::vector<LTetra*> adjtetlist;
  std::vector<int> adjfacelist;

  modified.clear();
  for ( it = map.begin() ; it != map.end(); it++ )
  {
    LTetra *t = it->first;
    int count_v = it->second;
    if ( count_v == 1 ) // on ajoute t au tableau "modified"
    {
      modified.push_back ( t );
    }
    if ( count_v == 2 ) // on supprime le tetra en ajoutant les adjacences orphelines
    {
      for ( int i = 0; i < 4; i++ )
        if ( t->getVertex ( i ) == mv && t->getAdj ( i ) )
        {
          adjtetlist.push_back ( t->getAdj ( i ) );
          adjfacelist.push_back ( t->getOpp ( i ) );
        }
    }
    if ( count_v >= 2 ) // on supprime purement et simplement, sans tenir compte des adjacences
      rmtetlist.push_back ( t );
  }

  for ( int i = 0; i < rmtetlist.size(); i++ )
    remove_tetra ( rmtetlist[i] );

  for ( int i = 0; i < list.size(); i++ )
    remove_vertex ( list[i] );

  add_vertex ( mv );

  for ( int i = 0; i < adjtetlist.size(); i++ )
  {
    if ( adjtetlist[i]->getIndex() != -1 )
      _hfs->addAdjFace ( adjtetlist[i], adjfacelist[i] );
  }

  for ( int i = 0; i < tetlist.size(); i++ )
  {
    LTetra *t = tetlist[i];
    if ( t->getIndex() != -1 )
    {
      for ( int j = 0; j < 4; j++ )
      {
        assert ( t->getVertex ( j )->getIndex() != -1 );
        t->getVertex ( j )->setTetra ( t );
      }
    }
  }
  return 1;
}

int Mesh::color ( int vol, int unfilled_vol )
{
  int count = 0;
  while ( _sc.size() )
  {
    LTetra *t = _sc.front();
    if ( t->getVol() == unfilled_vol )
    {
      t->setVol ( vol );
      count++;
      for ( int i = 0; i < 4; i++ )
        if ( t->getAdj ( i ) != NULL )
          _sc.push ( t->getAdj ( i ) );
    }
    _sc.pop();
  }
  return count;
}

void Mesh::compute_adjacencies()
{
  _hfs->clear();
  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    assert ( !t->isParent() );
    for ( int j = 0; j < 4; j++ )
      _hfs->addAdjFace ( t, j );
  }
  _hfs->clear();
}

void Mesh::check_adjacence()
{
  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    if ( t->getIndex() == -1 )
    {
      printf ( "LTetra %d has no index !!\n", i );
      getchar();
    }
    for ( int j = 0; j < 4; j++ )
    {
      if ( t->getAdj ( j ) != NULL )
      {
        LTetra *a = t->getAdj ( j );
        if ( a->getIndex() == -1 )
        {
          printf ( "Adj %d of tetra %d has no index !!\n", j, t->getIndex() );
          getchar();
        }
        int o = t->getOpp ( j );
        if ( a->getAdj ( o ) != t || a->getOpp ( o ) != j )
        {
          printf ( "Error in adjacency matrix !!\n" );
          getchar();
        }
      }
    }
  }
}

void Mesh::check_indexing()
{
//     printf("tetra size --> %d\n", tetra_size());
  for ( int i = 0; i < tetra_size(); i++ )
  {
    if ( get_tetra ( i ) != NULL )
    {
//             printf("tetra %d (%p)\n", get_tetra(i)->getIndex(), get_tetra(i));
      assert ( get_tetra ( i )->getIndex() != -1 );
      assert ( get_tetra ( i )->getIndex() == i );
      for ( int j = 0; j < 4; j++ )
      {
        assert ( get_tetra ( i )->getVertex ( j ) != NULL );
//                 printf("vertex %d --> %d (%p)\n", j, get_tetra(i)->getVertex(j)->getIndex(), get_tetra(i)->getVertex(j));
        assert ( get_tetra ( i )->getVertex ( j )->getIndex() != -1 );
        assert ( get_tetra ( i )->getVertex ( j )->getIndex() < vertex_size() );
        assert ( get_tetra ( i )->getVertex ( j )->getTetra()->getIndex() != -1 );
        assert ( get_tetra ( i )->getVertex ( j )->getTetra()->getIndex() < tetra_size() );
      }
    }
  }
//     printf("vertex size --> %d\n", vertex_size());
  for ( int i = 0; i < vertex_size(); i++ )
  {
    if ( get_vertex ( i ) != NULL )
    {
//      printf("vertex %d --> %d (%p) --> t %d (%p)\n", i, get_vertex(i)->getIndex(), get_vertex(i), get_vertex(i)->getTetra()->getIndex(), get_vertex(i)->getTetra());
      assert ( get_vertex ( i )->getIndex() != -1 );
      assert ( get_vertex ( i )->getIndex() == i );
      assert ( get_vertex ( i )->getTetra() != NULL );
//      if (get_vertex(i)->getTetra()->getIndex() == -1)
//        dump_tetra(get_vertex(i)->getTetra());
      assert ( get_vertex ( i )->getTetra()->getIndex() != -1 );
      assert ( get_vertex ( i )->getTetra()->getIndex() < tetra_size() );
    }
  }
}

void Mesh::check_surfaces ( int s )
{
  printf ( "Checking surface %d\n", s );
  // controle de l'absence d'arete instersectees
  for ( int i = 0; i<tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    for ( int j = 0; j<6; j++ )
    {
      LVertex *v0 = t->getEdgeVertex ( j, 0 );
      LVertex *v1 = t->getEdgeVertex ( j, 1 );
      compute_common_surf ( v0, v1 );
      for ( int i = 0; i < common_surf_size(); i++ )
      {
        int cs = get_common_surf ( i );
        if ( cs <= s )
        {
          double val[2];
          val[0] = v0->surfVal ( cs );
          val[1] = v1->surfVal ( cs );
          if ( !v0->checkZero ( cs ) && !v1->checkZero ( cs ) )
          {
            if ( val[0] * val[1] < 0.0 )
            {
              printf ( "surf %d --> : v0 (%d) --> %lf, v1 (%d) --> %lf\n", cs, v0->getIndex(), val[0], v1->getIndex(), val[1] );
            }
          }
        }
      }
    }
  }
  printf ( "End checking !\n" );
//     getchar();
}

Merge::Merge ( LTetra* t, int surf )
{
  _t = t;
  _s = surf;
  _n = NULL;
  _c = NULL;
  _ref = NULL;
  _imposed = 0;
  flag = -1;

  build();
//     if (_n != NULL)
//     {
//         printf("c --> %lf %lf %lf\n", _c->x(), _c->y(), _c->z());
//         printf("n --> %lf %lf %lf\n", _n->x(), _n->y(), _n->z());
//     }
}

void Merge::set ( LVertex *v0, LVertex *v1, LVertex *v2 )
{
//     printf("set tri\n");
  _c = new LVertex ( ( v0->x() +v1->x() +v2->x() ) /3.0, ( v0->y() +v1->y() +v2->y() ) /3.0, ( v0->z() +v1->z() +v2->z() ) /3.0 );

  LVertex uu, vv;
  _n = new LVertex;
  uu.setVector ( v0, v1 );
  vv.setVector ( v0, v2 );
  uu.crossproduct ( &vv, _n );
  _n->normalize();
}

void Merge::set ( LVertex *v0, LVertex *v1, LVertex *v2, LVertex *v3 )
{
//     printf("set quad\n");
  _c = new LVertex ( ( v0->x() +v1->x() +v2->x() +v3->x() ) /4.0, ( v0->y() +v1->y() +v2->y() +v3->y() ) /4.0, ( v0->z() +v1->z() +v2->z() +v3->z() ) /4.0 );

  LVertex uu, vv;
  LVertex n1, n2;
  _n = new LVertex;
  uu.setVector ( v0, v1 );
  vv.setVector ( v0, v2 );
  uu.crossproduct ( &vv, &n1 );
  n1.normalize();
  uu.setVector ( v0, v2 );
  vv.setVector ( v0, v3 );
  uu.crossproduct ( &vv, &n2 );
  n2.normalize();
  _n = new LVertex ( ( n1.x() +n2.x() ) /2.0, ( n1.y() +n2.y() ) /2.0, ( n1.z() +n2.z() ) /2.0 );
  _n->normalize();
}

void Merge::build()
{
  for ( int i = 0; i < 4; i++ )
    if ( _t->getFaceVertex ( i, 0 )->checkZero ( _s ) && _t->getFaceVertex ( i, 1 )->checkZero ( _s ) && _t->getFaceVertex ( i, 2 )->checkZero ( _s ) )
    {
      set ( _t->getFaceVertex ( i, 0 ), _t->getFaceVertex ( i, 1 ), _t->getFaceVertex ( i, 2 ) );
      return;
    }

  LVertex *v0, *v1;
  for ( int i = 0; i < 6; i++ )
  {
    v0 = _t->getEdgeVertex ( i, 0 );
    v1 = _t->getEdgeVertex ( i, 1 );
    double tt = -v0->surfVal ( _s ) / ( v1->surfVal ( _s )-v0->surfVal ( _s ) );
    if ( tt > 0.0 && tt < 1.0 )
    {
//             _tlist.push_back(tt);
      LVertex *vi = new LVertex ( v0->x() +tt* ( v1->x()-v0->x() ), v0->y() +tt* ( v1->y()-v0->y() ), v0->z() +tt* ( v1->z()-v0->z() ) );
      _vlist[i] = vi;
    }
    else
      _vlist[i] = NULL;
  }

  static const int tri[4][3] =
  {
    {0, 1, 2},
    {0, 3, 4},
    {1, 5, 3},
    {2, 4, 5}
  };

  static const int quad[2][4] =
  {
    {1, 3, 4, 2},
    {1, 0, 4, 5}
  };

  for ( int i = 0; i < 2; i++ )
  {
    if ( _vlist[quad[i][0]] != NULL && _vlist[quad[i][1]] != NULL && _vlist[quad[i][2]] != NULL && _vlist[quad[i][3]] != NULL )
    {
      set ( _vlist[quad[i][0]], _vlist[quad[i][1]], _vlist[quad[i][2]], _vlist[quad[i][3]] );
      for ( int j = 0; j < 6; j++ )
        if ( _vlist[j] != NULL )
          delete _vlist[j];
      return;
    }
  }

  for ( int i = 0; i < 4; i++ )
  {
    if ( _vlist[tri[i][0]] != NULL && _vlist[tri[i][1]] != NULL && _vlist[tri[i][2]] != NULL )
    {
      set ( _vlist[tri[i][0]], _vlist[tri[i][1]], _vlist[tri[i][2]] );
      for ( int j = 0; j < 6; j++ )
        if ( _vlist[j] != NULL )
          delete _vlist[j];

      return;
    }
  }
}

int Merge::compare ( Merge *m, double dotmin, double distmax )
{
  LVertex *n1 = get_normal();
  LVertex *n2 = m->get_normal();
  double dot = n1->dotproduct ( n2 );
  if ( fabs ( dot ) > dotmin )
  {
    LVertex inter;
    inter.setVector ( get_centroid(), m->get_centroid() );
    double dist = inter.dotproduct ( n1 );
    if ( fabs ( dist ) < distmax )
    {
      return 1;
    }
  }
  return 0;
}

void Merge::merge_with ( Merge *m )
{
  if ( get_ref() != NULL )
  {
    Merge *rm = get_ref();
    m->set_ref ( rm );
  }
  set_ref ( m );
}

int Merge::apply_merge()
{
  if ( get_ref() != NULL )
  {
    Merge *init = get_ref();
    while ( init->get_ref() != NULL && init->get_ref() != get_ref() )
      init = init->get_ref();
    int s0 = init->get_surf();
    int s1 = get_surf();
    LTetra *t = get_tetra();
    for ( int i = 0; i<4; i++ )
    {
      if ( t->getVertex ( i )->checkZero ( s0 ) )
        t->getVertex ( i )->addSurfZero ( s1 );
      else if ( t->getVertex ( i )->checkZero ( s1 ) )
        t->getVertex ( i )->addSurfZero ( s0 );
      else
      {
        double val = t->getVertex ( i )->surfVal ( s0 );
        t->getVertex ( i )->setSurf ( s1, val );
      }
    }
    return 1;
  }
  return 0;
}

void Mesh::build_iso_zero_tri ( LTetra *t, std::vector<Merge*> &mergelist )
{
  compute_common_surf ( t );
  if ( common_surf_size() >= 2 )
  {
    for ( int i = 0; i<common_surf_size(); i++ )
    {
      int surf = get_common_surf ( i );
      LVertex *v0, *v1;

      Merge *mg = new Merge ( t, surf );
      if ( mg->get_normal() != NULL )
        mergelist.push_back ( mg );
      else
        delete mg;
    }

  }
}

int Mesh::merged_surfaces ( LTetra *t, double dotmin, double distmax )
{
  std::vector<Merge*> mergelist;
  std::vector<Merge *> tomerge;
  build_iso_zero_tri ( t, mergelist );


  int count = 0;
#if 0     /* ----- #if 0 : If0Label_1 ----- */
  if ( mergelist.size() >= 3 )
  {
    for ( int i = 0; i < mergelist.size(); i++ )
    {
      int surf = mergelist[i]->get_surf();
      if ( get_entity_manager()->is_virtual_face ( surf ) ) // surface virtuelle
      {
        int s0, s1;
        get_entity_manager()->get_cutted_face_id ( surf, &s0, &s1 );
        int found = 0;
        int ids[2];
        for ( int j = 0; j < mergelist.size() && found < 2; j++ )
        {
          if ( mergelist[j]->get_surf() == s0 )
            ids[found++] = j;
          if ( mergelist[j]->get_surf() == s1 )
            ids[found++] = j;
        }
        if ( found == 2 )
        {
          mergelist[ids[1]]->merge_with ( mergelist[ids[0]] );
          if ( mergelist[ids[1]]->get_flag() == -1 )
            mergelist[ids[1]]->set_flag ( 1 );
          tomerge.push_back ( mergelist[ids[1]] );
        }
      }
    }
  }

  for ( int i = 0; i<tomerge.size(); i++ )
    if ( tomerge[i]->apply_merge() )
      count++;

//     printf("deleting ...\n");
  for ( int i = 0; i <  mergelist.size(); i++ )
    delete mergelist[i];

  mergelist.clear();
  tomerge.clear();
#endif     /* ----- #if 0 : If0Label_1 ----- */


  if ( mergelist.size() >= 2 )
  {
    for ( int i = 0; i < mergelist.size()-1; i++ )
    {
      for ( int j = i+1; j < mergelist.size(); j++ )
      {
        if ( mergelist[i]->compare ( mergelist[j], dotmin, distmax ) )
        {
          mergelist[j]->merge_with ( mergelist[i] );
          if ( mergelist[j]->get_flag() == -1 )
            mergelist[j]->set_flag ( 1 );
          tomerge.push_back ( mergelist[j] );
        }
      }
    }
  }

  for ( int i = 0; i<tomerge.size(); i++ )
    if ( tomerge[i]->apply_merge() )
      count++;

//     printf("deleting ...\n");
  for ( int i = 0; i <  mergelist.size(); i++ )
    delete mergelist[i];

  return count;
}

void Mesh::surface_merging ( double dotmin, double distmax )
{
  // fusion des surfaces quasi paralleles
  tprintf ( "Merging surfaces START\n" );
  int merged = 0;
  progress_bar pbar ( tetra_size() );
  pbar.start();
  for ( int i = 0; i<tetra_size(); i++ )
  {
    pbar.progress ( i );
    LTetra *t = get_tetra ( i );
    merged += merged_surfaces ( t, dotmin, distmax );
  }
  pbar.end();

  tprintf ( "--> %d surfaces were merged !\n", merged );
  tprintf ( "Merging surfaces END\n" );
}

void Mesh::writeMeshVTK ( const char *name )
{
  if ( !_opts->debug )
    return;

  char filename[100];
  sprintf ( filename, "%s_%s.vtk", _opts->basename, name );
  tprintf ( "Writing VTK file %s ....\n", filename );
  FILE *fp = fopen ( filename, "w" );

  fprintf ( fp, "# vtk DataFile Version 2.0\n" );
  fprintf ( fp, "Really cool data\n" );
  fprintf ( fp, "ASCII\n" );
  fprintf ( fp, "DATASET UNSTRUCTURED_GRID\n" );

//     tprintf("--> %d vertices\n", vertex_size());
  fprintf ( fp, "POINTS %d float\n", vertex_size() );
  for ( int i = 0; i < vertex_size(); i++ )
    fprintf ( fp, "%lf %lf %lf\n", get_vertex ( i )->x(), get_vertex ( i )->y(), get_vertex ( i )->z() );

//     tprintf("--> %d tetrahedra\n", tetra_size());
  fprintf ( fp, "CELLS %d %d\n", tetra_size(), tetra_size() *5 );
  for ( int i = 0; i < tetra_size(); i++ )
    fprintf ( fp, "%d %d %d %d %d\n", 4, get_tetra ( i )->getVertex ( 0 )->getIndex(), get_tetra ( i )->getVertex ( 1 )->getIndex(), get_tetra ( i )->getVertex ( 2 )->getIndex(), get_tetra ( i )->getVertex ( 3 )->getIndex() );

  fprintf ( fp, "CELL_TYPES %d\n", tetra_size() );
  for ( int i = 0; i < tetra_size(); i++ )
    fprintf ( fp, "%d\n", 10 );

//     fprintf(fp, "POINT_DATA %d\n", vertex_size());
//     for (int i = 0; i < get_entity_manager()->face_size(); i++)
//     {
//         fprintf(fp, "SCALARS surf_%d int 1\n", i);
//         fprintf(fp, "LOOKUP_TABLE default\n");
//         for (int j = 0; j < vertex_size(); j++)
//             fprintf(fp, "%d\n", get_vertex(j)->checkZero(i) ? 0 : 1);
//     }

//         const char tagp = '0';
//     fprintf(fp, "POINT_DATA %d\n", vertex_size());
//     for (int i = 0; i < get_entity_manager()->face_size(); i++)
//     {
//         fprintf(fp, "SCALARS surf_%d double 1\n", i);
//         fprintf(fp, "LOOKUP_TABLE default\n");
//         for (int j = 0; j < vertex_size(); j++)
//             fprintf(fp, "%lf\n", (get_vertex(j)->surfVal(i) == INFINITY) ? 0/*nanl(&tagp)*/:get_vertex(j)->surfVal(i));
//     }

//     fprintf(fp, "CELL_DATA %d\n", tetra_size());
//     fprintf(fp, "SCALARS vol_id int 1\n");
//     fprintf(fp, "LOOKUP_TABLE default\n");
//     for (int i = 0; i < tetra_size(); i++)
//     {
//         fprintf(fp, "%d\n", get_tetra(i)->getVol());
//     }
//
//     fprintf(fp, "SCALARS adj_nb int 1\n");
//     fprintf(fp, "LOOKUP_TABLE default\n");
//     for (int i = 0; i < tetra_size(); i++)
//     {
//         LTetra *t = get_tetra(i);
//  int count = 0;
//  for (int j = 0; j < 4; j++)
//    if (t->getAdj(j) != NULL)
//      count++;
//         fprintf(fp, "%d\n", count);
//     }

  fclose ( fp );
}

void Mesh::writeSupportVTK ( int local_id_surf )
{
  if ( !_opts->debug )
    return;

  char filename[100];
  sprintf ( filename, "%s_support_%d.vtk", _opts->basename, local_id_surf );

  std::vector<LTetra *> support;
  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    compute_common_surf ( t );
    for ( int j = 0; j < common_surf_size();j++ )
      if ( get_common_surf ( j ) == local_id_surf )
        support.push_back ( t );
  }

  tprintf ( "Writing VTK file %s ....\n", filename );
  FILE *fp = fopen ( filename, "w" );

  fprintf ( fp, "# vtk DataFile Version 2.0\n" );
  fprintf ( fp, "Really cool data\n" );
  fprintf ( fp, "ASCII\n" );
  fprintf ( fp, "DATASET UNSTRUCTURED_GRID\n" );

//     tprintf("--> %d vertices\n", vertex_size());
  fprintf ( fp, "POINTS %d float\n", 4* ( int ) support.size() );
  for ( int i = 0; i < support.size(); i++ )
  {
    LTetra *t = support[i];
    for ( int j = 0; j < 4;j++ )
      fprintf ( fp, "%lf %lf %lf\n", t->getVertex ( j )->x(), t->getVertex ( j )->y(), t->getVertex ( j )->z() );
  }

//     tprintf("--> %d tetrahedra\n", tetra_size());
  fprintf ( fp, "CELLS %d %d\n", ( int ) support.size(), ( int ) support.size() *5 );
  for ( int i = 0; i < support.size(); i++ )
  {
    LTetra *t = support[i];
//         fprintf(fp, "%d %d %d %d %d\n", 4, t->getVertex(0)->getIndex(), t->getVertex(1)->getIndex(), t->getVertex(2)->getIndex(), t->getVertex(3)->getIndex());
    fprintf ( fp, "%d %d %d %d %d\n", 4, 4*i, 4*i+1, 4*i+2, 4*i+3 );
  }

  fprintf ( fp, "CELL_TYPES %d\n", ( int ) support.size() );

  for ( int i = 0; i < support.size(); i++ )
    fprintf ( fp, "%d\n", 10 );

//     fprintf(fp, "POINT_DATA %d\n", vertex_size());
//     for (int i = 0; i < get_entity_manager()->face_size(); i++)
//     {
//         fprintf(fp, "SCALARS surf_%d int 1\n", i);
//         fprintf(fp, "LOOKUP_TABLE default\n");
//         for (int j = 0; j < vertex_size(); j++)
//             fprintf(fp, "%d\n", get_vertex(j)->checkZero(i) ? 0 : 1);
//     }

  fprintf ( fp, "POINT_DATA %d\n", 4* ( int ) support.size() );
  fprintf ( fp, "SCALARS surf_%d double 1\n", local_id_surf );
  fprintf ( fp, "LOOKUP_TABLE default\n" );
  for ( int i = 0; i < support.size(); i++ )
  {
    LTetra *t = support[i];
    for ( int j = 0; j < 4;j++ )
      fprintf ( fp, "%lf\n", t->getVertex ( j )->surfVal ( local_id_surf ) );
  }

  fclose ( fp );
}

void Mesh::writeMesh()
{
  char filename[100];
  sprintf ( filename, "%s_cut.mesh", _opts->basename );
  tprintf ( "Writing mesh file %s ....\n", filename );
  FILE *fp = fopen ( filename, "w" );

  fprintf ( fp, "%d\n", vertex_size() );
  for ( int i = 0; i < vertex_size(); i++ )
  {
    LVertex *v = get_vertex ( i );
    fprintf ( fp, "%lf %lf %lf\n", v->x(), v->y(), v->z() );
  }
  fprintf ( fp, "%d\n", tetra_size() );
  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    fprintf ( fp, "%d %d %d %d\n", t->getVertex ( 0 )->getIndex(), t->getVertex ( 1 )->getIndex(), t->getVertex ( 2 )->getIndex(), t->getVertex ( 3 )->getIndex() );
  }

  fclose ( fp );
}


// void Mesh::writeMeshByVolumeId(char *name)
// {
//     char filename[100];
//     for (int v = 0; v < vol_size(); v++)
//     {
//         int volume = get_vol_id(v);
//         sprintf(filename, "%s_vol_%d.vtk", name, volume);
//         tprintf("Writing %s ....\n", filename);
//         int count = 0;
//         for (int j = 0; j < tetra_size(); j++)
//         {
//             LTetra *t = get_tetra(j);
//             if (t->getVol() == volume)
//                 count++;
//         }
//         FILE *fp = fopen(filename, "w");
//
//         fprintf (fp, "# vtk DataFile Version 2.0\n");
//         fprintf (fp, "Really cool data\n");
//         fprintf (fp, "ASCII\n");
//         fprintf (fp, "DATASET UNSTRUCTURED_GRID\n");
//
//         tprintf("--> %d vertices\n", vertex_size());
//         fprintf (fp, "POINTS %d float\n", vertex_size());
//         for (int i = 0; i < vertex_size(); i++)
//             fprintf(fp, "%lf %lf %lf\n", get_vertex(i)->x(), get_vertex(i)->y(), get_vertex(i)->z());
//
//         tprintf("--> %d tetrahedra\n", count);
//         fprintf(fp, "CELLS %d %d\n", count, count*5);
//         for (int i = 0; i < tetra_size(); i++)
//             if (get_tetra(i)->getVol() == volume)
//                 fprintf(fp, "%d %d %d %d %d\n", 4, get_tetra(i)->getVertex(0)->getIndex(), get_tetra(i)->getVertex(1)->getIndex(), get_tetra(i)->getVertex(2)->getIndex(), get_tetra(i)->getVertex(3)->getIndex());
//
//         fprintf(fp, "CELL_TYPES %d\n", count);
//         for (int i = 0; i < count; i++)
//             fprintf(fp, "%d\n", 10);
//
//         fclose(fp);
//     }
// }

Bucket::Bucket ( LVertex min, LVertex max, double enlarge, int max_size )
{
  LVertex center ( ( min.x() +max.x() ) /2.0, ( min.y() +max.y() ) /2.0, ( min.z() +max.z() ) /2.0 );
  LVertex semidiag ( ( max.x()-min.x() ) /2.0, ( max.y()-min.y() ) /2.0, ( max.z()-min.z() ) /2.0 );

  _min.setPosition ( center.x() - semidiag.x() * ( 1.0+enlarge ), center.y() - semidiag.y() * ( 1.0+enlarge ), center.z() - semidiag.z() * ( 1.0+enlarge ) );
  _max.setPosition ( center.x() + semidiag.x() * ( 1.0+enlarge ), center.y() + semidiag.y() * ( 1.0+enlarge ), center.z() + semidiag.z() * ( 1.0+enlarge ) );

  double dmax = _max.x()-_min.x();
  if ( dmax < _max.y()-_min.y() )
    dmax = _max.y()-_min.y();
  if ( dmax < _max.z()-_min.z() )
    dmax = _max.z()-_min.z();

  _ref_size = dmax/ ( double ) max_size;

  _nx = floor ( ( _max.x()-_min.x() ) /_ref_size ) +1;
  _ny = floor ( ( _max.y()-_min.y() ) /_ref_size ) +1;
  _nz = floor ( ( _max.z()-_min.z() ) /_ref_size ) +1;

  _maxn = _nx;
  if ( _maxn < _ny )
    _maxn = _ny;
  if ( _maxn < _nz )
    _maxn = _nz;

  _min.setPosition ( center.x()- ( double ) _nx*_ref_size/2.0, center.y()- ( double ) _ny*_ref_size/2.0, center.z()- ( double ) _nz*_ref_size/2.0 );
  _max.setPosition ( center.x() + ( double ) _nx*_ref_size/2.0, center.y() + ( double ) _ny*_ref_size/2.0, center.z() + ( double ) _nz*_ref_size/2.0 );

  _bucket.resize ( _nx*_ny*_nz );
  _flag.resize ( _nx*_ny*_nz, 0 );
  _request_id = 0;
}

void Bucket::dump()
{
  char filename[100];
  sprintf ( filename, "%s", "bucket_dump.vtk" );
  printf ( "output file %s\n", filename );
  FILE *fp = fopen ( filename, "w" );

  fprintf ( fp, "# vtk DataFile Version 2.0\n" );
  fprintf ( fp, "Really cool data\n" );
  fprintf ( fp, "ASCII\n" );
  fprintf ( fp, "DATASET UNSTRUCTURED_GRID\n" );

  fprintf ( fp, "POINTS %d float\n", 8 );
  fprintf ( fp, "%lf %lf %lf\n", _min.x(), _min.y(), _min.z() );
  fprintf ( fp, "%lf %lf %lf\n", _max.x(), _min.y(), _min.z() );
  fprintf ( fp, "%lf %lf %lf\n", _max.x(), _max.y(), _min.z() );
  fprintf ( fp, "%lf %lf %lf\n", _min.x(), _max.y(), _min.z() );
  fprintf ( fp, "%lf %lf %lf\n", _min.x(), _min.y(), _max.z() );
  fprintf ( fp, "%lf %lf %lf\n", _max.x(), _min.y(), _max.z() );
  fprintf ( fp, "%lf %lf %lf\n", _max.x(), _max.y(), _max.z() );
  fprintf ( fp, "%lf %lf %lf\n", _min.x(), _max.y(), _max.z() );

  fprintf ( fp, "CELLS %d %d\n", 1, 9 );
  fprintf ( fp, "%d %d %d %d %d %d %d %d %d", 8, 0, 1, 2, 3, 4, 5, 6, 7 );

  fprintf ( fp, "CELL_TYPES %d\n", 1 );
  fprintf ( fp, "%d\n", 12 );

}

int Bucket::contain ( LVertex *pt )
{
  if ( pt->x() < _min.x() )
    return false;
  if ( pt->y() < _min.y() )
    return false;
  if ( pt->z() < _min.z() )
    return false;
  if ( pt->x() > _max.x() )
    return false;
  if ( pt->y() > _max.y() )
    return false;
  if ( pt->z() > _max.z() )
    return false;

  return true;
}

void Bucket::get_coord ( LVertex *pt, int &x, int &y, int &z )
{
  x = ( pt->x()-_min.x() ) /_ref_size;
  y = ( pt->y()-_min.y() ) /_ref_size;
  z = ( pt->z()-_min.z() ) /_ref_size;
}

int Bucket::get_index ( int x, int y, int z )
{
  return x + y*_nx + z*_nx*_ny;
}

void Bucket::add_point ( LVertex *pt )
{
  int x, y, z;
  get_coord ( pt, x, y, z );
  int index = get_index ( x, y, z );
  _bucket[index].push_back ( pt );
}

LVertex *Bucket::get_closest_in_bucket ( int index, LVertex *pt, double *sq_dist )
{
  LVertex *found = NULL;
  for ( int i = 0; i < _bucket[index].size(); i++ )
  {
    LVertex *pb = _bucket[index][i];
    LVertex ab;
    ab.setVector ( pt, pb );
    double d = ab.sq_norm();
    if ( d < *sq_dist )
    {
      found = pb;
      *sq_dist = d;
    }
  }
  _flag[index] = _request_id;
  return found;
}


LVertex *Bucket::get_closest ( LVertex *pt )
{
  LVertex *found = NULL;
  double dist = INFINITY;

  _request_id++;

  int x, y, z;
  get_coord ( pt, x, y, z );
  int index = get_index ( x, y, z );

  int level = 0;
  int level_stop = _maxn;
  LVertex *tmp;

  int imax;
  int imin;
  int jmax;
  int jmin;
  int kmax;
  int kmin;

  while ( level <= level_stop || ( ( level < 2 ) && ( level < _maxn ) ) )
  {
    if ( found )
      level_stop = floor ( sqrt ( dist ) /_ref_size ) + 1;

    imin = x-level;
    imax = x+level;
    jmin = y-level;
    jmax = y+level;
    kmin = z-level;
    kmax = z+level;
    if ( imin < 0 )
      imin = 0;
    if ( imax > _nx-1 )
      imax = _nx-1;
    if ( jmin < 0 )
      jmin = 0;
    if ( jmax > _ny-1 )
      jmax = _ny-1;
    if ( kmin < 0 )
      kmin = 0;
    if ( kmax > _nz-1 )
      kmax = _nz-1;

    for ( int k = kmin; k <= kmax; k++ )
      for ( int j = jmin; j <= jmax; j++ )
        for ( int i = imin; i <= imax; i++ )
        {
          index = get_index ( i, j, k );
          if ( _flag[index] != _request_id )
          {
            tmp = get_closest_in_bucket ( index, pt, &dist );
            if ( tmp != NULL )
              found = tmp;
          }
        }
    level++;
  }
  return found;
}