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

//              3___9___2
//             / `\    /´\
//            /    `\/´   \
//           /     / `\    \
//          /    5´    `\   7
//         6   /´        `8  \
//        /  /´            `\ \
//       / /´                `\\
//    0 /´________4____________`\1

// code / indice / nombre
static int code[64][3] =
{
  {0, 0, 0},
  {1, 24, 2},
  {2, 26, 2},
  {3, 0, 0},
  {4, 28, 2},
  {5, 0, 0},
  {6, 0, 0},
  {7, 0, 0},
  {8, 30, 2},
  {9, 0, 0},
  {10, 0, 0},
  {11, 20, 4},
  {12, 65, 4},
  {13, 0, 0},
  {14, 0, 0},
  {15, 0, 0},
  {16, 32, 2},
  {17, 0, 0},
  {18, 58, 4},
  {19, 0, 0},
  {20, 0, 0},
  {21, 16, 4},
  {22, 0, 0},
  {23, 0, 0},
  {24, 0, 0},
  {25, 0, 0},
  {26, 0, 0},
  {27, 0, 0},
  {28, 0, 0},
  {29, 0, 0},
  {30, 0, 0},
  {31, 0, 0},
  {32, 34, 2},
  {33, 48, 4},
  {34, 0, 0},
  {35, 0, 0},
  {36, 0, 0},
  {37, 0, 0},
  {38, 12, 4},
  {39, 0, 0},
  {40, 0, 0},
  {41, 0, 0},
  {42, 0, 0},
  {43, 0, 0},
  {44, 0, 0},
  {45, 0, 0},
  {46, 0, 0},
  {47, 0, 0},
  {48, 0, 0},
  {49, 0, 0},
  {50, 0, 0},
  {51, 0, 0},
  {52, 0, 0},
  {53, 0, 0},
  {54, 0, 0},
  {55, 0, 0},
  {56, 8, 4},
  {57, 0, 0},
  {58, 0, 0},
  {59, 0, 0},
  {60, 0, 0},
  {61, 0, 0},
  {62, 0, 0},
  {63, 0, 8},
};

static const int valid_code[] =
{
  1, 2, 4, 8, 16, 32, 11, 21, 38, 56, 12, 18, 33, 63
};


static const int idnt[][3][4] =
{
  // refine 1 --> 8 // code 63
  {{0, 4, 5, 6}, {0, 4, 5, 6}, {0, 4, 5, 6}},
  {{1, 7, 4, 8}, {1, 7, 4, 8}, {1, 7, 4, 8}},
  {{2, 5, 7, 9}, {2, 5, 7, 9}, {2, 5, 7, 9}},
  {{3, 6, 9, 8}, {3, 6, 9, 8}, {3, 6, 9, 8}},
  {{4, 6, 8, 9}, {5, 6, 4, 8}, {6, 8, 4, 7}},
  {{4, 8, 7, 9}, {5, 4, 7, 8}, {6, 4, 5, 7}},
  {{4, 7, 5, 9}, {5, 7, 9, 8}, {6, 5, 9, 7}},
  {{4, 5, 6, 9}, {5, 9, 6, 8}, {6, 9, 8, 7}},
  // refine 1 --> 4
  // face 0 // code 56
  {{1, 8, 7, 0}, {1, 8, 7, 0}, {1, 8, 7, 0}},
  {{2, 7, 9, 0}, {2, 7, 9, 0}, {2, 7, 9, 0}},
  {{3, 9, 8, 0}, {3, 9, 8, 0}, {3, 9, 8, 0}},
  {{7, 8, 9, 0}, {7, 8, 9, 0}, {7, 8, 9, 0}},
  // face 1 // code 38
  {{0, 5, 6, 1}, {0, 5, 6, 1}, {0, 5, 6, 1}},
  {{2, 9, 5, 1}, {2, 9, 5, 1}, {2, 9, 5, 1}},
  {{3, 6, 9, 1}, {3, 6, 9, 1}, {3, 6, 9, 1}},
  {{5, 9, 6, 1}, {5, 9, 6, 1}, {5, 9, 6, 1}},
  // face 2 // code 21
  {{0, 6, 4, 2}, {0, 6, 4, 2}, {0, 6, 4, 2}},
  {{1, 4, 8, 2}, {1, 4, 8, 2}, {1, 4, 8, 2}},
  {{3, 8, 6, 2}, {3, 8, 6, 2}, {3, 8, 6, 2}},
  {{6, 8, 4, 2}, {6, 8, 4, 2}, {6, 8, 4, 2}},
  // face 3 // code 11
  {{0, 4, 5, 3}, {0, 4, 5, 3}, {0, 4, 5, 3}},
  {{1, 7, 4, 3}, {1, 7, 4, 3}, {1, 7, 4, 3}},
  {{2, 5, 7, 3}, {2, 5, 7, 3}, {2, 5, 7, 3}},
  {{4, 7, 5, 3}, {4, 7, 5, 3}, {4, 7, 5, 3}},
  // refine 1 --> 2
  // arete 0 // code 1
  {{0, 4, 2, 3}, {0, 4, 2, 3}, {0, 4, 2, 3}}, // 24
  {{4, 1, 2, 3}, {4, 1, 2, 3}, {4, 1, 2, 3}},
  // arete 1 // code 2
  {{2, 5, 1, 3}, {2, 5, 1, 3}, {2, 5, 1, 3}},
  {{5, 0, 1, 3}, {5, 0, 1, 3}, {5, 0, 1, 3}},
  // arete 2 // code 4
  {{3, 6, 2, 1}, {3, 6, 2, 1}, {3, 6, 2, 1}},
  {{6, 0, 2, 1}, {6, 0, 2, 1}, {6, 0, 2, 1}},
  // arete 3 // code 8
  {{1, 7, 0, 3}, {1, 7, 0, 3}, {1, 7, 0, 3}},
  {{7, 2, 0, 3}, {7, 2, 0, 3}, {7, 2, 0, 3}},
  // arete 4 // code 16
  {{1, 8, 2, 0}, {1, 8, 2, 0}, {1, 8, 2, 0}},
  {{8, 3, 2, 0}, {8, 3, 2, 0}, {8, 3, 2, 0}},
  // arete 5  // code 32
  {{2, 9, 0, 1}, {2, 9, 0, 1}, {2, 9, 0, 1}}, // 34
  {{9, 3, 0, 1}, {9, 3, 0, 1}, {9, 3, 0, 1}},
  // non implemente pour l'instant
  // refine 1 --> 3
  // arete 0 1 // code 3
  {{0, 4, 5, 3}, {0, 4, 5, 3}, {0, 4, 5, 3}},
  {{4, 2, 5, 3}, {4, 2, 5, 3}, {4, 2, 5, 3}},
  {{4, 1, 2, 3}, {4, 1, 2, 3}, {4, 1, 2, 3}},
  // arete 0 2 // code 5
  {{0, 6, 4, 2}, {0, 6, 4, 2}, {0, 6, 4, 2}},
  {{6, 1, 4, 2}, {6, 1, 4, 2}, {6, 1, 4, 2}},
  {{6, 3, 1, 2}, {6, 3, 1, 2}, {6, 3, 1, 2}},
  // arete 0 3 // code 9
  {{1, 7, 4, 3}, {1, 7, 4, 3}, {1, 7, 4, 3}},  // 42
  {{7, 0, 4, 3}, {7, 0, 4, 3}, {7, 0, 4, 3}},
  {{7, 2, 1, 3}, {7, 2, 1, 3}, {7, 2, 1, 3}},
  // arete 0 4 // code 17
  {{1, 4, 8, 2}, {1, 4, 8, 2}, {1, 4, 8, 2}},
  {{4, 3, 8, 2}, {4, 3, 8, 2}, {4, 3, 8, 2}},
  {{4, 0, 3, 2}, {4, 0, 3, 2}, {4, 0, 3, 2}},
  // arete 0 5 // code 33
  {{0, 4, 2, 9}, {0, 4, 2, 9}, {0, 4, 2, 9}},
  {{0, 4, 9, 3}, {0, 4, 9, 3}, {0, 4, 9, 3}},
  {{4, 1, 2, 9}, {4, 1, 2, 9}, {4, 1, 2, 9}},
  {{4, 1, 9, 3}, {4, 1, 9, 3}, {4, 1, 9, 3}},
  // arete 1 2 // code 6
  {{0, 5, 6, 1}, {0, 5, 6, 1}, {0, 5, 6, 1}},  // 52
  {{5, 3, 6, 1}, {5, 3, 6, 1}, {5, 3, 6, 1}},
  {{5, 2, 3, 1}, {5, 2, 3, 1}, {5, 2, 3, 1}},
  // arete 1 3 // code 10
  {{2, 5, 7, 3}, {2, 5, 7, 3}, {2, 5, 7, 3}},
  {{5, 1, 7, 3}, {5, 1, 7, 3}, {5, 1, 7, 3}},
  {{5, 0, 1, 3}, {5, 0, 1, 3}, {5, 0, 1, 3}},
  // arete 1 4 // code 18
  {{2, 5, 1, 8}, {2, 5, 1, 8}, {2, 5, 1, 8}},
  {{2, 5, 8, 3}, {2, 5, 8, 3}, {2, 5, 8, 3}},
  {{5, 0, 1, 8}, {5, 0, 1, 8}, {5, 0, 1, 8}},
  {{5, 0, 8, 3}, {5, 0, 8, 3}, {5, 0, 8, 3}},
  // arete 1 5 // code 34
  {{2, 9, 5, 1}, {2, 9, 5, 1}, {2, 9, 5, 1}},  // 62
  {{9, 0, 5, 1}, {9, 0, 5, 1}, {9, 0, 5, 1}},
  {{9, 3, 0, 1}, {9, 3, 0, 1}, {9, 3, 0, 1}},
  // arete 2 3 // code 12
  {{1, 7, 0, 6}, {1, 7, 0, 6}, {1, 7, 0, 6}},
  {{1, 7, 6, 3}, {1, 7, 6, 3}, {1, 7, 6, 3}},
  {{7, 2, 0, 6}, {7, 2, 0, 6}, {7, 2, 0, 6}},
  {{7, 2, 6, 3}, {7, 2, 6, 3}, {7, 2, 6, 3}},
  // arete 2 4 // code 20
  {{3, 8, 6, 2}, {3, 8, 6, 2}, {3, 8, 6, 2}},
  {{8, 0, 6, 2}, {8, 0, 6, 2}, {8, 0, 6, 2}},
  {{8, 1, 0, 2}, {8, 1, 0, 2}, {8, 1, 0, 2}},
  // arete 2 5 // code 36
  {{3, 6, 9, 1}, {3, 6, 9, 1}, {3, 6, 9, 1}},  // 72
  {{6, 2, 9, 1}, {6, 2, 9, 1}, {6, 2, 9, 1}},
  {{6, 0, 2, 1}, {6, 0, 2, 1}, {6, 0, 2, 1}},
  // refine 1 --> 5
  // face 0 / arete 0
  {{1, 8, 7, 0}, {1, 8, 7, 0}, {1, 8, 7, 0}},
  {{2, 7, 9, 0}, {2, 7, 9, 0}, {2, 7, 9, 0}},
  {{3, 9, 8, 0}, {3, 9, 8, 0}, {3, 9, 8, 0}},
  {{7, 8, 9, 0}, {7, 8, 9, 0}, {7, 8, 9, 0}},
  // face 1
  {{0, 5, 6, 1}, {0, 5, 6, 1}, {0, 5, 6, 1}},
  {{2, 9, 5, 1}, {2, 9, 5, 1}, {2, 9, 5, 1}},
  {{3, 6, 9, 1}, {3, 6, 9, 1}, {3, 6, 9, 1}},
  {{5, 9, 6, 1}, {5, 9, 6, 1}, {5, 9, 6, 1}},
  // face 2
  {{0, 6, 4, 2}, {0, 6, 4, 2}, {0, 6, 4, 2}},
  {{1, 4, 8, 2}, {1, 4, 8, 2}, {1, 4, 8, 2}},
  {{3, 8, 6, 2}, {3, 8, 6, 2}, {3, 8, 6, 2}},
  {{6, 8, 4, 2}, {6, 8, 4, 2}, {6, 8, 4, 2}},
  // face 3
  {{0, 4, 6, 3}, {0, 4, 6, 3}, {0, 4, 6, 3}},
  {{1, 5, 4, 3}, {1, 5, 4, 3}, {1, 5, 4, 3}},
  {{2, 6, 5, 3}, {2, 6, 5, 3}, {2, 6, 5, 3}},
  {{4, 5, 6, 3}, {4, 5, 6, 3}, {4, 5, 6, 3}}
};

int Mesh::split ( LTetra *t )
{
  if ( t->getRefineField()->getState() == SPLITTED )
    return 0;

//     printf("split tetra %d\n", t->getIndex());

  int state = 0;

  for ( int i = 0; i < 6; i++ ) //calcul des nouveaux vertex si besoin
  {
    if ( !t->getRefineField()->edge_state ( i ) )
    {
      state = 1;
      LVertex *a = t->getEdgeVertex ( i, 0 );
      LVertex *b = t->getEdgeVertex ( i, 1 );
      LVertex *nv = new_vertex ( ( a->x() +b->x() ) /2.0, ( a->y() +b->y() ) /2.0, ( a->z() +b->z() ) /2.0 );
      add_vertex ( nv );
      _hes->addEdge ( t, i, nv );
      t->split_edge ( i );
      _sot->set_edge ( t, i );
      _sot->build();
      for ( int j = 0; j < _sot->size(); j++ )
      {
//    printf("_sot size = %d\n", _sot->size());
        LTetra *tt = NULL;
        int ee;
        _sot->get_tetra ( j, &tt, &ee );

        if ( !tt->getRefineField()->edge_state ( ee ) )
        {
//        LTetra *pp = tt->getFather();
//        if (pp != NULL && pp->edge_code() != FULL_SPLIT)
//           _storage_residual_stack[pp->getGen()].push(pp);
          tt->split_edge ( ee );
        }
      }
    }
    assert ( t->getRefineField()->edge_state ( i ) );
  }

  t->getRefineField()->setState ( SPLITTED );
  return state;
}

void Mesh::collect_neighbors ( LTetra *t )
{
  for ( int i = 0; i < 6; i++ )
  {
    _sot->set_edge ( t, i );
    _sot->build();
    for ( int j = 0; j < _sot->size(); j++ )
    {
      LTetra *tt = NULL;
      int ee;
      _sot->get_tetra ( j, &tt, &ee );

//      printf("flag tt = %d, t index = %d\n", tt->getFlag(), t->getIndex());
      if ( tt->getFlag() != t->getIndex() )
      {
        tt->setFlag ( t->getIndex() );
        if ( tt->getRefineField()->getState() != SPLITTED )
        {
//        printf("adding tt (%d) --> flag tt = %d, t index = %d\n", tt->getRefineField()->getState(), tt->getFlag(), t->getIndex());
          _conforming_stack.push ( tt );
        }
      }
    }
  }
}

int Mesh::valid_scheme ( LTetra *t )
{
  int ecode = t->getRefineField()->edge_code();
  if ( ecode != valid_code[0] && ecode != valid_code[1] && ecode != valid_code[2]
       && ecode != valid_code[3] && ecode != valid_code[4] && ecode != valid_code[5]
       && ecode != valid_code[6] && ecode != valid_code[7] && ecode != valid_code[8]
       && ecode != valid_code[9] && ecode != valid_code[10] && ecode != valid_code[11]
       && ecode != valid_code[12] && ecode != valid_code[13]
     )
    return 0;
  else
    return 1;
}

void Mesh::refining()
{
  if ( _opts->debug )
  {
    tprintf ( "--> split edges\n" );
    tprintf ( "refining stack --> %d\n", ( int ) _refining_stack.size() );
  }

  while ( !_refining_stack.empty() )
  {
    LTetra *t = _refining_stack.front();
    _refining_stack.pop();
    split ( t );
    _refined_stack.push ( t );
  }

  if ( _opts->debug )
  {
    tprintf ( "--> collect neighbours\n" );
    tprintf ( "refined stack --> %d\n", ( int ) _refined_stack.size() );
  }

  while ( !_refined_stack.empty() )
  {
    LTetra *t = _refined_stack.front();
    _refined_stack.pop();
    collect_neighbors ( t );
  }

  if ( _opts->debug )
  {
    tprintf ( "--> conforming stack size %d\n", ( int ) _conforming_stack.size() );
  }

  while ( !_conforming_stack.empty() )
  {
    if ( _opts->debug )
    {
      tprintf ( "--> split edges\n" );
    }

    while ( !_conforming_stack.empty() )
    {
      LTetra *t = _conforming_stack.front();
      _conforming_stack.pop();
//      printf("tetra %d --> edge code %d\n", t->getIndex(), t->getRefineField()->edge_code());
      if ( t->getRefineField()->edge_code() != SPLITTED && !valid_scheme ( t ) )
      {
        split ( t );
        _refined_stack.push ( t );
      }
    }

    if ( _opts->debug )
    {
      tprintf ( "--> collect neighbours\n" );
      tprintf ( "refined stack --> %d\n", ( int ) _refined_stack.size() );
    }

    while ( !_refined_stack.empty() )
    {
      LTetra *t = _refined_stack.front();
      _refined_stack.pop();
      collect_neighbors ( t );
    }

    if ( _opts->debug )
    {
      tprintf ( "--> conforming stack size %d\n", ( int ) _conforming_stack.size() );
    }
  }

  if ( _opts->debug )
  {
    tprintf ( "--> subdivide\n" );
  }

  for ( int i = 0; i < tetra_size(); i++ )  // boucle sur tout les tetraedres
  {
    LTetra *t = get_tetra ( i );
    if ( t->getRefineField()->getState() != REFINED )
      subdivide ( t );
  }
  compute_refinement_adjacencies();
}


void Mesh::subdivide ( LTetra *t )
{
  int ecode = t->getRefineField()->edge_code();

//   printf("tetra %d --> code %d\n", t->getIndex(), ecode);

  int index = code[ecode][1];
  int nbt = code[ecode][2];
  int conf = 0;

  assert ( ecode == 0 || nbt != 0 );

  if ( nbt != 0 )
  {
    LVertex *nv[10];
    nv[0] = t->getVertex ( 0 );
    nv[1] = t->getVertex ( 1 );
    nv[2] = t->getVertex ( 2 );
    nv[3] = t->getVertex ( 3 );
    nv[4] = NULL;
    nv[5] = NULL;
    nv[6] = NULL;
    nv[7] = NULL;
    nv[8] = NULL;
    nv[9] = NULL;

    for ( int i = 0; i < 6; i++ ) //calcul des nouveaux vertex si besoin
    {
      if ( t->getRefineField()->edge_state ( i ) )
      {
        hashEdgeKey *k1 = new hashEdgeKey ( t, i, NULL );
        hashEdgeKey *k2 = _hes->query ( k1 );
        _hes->unlock_query();
        assert ( k2 != NULL );
        delete k1;

        LVertex *m = k2->middle();
        assert ( m != NULL );
        nv[4+i] = m;
      }
    }

    if ( ecode == FULL_SPLIT )
    {
      LVertex ab;
      double dl = INFINITY;
      for ( int i = 0; i < 3; i++ )
      {
        ab.setVector ( nv[4+i], nv[9-i] );
        double tmp = ab.sq_norm();
        if ( tmp < dl )
        {
          conf = i;
          dl = tmp;
        }
      }
    }

    for ( int i = 0; i < nbt; i++ )
    {
      int ii = index+i;
      LTetra * nt = new_tetra ( nv[idnt[ii][conf][0]], nv[idnt[ii][conf][1]], nv[idnt[ii][conf][2]], nv[idnt[ii][conf][3]] );
      nt->addRefineField();
      add_tetra ( nt );
      t->getRefineField()->setChild ( i, nt );
      nt->getRefineField()->setFather ( t );
//      if (nt->getRefineField()->getRefDepth()<0)
      nt->getRefineField()->setRefDepth ( 0 );
      _new_tetra_stack.push ( nt );
    }
    t->getRefineField()->setState ( REFINED );
    _father_stack.push ( t );

    for ( int i = 0; i < t->getRefineField()->inc_size(); i++ )
    {
      LVertex *v, *n;
      t->getRefineField()->getInc ( i, v, n );
      for ( int j = 0; j < 8; j++ )
      {
        LTetra *c = t->getRefineField()->getChild ( j );
        if ( c == NULL )
          break;
        if ( c->contain ( v ) )
        {
          c->getRefineField()->addInc ( v, n );
          break;
        }
      }
    }
  }
}

void Mesh::compute_refinement_adjacencies()
{
  if ( _opts->debug )
  {
    tprintf ( "--> compute adjacencies\n" );
    tprintf ( "%d child tetras\n", ( int ) _new_tetra_stack.size() );
  }
  _hfs->clear();
  while ( !_new_tetra_stack.empty() )
  {
    LTetra *t = _new_tetra_stack.front();
    _new_tetra_stack.pop();
    for ( int i = 0; i < 4; i++ )
      _hfs->addAdjFace ( t, i );
  }

  while ( !_father_stack.empty() )
  {
    LTetra *t = _father_stack.front();
    _father_stack.pop();
    for ( int i = 0; i < 4; i++ )
    {
      LTetra *adj = t->getAdj ( i );
      int opp = t->getOpp ( i );
      if ( adj != NULL && adj->getRefineField()->getState() == NOT_REFINED )
        _hfs->addAdjFace ( adj, opp );
    }
  }

//     check_adjacencies();
}

void Mesh::check_adjacencies()
{
  int count = 0;
  for ( int i = 0; i < tetra_size(); i++ )  // boucle sur tout les tetraedres
  {
    LTetra *t = get_tetra ( i );
    if ( t->getRefineField()->getState() == NOT_REFINED )
    {
      if ( t->getRefineField()->getFather() != NULL )
        count++;
      for ( int i = 0; i < 4; i++ )
      {
        LTetra *adj = t->getAdj ( i );
        int opp = t->getOpp ( i );
        if ( t->getAdj ( i ) != NULL )
        {
//        assert(adj->getRefineField()->getState() != REFINED);
          assert ( adj->getAdj ( opp ) == t );
        }
      }
    }
  }
  printf ( "%d child tetras\n", count );
}


void Mesh::equilibrate_refinement()
{
  tprintf ( "--> equilibrate refinement\n" );

  std::queue<LTetra *> refine_stack;
  std::queue<LTetra *> neigh_stack;

  for ( int i = 0; i < tetra_size(); i++ )  // boucle sur tout les tetraedres
  {
    LTetra *t = get_tetra ( i );
    if ( t->getRefineField()->getRefDepth() > 0 )
      refine_stack.push ( t );
  }

  while ( !refine_stack.empty() )  // boucle sur tous les tetraedres
  {
    LTetra *t = refine_stack.front();
    // on verifie chacun des tetraedres
    if ( t->getRefineField()->getRefDepth() > 0 )
    {
      for ( int i = 0; i < 6; i++ )
      {
        _sot->set_edge ( t, i );
        _sot->build();
        for ( int j = 0; j < _sot->size(); j++ )
        {
          LTetra *tt = NULL;
          int ee;
          _sot->get_tetra ( j, &tt, &ee );

          if ( tt->getRefineField()->getRefDepth() < t->getRefineField()->getRefDepth()-1 )
          {
            tt->getRefineField()->setRefDepth ( t->getRefineField()->getRefDepth()-1 );
            assert ( tt->getRefineField()->getRefDepth() >= 0 );
            refine_stack.push ( tt );
          }
        }
      }
//             for (int i = 0; i < 4; i++)
//             {
//                 _bot->set_vertex(t->getVertex(i));
//                 _bot->build();
//                 for (int j = 0; j < _bot->size(); j++ )
//                 {
//                     LTetra *tt = NULL;
//                     int ee;
//                     _bot->get_tetra(j, &tt, &ee);
//
//                     if (tt->getRefineField()->getRefDepth() < t->getRefineField()->getRefDepth()-1)
//                     {
//                         tt->getRefineField()->setRefDepth(t->getRefineField()->getRefDepth()-1);
//                         assert(tt->getRefineField()->getRefDepth() >= 0);
//                         refine_stack.push(tt);
//                     }
//                 }
//      }
    }
    refine_stack.pop();
  }
}

void Mesh::init_stack()
{
  assert ( _refining_stack.empty() );
  for ( int i = 0; i < tetra_size(); i++ )  // boucle sur tout les tetraedres
  {
    LTetra *t = get_tetra ( i );
//  printf("tetra %d --> state %d -- depth %d\n", t->getIndex(), t->getRefineField()->getState(), t->getRefineField()->getRefDepth());
    if ( t->getRefineField()->getState() == NOT_REFINED && t->getRefineField()->getRefDepth() > 0 )
    {
//      printf("add tetra %d\n", t->getIndex());
      _refining_stack.push ( t );
    }
  }
}

void Mesh::refine_triangulation()
{
  equilibrate_refinement();
  writeMeshVTK ( "before_ref" );

  init_stack();
  refining();
  writeMeshVTK ( "after_ref" );
}

int Mesh::normalize_curv ( double rcurv, double seglenght, double factor )
{
//     double alpha = acos(seglenght/(2.0*rcurv));
//     double arrow = rcurv - sin(alpha)*rcurv;

  double ideal_lenght = 2.0*sqrt ( ( 2.0*rcurv - factor*rcurv ) *factor*rcurv );

  double nb_sub = seglenght/ideal_lenght;

  int ref = ( int ) floor ( sqrt ( nb_sub ) );

  return ref;
}

double Mesh::evaluate_refinement_params()
{
  double max_curv_mesh = 0.0;
  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    if ( t->getRefineField()->getState() == NOT_REFINED )
    {
      for ( int j = 0; j < t->getRefineField()->inc_size(); j++ )
      {
        LVertex *v, *n;
        t->getRefineField()->getInc ( j, v, n );
//    printf("curv --> %lf\n", v->getCurvature());
        if ( v->getCurvature() > max_curv_mesh )
          max_curv_mesh = v->getCurvature();
      }
    }
  }

  double max_lenght = longuest_edge();
  double max_lenght_refined = max_lenght/pow ( 2.0, _opts->max_refinement_level );

  tprintf ( "min radius over mesh --> %lf\n", 1.0/max_curv_mesh );
  tprintf ( "longuest --> %lf\n", longuest_edge() );
  tprintf ( "refined longuest --> %lf\n", max_lenght_refined );

  double min_radius_allowed = sqrt ( ( max_lenght_refined*max_lenght_refined ) / ( 8.0*_opts->refinement_error_threshold-4.0*_opts->refinement_error_threshold*_opts->refinement_error_threshold ) );

  tprintf ( "min radius allowed --> %lf\n", min_radius_allowed );

  double max_curv_allowed = 1.0/min_radius_allowed;

  tprintf ( "max curvature allowed --> %lf\n", max_curv_allowed );

  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    if ( t->getRefineField()->getState() == NOT_REFINED )
    {
      for ( int j = 0; j < t->getRefineField()->inc_size(); j++ )
      {
        LVertex *v, *n;
        t->getRefineField()->getInc ( j, v, n );
        if ( v->getCurvature() > max_curv_allowed )
          v->setCurvature ( max_curv_allowed );
      }
    }
  }
  return max_curv_allowed;
}

int Mesh::adapt ( double max_curv_allowed )
{
  tprintf ( "--> adapting tetras\n" );

  int count = 0;

  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    if ( t->getRefineField()->getState() == NOT_REFINED )
    {
      double max_curv_tetra = 0.0;
      for ( int j = 0; j < t->getRefineField()->inc_size(); j++ )
      {
        LVertex *v, *n;
        t->getRefineField()->getInc ( j, v, n );
        if ( v->getCurvature() > max_curv_tetra )
          max_curv_tetra = v->getCurvature();
      }

      if ( max_curv_tetra > max_curv_allowed )
        max_curv_tetra = max_curv_allowed;

//      printf("max_curv_tetra --> %lf %lf %lf\n", max_curv_tetra, t->getRefineField()->getlenght(), _opts->refinement_error_threshold);

      if ( max_curv_tetra > 0.0 )
      {
        int nrf = normalize_curv ( 1.0/max_curv_tetra, t->getRefineField()->getlenght(), _opts->refinement_error_threshold );

        if ( t->getRefineField()->getRefDepth() < nrf )
        {
          t->getRefineField()->setRefDepth ( nrf );
          count++;
        }
      }
    }
  }
  tprintf ( "--> adapt = %d\n",count );
  return count;
}

// TODO clean memory
void Mesh::renumber()
{
  int count = 0;
  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    if ( t->getRefineField()->getState() == NOT_REFINED )
    {
      t->setIndex ( count++ );
      t->getRefineField()->resetFather();
    }
    else
    {
      delete t->getRefineField();
      delete t;
      _t[i] = NULL;
    }
  }

  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    if ( t && t->getRefineField()->getState() == NOT_REFINED )
      _t[t->getIndex() ] = t;
  }

  set_tetra_size ( count );

  for ( int i = 0; i < tetra_size(); i++ )
  {
    LTetra *t = get_tetra ( i );
    delete t->getRefineField();
  }
}