/*
    <one line to give the program's name and a brief idea of what it does.>
    Copyright (C) <year>  <name of author>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
#include <string.h>

#include "cadlevelset.h"
#include "simplex.h"
#include "GModelIO_OCC.h"
#include "OCCFace.h"
#include "OCCEdge.h"

void cadLevelset::readMeshFile()
{
    _m->read(_basename);
}

void cadLevelset::readInputFile(char *name)
{
  std::string filename = name ;
  std::string::size_type idx = filename.find('.');
  sprintf(_basename, "%s", filename.substr(0, idx).c_str());
  std::string extension = filename.substr(idx+1);
  
  printf("--> base = %s\n", _basename);
  printf("--> extension = %s\n", extension.c_str());
  
  if ( !extension.compare("step") )
  {
    _fileType=0;
    readStepFile();
  }
  else if ( !extension.compare("lsn") )
    {
        _fileType=1;
#if 1
        readLsnFile();
#else
	Plane *pl = new Plane(0.0, 0.0, 1.0, -3.0);
        Surface *surf = new Surface(0, pl);
        surf->generate();
// 	  surf->dump(0);
        surf->bucketize(_m->get_longuest());
        _lsurf.push_back(surf);
	pl = new Plane(0.0, 0.0, 1.0, 3.0);
        surf = new Surface(1, pl);
        surf->generate();
// 	  surf->dump(0);
        surf->bucketize(_m->get_longuest());
        _lsurf.push_back(surf);
	
	pl = new Plane(0.0, 1.0, 0.0, 0.0);
        surf = new Surface(2, pl);
        surf->generate();
// 	  surf->dump(0);
        surf->bucketize(_m->get_longuest());
        _lsurf.push_back(surf);
	pl = new Plane(0.0, 1.0, 0.0, 10.0);
        surf = new Surface(3, pl);
        surf->generate();
// 	  surf->dump(0);
        surf->bucketize(_m->get_longuest());
        _lsurf.push_back(surf);
	
	pl = new Plane(1.0, 0.0, 0.0, 0.0);
        surf = new Surface(4, pl);
        surf->generate();
// 	  surf->dump(0);
        surf->bucketize(_m->get_longuest());
        _lsurf.push_back(surf);
	pl = new Plane(1.0, 0.0, 0.0, -20.0);
        surf = new Surface(5, pl);
        surf->generate();
// 	  surf->dump(0);
        surf->bucketize(_m->get_longuest());
        _lsurf.push_back(surf);
#endif
  }
  else
  {
    tprintf("Input file extension error.\n");
  }
//   printf("lsurf size --> %d\n", (int)_lsurf.size());
}

void cadLevelset::readStepFile()
{
//    OCC_Internals *_occ = new OCC_Internals;
    _pModel = new GModel();
    char filename[100];
    sprintf(filename, "%s.step", _basename);
//    _occ->loadSTEP(filename);
//    _occ->buildGModel(_pModel);
    _pModel->readOCCSTEP(filename);
    tprintf("CAO model summary :\n");
    tprintf("--> %d vertices\n", _pModel->getNumVertices());
    tprintf("--> %d edges\n", _pModel->getNumEdges());
    tprintf("--> %d faces\n", _pModel->getNumFaces());
    tprintf("--> %d regions\n", _pModel->getNumRegions());
}

void cadLevelset::readLsnFile()
{
  int error = 0;
  char filename[100];
  sprintf(filename, "%s.lsn", _basename);
  tprintf("Reading Lsn file %s ....\n", filename);
  FILE *fp = fopen(filename, "r");

  if (fp==NULL)
    error++;

  int i,nb_ls=0;
  if ( fscanf(fp, "%d", &nb_ls) != 1 )
    error++;

  for (i = 0; i < nb_ls; i++)
  {
    DiscreteSurface *surf = new DiscreteSurface(fp, _lsurf.size());
    _lsurf.push_back(surf);
  }
  fclose ( fp );

  int nb_nodes=0;
  tprintf("Discrete LS summary :\n");
  tprintf("--> %d level-sets,\n", nb_ls);
  for (i = 0; i < nb_ls; i++)
  {
    tprintf("--> lsn[%d] contains %d nodes,\n", i, _lsurf[i]->size());
    nb_nodes += _lsurf[i]->size();
  }
  tprintf("--> total : %d nodes.\n", nb_nodes);
}

// void cadLevelset::readMeshFile(char *name)
// {
// //     delete _pModel;
// //     _pModel->destroy();
//     _pModel = new GModel();
//     _pModel->readMSH(name);
// //     _pModel->writeMSH("test_init.msh",2.2,false,false);
// //     computeDiscreteLevelset();
// }



// hashFaceKey::hashFaceKey(Tetra *t, int face)
// {
//     assert(t != NULL);
//     assert(face>=0 && face<4);
// 
//     _t = t;
//     _f = face;
//     _key = t->getFaceVertex(face, 0)->getIndex() + t->getFaceVertex(face, 1)->getIndex() + t->getFaceVertex(face, 2)->getIndex();
// 
//     min = t->getFaceVertex(face, 0)->getIndex();
//     if (t->getFaceVertex(face, 1)->getIndex() < min)
//         min = t->getFaceVertex(face, 1)->getIndex();
//     if (t->getFaceVertex(face, 2)->getIndex() < min)
//         min = t->getFaceVertex(face, 2)->getIndex();
// 
//     max = t->getFaceVertex(face, 0)->getIndex();
//     if (t->getFaceVertex(face, 1)->getIndex() > max)
//         max = t->getFaceVertex(face, 1)->getIndex();
//     if (t->getFaceVertex(face, 2)->getIndex() > max)
//         max = t->getFaceVertex(face, 2)->getIndex();
//     _id = -1;
// }
// 
// void hashFaceStruct::addFace(Tetra *t, int face)
// {
//     assert(t != NULL);
//     assert(face>=0 && face<4);
// 
//     hashFaceKey *k1 = new hashFaceKey(t, face);
//     std::multimap<int, hashFaceKey*>::iterator it;
//     std::pair<std::multimap<int, hashFaceKey*>::iterator,std::multimap<int, hashFaceKey*>::iterator> ret;
//     ret = table.equal_range(k1->key());
//     int found = 0;
//     for (it=ret.first; it!=ret.second; ++it)
//     {
//         hashFaceKey *k2 = it->second;
//         if (k2->equal(k1))
//         {
//             k1->tetra()->setMutualAdj(k1->face(), k2->tetra(), k2->face());
//             table.erase (it);
//             delete k1;
//             delete k2;
//             found = 1;
//             break;
//         }
//     }
//     if (!found)
//         table.insert(std::pair<int, hashFaceKey*>(k1->key(), k1));
// }

// void Mesh::create(GModel *gm, int max_step, double rel_enlarge)
void Mesh::create(Vertex *min, Vertex *max, int max_step, double rel_enlarge)
{
    tprintf("Building mesh START\n");

    Vertex *_min = new Vertex;
    Vertex *_max = new Vertex;

    double dl[3];
    dl[0] = max->x() - min->x();
    dl[0] *= rel_enlarge;
    dl[1] = max->y() - min->y();
    dl[1] *= rel_enlarge;
    dl[2] = max->z() - min->z();
    dl[2] *= rel_enlarge;
    _min->setPosition(min->x() - dl[0], min->y() - dl[1], min->z() - dl[2]);
    _max->setPosition(max->x() + dl[0], max->y() + dl[1], max->z() + dl[2]);

    tprintf("Bounds : %lf %lf %lf -- %lf %lf %lf\n", min->x(), min->y(), min->z(), max->x(), max->y(), max->z());

    double step = _max->x() - _min->x();
    if (_max->y() - _min->y() > step)
        step = _max->y() - _min->y();
    if (_max->z() - _min->z() > step)
        step = _max->z() - _min->z();

    step /= (double)max_step;

    int nx = (int)((_max->x() - _min->x()) / step);
    int ny = (int)((_max->y() - _min->y()) / step);
    int nz = (int)((_max->z() - _min->z()) / step);

    double dx = (_max->x() - _min->x()) / (double)nx;
    double dy = (_max->y() - _min->y()) / (double)ny;
    double dz = (_max->z() - _min->z()) / (double)nz;

    tprintf("Mesh : %d (%lf) %d (%lf) %d (%lf)\n", nx, dx, ny, dy, nz, dz);

    set_vertex_size((nx+1)*(ny+1)*(nz+1));   

    for (int k = 0; k < nz+1; k++)
        for (int j = 0; j < ny+1; j++)
            for (int i = 0; i < nx+1; i++)
            {
                int index = i+j*(nx+1)+k*(nx+1)*(ny+1);
                Vertex *v = new_vertex(index, _min->x()+(double)i*dx, _min->y()+(double)j*dy, _min->z()+(double)k*dz);
// 	  printf("index = %d --> %p\n", index, v);
            }

    static const int tt[6][4] = { {0, 1, 2, 6}, {0, 5, 6, 4}, {0, 1, 6, 5}, {1, 7, 6, 5}, {1, 3, 2, 7}, {1, 2, 6, 7} };
    static const int pp[8][3] = { {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {1, 1, 0}, {0, 0, 1}, {1, 0, 1}, {0, 1, 1}, {1, 1, 1} };

    progress_bar pbar;
    pbar.start(nx*ny*nz);
    set_tetra_size(6*nx*ny*nz);

    hashFaceStruct *hfs = new hashFaceStruct;

    for (int k = 0; k < nz; k++)
        for (int j = 0; j < ny; j++)
            for (int i = 0; i < nx; i++)
	    {
	      pbar.progress(i+j*nx+k*nx*ny);
                for (int tet = 0; tet < 6; tet++)
                {
                    int num;
                    num = i+pp[tt[tet][0]][0] + (j+pp[tt[tet][0]][1])*(nx+1) + (k+pp[tt[tet][0]][2])*(nx+1)*(ny+1);
// 		    printf("%d %d %d %d\n", i, j, k, tet);
                    Vertex *e0 = get_vertex(num);
                    num = i+pp[tt[tet][1]][0] + (j+pp[tt[tet][1]][1])*(nx+1) + (k+pp[tt[tet][1]][2])*(nx+1)*(ny+1);
// 		    printf("%d %d %d %d\n", i, j, k, tet);
                    Vertex *e1 = get_vertex(num);
                    num = i+pp[tt[tet][2]][0] + (j+pp[tt[tet][2]][1])*(nx+1) + (k+pp[tt[tet][2]][2])*(nx+1)*(ny+1);
// 		    printf("%d %d %d %d\n", i, j, k, tet);
                    Vertex *e2 = get_vertex(num);
                    num = i+pp[tt[tet][3]][0] + (j+pp[tt[tet][3]][1])*(nx+1) + (k+pp[tt[tet][3]][2])*(nx+1)*(ny+1);
// 		    printf("%d %d %d %d\n", i, j, k, tet);
                    Vertex *e3 = get_vertex(num);
                    int index = tet+6*i+j*6*nx+k*6*nx*ny;
                    Tetra *t = new_tetra(index, e0, e1, e2, e3);
                    for (int ii = 0; ii < 4; ii++)
                        hfs->addFace(t, ii);
                }
	    }

//     _bm = new Bucket(*_min, *_max, 0.05, 50);
//     for (int i = 0; i < vertex_size(); i++)
//         _bm->add_point(get_vertex(i));
    set_longuest(get_tetra(0)->getVertex(1), get_tetra(0)->getVertex(3));
    
    pbar.end();
    tprintf("Mesh : %d cells, %d vertices\n", tetra_size(), vertex_size());
    tprintf("Building mesh END\n");
}

void Mesh::read(char *basename)
{
  tprintf("Reading mesh START\n");
  int error = 0;
      char filename[100];
    sprintf(filename, "%s.mesh", basename);
    tprintf("Reading %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;
    double min[3], max[3];
    for (int i = 0; i < nb_vx; i++)
    {
        if (fscanf(fp, "%f %f %f", &x, &y, &z) != 3)
            error++;
        Vertex *v = new_vertex(i, x,y,z);

        if (min[0] > x)
            min[0] = x;
        if (min[1] > y)
            min[1] = y;
        if (min[2] > z)
            min[2] = z;
        if (max[0] < x)
            max[0] = x;
        if (max[1] < y)
            max[1] = y;
        if (max[2] < z)
            max[2] = z;
// 	printf("%lf %lf %lf\n", x, y, z);
    }
    
    _min = new Vertex;
    _min->setPosition(min[0], min[1], min[2]);
    _max = new Vertex;
    _max->setPosition(max[0], max[1], max[2]);
    
    int nb_tet;
    hashFaceStruct *hfs = new hashFaceStruct;
    
    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++;
        Tetra *t = new_tetra(i, get_vertex(v0), get_vertex(v1), get_vertex(v2), get_vertex(v3));
        for (int j = 0; j < 6; j++)
            set_longuest(t->getEdgeVertex(j, 0), t->getEdgeVertex(j, 1));
        for (int j = 0; j < 4; j++)
            hfs->addFace(t, j);
    }
    fclose(fp);
    
    tprintf("Mesh : %d cells, %d vertices\n", tetra_size(), vertex_size());
    tprintf("Reading mesh END\n");
}

void Mesh::write(char *name)
{
    char filename[100];
    sprintf(filename, "%s.mesh", name);
    _mf = fopen(filename, "w");
    fprintf(_mf, "%d\n", vertex_size());
    for (int i = 0; i < vertex_size(); i++)
    {
        Vertex *v = get_vertex(i);
        fprintf(_mf, "%lf %lf %lf\n", v->x(), v->y(), v->z());
    }
    
    int count = 0;
    for (int i = 0; i < tetra_size(); i++)
    {
      if (get_tetra(i)->getState() == NOT_REFINED)
	count++;
    }
    
    fprintf(_mf, "%d\n", count);
    for (int i = 0; i < tetra_size(); i++)
    {
        Tetra *t = get_tetra(i);
	if (t->getState() == NOT_REFINED)
          fprintf(_mf, "%d %d %d %d\n", t->getVertex(0)->getIndex(), t->getVertex(1)->getIndex(), t->getVertex(2)->getIndex(), t->getVertex(3)->getIndex());
    }
    fclose(_mf);
}

int computeBaryCoordTri(SPoint3 p1, SPoint3 p2, SPoint3 p3, SPoint3 e1, SPoint3 e2, SPoint3 &c, double b[3])
{
    // compute plane
    SVector3 uu(p1, p2);
    SVector3 vv(p1, p3);
    SVector3 pl = crossprod(uu, vv);
    pl.normalize();
    double dd = dot(pl, p1);
    double d1 = dot(pl, e1) - dd;
    SVector3 ee(e1, e2);
    double d2 = dot(pl, ee);
    double t = d1/d2;
    printf("d1 = %lf, d2 = %lf, t = %lf\n", d1, d2, t);
    if (t < 0.0 || t > 1.0)
        return 0;
    c.setPosition(e1.x() + t*ee.x(), e1.y() + t*ee.y(), e1.z() + t*ee.z());
    printf("c : %lf %lf %lf\n", c.x(), c.y(), c.z());
    // compute barycenter coordinates
    double tot = pl.norm();
    if (tot == 0.0)
        return 0;
    b[0] = crossprod(p3-p2, c-p2).norm()/tot;
    b[1] = crossprod(p1-p3, c-p3).norm()/tot;
    b[2] = crossprod(p2-p1, c-p1).norm()/tot;
    printf("%lf %lf %lf\n", b[0], b[1], b[2]);
    return 1;
}

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

int Tetra::barycentric(Vertex *c, double b[4])
{
    Vertex *p1 = getVertex(0);
    Vertex *p2 = getVertex(1);
    Vertex *p3 = getVertex(2);
    Vertex *p4 = getVertex(3);
    Vertex uu, vv, ww, pl;
    uu.setVector(p1, p2);
    vv.setVector(p1, p3);
    ww.setVector(p1, p4);
    uu.crossproduct(&vv, &pl);
    double tot = pl.dotproduct(&ww);
//     printf("tot --> %lf\n", tot);
    if (tot == 0.0)
        return 0;
    uu.setVector(p2, p4);
    vv.setVector(p2, p3);
    ww.setVector(p2, c);
    uu.crossproduct(&vv, &pl);
    b[0] = pl.dotproduct(&ww)/tot;
    uu.setVector(p3, p4);
    vv.setVector(p3, p1);
    ww.setVector(p3, c);
    uu.crossproduct(&vv, &pl);
    b[1] = pl.dotproduct(&ww)/tot;
    uu.setVector(p1, p4);
    vv.setVector(p1, p2);
    ww.setVector(p1, c);
    uu.crossproduct(&vv, &pl);
    b[2] = pl.dotproduct(&ww)/tot;
    uu.setVector(p1, p2);
    vv.setVector(p1, p3);
    ww.setVector(p1, c);
    uu.crossproduct(&vv, &pl);
    b[3] = pl.dotproduct(&ww)/tot;
//     printf("%lf %lf %lf %lf\n", b[0], b[1], b[2], b[3]);
    return 1;
}

int Tetra::contain(Vertex *v)
{
    static double b[4] = {INFINITY, INFINITY, INFINITY, INFINITY};
//     if (b[0] == INFINITY)
      barycentric(v, b);
//     printf("%lf %lf %lf %lf\n", b[0], b[1], b[2], b[3]);
    for (int i = 0; i < 4; i++)
        if (b[i] < -THRESHOLD_FACE)
            return 0;
    return 1;
}

/*int Tetra::visibility(int face, Vertex *v0, Vertex *v1, double b[4], Vertex **inter, int *inside, int *vertex, int *edge)
{
    static int vt[3][3] = {{-1, 2, 1},{2, -1, 0},{1, 0, -1}};
    int idfv[3];
    Vertex *p1 = getFaceVertex(face, 0);
    Vertex *p2 = getFaceVertex(face, 1);
    Vertex *p3 = getFaceVertex(face, 2);
    Tetra t(p1, p2, p3, v0);
    idfv[0] = getFaceVertexIndex(face, 0);
    idfv[1] = getFaceVertexIndex(face, 1);
    idfv[2] = getFaceVertexIndex(face, 2);
    b[0] = b[1] = b[2] = b[3] = INFINITY;
    (*inter) = NULL;
    (*inside) = 0;
    (*vertex) = -1;
    (*edge) = -1;
    t.barycentric(v1, b);
//     printf("v --> %lf %lf %lf --> %lf %lf %lf %lf\n", v0->x(), v0->y(), v0->z(), b[0], b[1], b[2], b[3] );
    int visible = 0;
    int nz = 0;
    int zero[3];
    for (int i = 0; i < 3; i++)
    {
        if (b[i] > -THRESHOLD_FACE)
            visible++;
        if (fabs(b[i]) < THRESHOLD_FACE)
            zero[nz++] = i;
    }
    if ((visible == 3) && (b[3] > -THRESHOLD_FACE))
    {
        *inside = (b[0] > -THRESHOLD_FACE) && (b[1] > -THRESHOLD_FACE) && (b[2] > -THRESHOLD_FACE);
        return 0;
    }
//     printf("visible --> %d\n", visible);
    if (visible == 3)
    {
        if (nz == 2)
        {
            int vfi = vt[zero[0]][zero[1]];
            *inter = getFaceVertex(face, vfi);
            *vertex = getFaceVertexIndex(face, vfi);
        }
        else
        {
            if (nz == 1)
            {
                *edge = getFaceEdgeIndex(face, zero[0]);
            }
            double sum = b[0]+b[1]+b[2];
            double xx = (b[0]*p1->x()+b[1]*p2->x()+b[2]*p3->x())/sum;
            double yy = (b[0]*p1->y()+b[1]*p2->y()+b[2]*p3->y())/sum;
            double zz = (b[0]*p1->z()+b[1]*p2->z()+b[2]*p3->z())/sum;
            *inter = new Vertex(xx, yy, zz);
        }
        return 1;
    }
    return 0;
}
*/

void Ball::set_vertex(Vertex *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);
//     assert_message(0, "Should not arrive here !!");
}

void Ball::build()
{
    int seek = 0;
    while (seek < _t.size())
    {
        Tetra *t = _t[seek++];
        assert(t->getIndex() != -1);
        for (int i = 0; i < 4; i++)
            if (t->getVertex(i) != _v)
            {
                Tetra *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]->setFlag(-1);
}

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

void Shell::build()
{
    int count = 0;
    Tetra *tn = _t[0];
    int opp = tn->getOppEdgeVertexIndex(_e[0], 0); // a surveiller
    int edge = tn->getEdgeIndex(_v[0], _v[1]);
    Vertex *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(Tetra *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, Tetra **t, int *edge)
{
    assert(i<_t.size());
    *t = _t[i];
    *edge = _e[i];
}

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


// void Mesh::compute_ball(Vertex *v, int query_id, std::vector<Tetra*> &ball)
// {
//
//     Tetra *t = v->getTetra();
//     t->setFlag(query_id);
//     ball.push_back(t);
//     int seek = 0;
//     while (seek < ball.size())
//     {
//         t = ball[seek++];
//         for (int i = 0; i < 4; i++)
//             if (t->getVertex(i) != v)
//             {
//                 Tetra *adj = t->getAdj(i);
//                 if (adj != NULL && adj->getFlag() != query_id)
//                 {
//                     adj->setFlag(query_id);
//                     ball.push_back(adj);
//                 }
//             }
//     }
// }

int Mesh::isInBall(Vertex *v, Vertex *closest, std::vector<Tetra*> &ball) // attention --> int Tetra::contain(pt) repete trop souvent --> super lent
{
    compute_ball(v);
    Tetra *tb;
    int eb;
    for (int i = 0; i < _bot->size(); i++)
    {
        _bot->get_tetra(i, &tb, &eb);
        ball.push_back(tb);
    }
#if 0
    // printf("ball size --> %d\n", ball.size());
    for (int i = 0; i < ball.size(); i++)
    {
        Tetra *t = ball[i];
        if (t->contain(closest))
            return 1;
    }
    return 0;
#endif
    return 1; // version inexacte, mais corrigee par cutmesh
}

void DiscreteSurface::point(double uu, double vv, Vertex *pt, Vertex *n, double &curv)
{
    double u = this->u(uu);
    double v = this->v(vv);
    GPoint p1, p2;
    if (u < _minx && v < _miny) // coins
    {
        p1 = _face->point(_minx + (_minx-u), _miny + (_miny-v));
        p2 = _face->point(_minx, _miny);
        pt->setPosition(p2.x()+(p2.x()-p1.x()), p2.y()+(p2.y()-p1.y()), p2.z()+(p2.z()-p1.z()));
        SVector3 nn = _face->normal(SPoint2(_minx, _miny));
        curv = _face->curvature(SPoint2(_minx, _miny));
        n->setPosition(nn.x(), nn.y(), nn.z());
    }
    else if (u > _maxx && v < _miny)
    {
        p1 = _face->point(_maxx - (u-_maxx), _miny + (_miny-v));
        p2 = _face->point(_maxx, _miny);
        pt->setPosition(p2.x()+(p2.x()-p1.x()), p2.y()+(p2.y()-p1.y()), p2.z()+(p2.z()-p1.z()));
        SVector3 nn = _face->normal(SPoint2(_maxx, _miny));
	curv = _face->curvature(SPoint2(_maxx, _miny));
        n->setPosition(nn.x(), nn.y(), nn.z());
    }
    else if (u < _minx && v > _maxy)
    {
        p1 = _face->point(_minx + (_minx-u), _maxy - (v-_maxy));
        p2 = _face->point(_minx, _maxy);
        pt->setPosition(p2.x()+(p2.x()-p1.x()), p2.y()+(p2.y()-p1.y()), p2.z()+(p2.z()-p1.z()));
        SVector3 nn = _face->normal(SPoint2(_minx, _maxy));
	curv = _face->curvature(SPoint2(_minx, _maxy));
        n->setPosition(nn.x(), nn.y(), nn.z());
    }
    else if (u > _maxx && v > _maxy)
    {
        p1 = _face->point(_maxx - (u-_maxx), _maxy - (v-_maxy));
        p2 = _face->point(_maxx, _maxy);
        pt->setPosition(p2.x()+(p2.x()-p1.x()), p2.y()+(p2.y()-p1.y()), p2.z()+(p2.z()-p1.z()));
        SVector3 nn = _face->normal(SPoint2(_maxx, _maxy));
	curv = _face->curvature(SPoint2(_maxx, _maxy));
        n->setPosition(nn.x(), nn.y(), nn.z());
    }
    else if (u >= _minx && u <= _maxx && v <= _miny) // borders
    {
        p1 = _face->point(u, _miny + (_miny-v));
        p2 = _face->point(u, _miny);
        pt->setPosition(p2.x()+(p2.x()-p1.x()), p2.y()+(p2.y()-p1.y()), p2.z()+(p2.z()-p1.z()));
        SVector3 nn = _face->normal(SPoint2(u, _miny));
	curv = _face->curvature(SPoint2(u, _miny));
        n->setPosition(nn.x(), nn.y(), nn.z());
    }
    else if (u >= _minx && u <= _maxx && v >= _maxy)
    {
        p1 = _face->point(u, _maxy - (v-_maxy));
        p2 = _face->point(u, _maxy);
        pt->setPosition(p2.x()+(p2.x()-p1.x()), p2.y()+(p2.y()-p1.y()), p2.z()+(p2.z()-p1.z()));
        SVector3 nn =  _face->normal(SPoint2(u, _maxy));
	curv = _face->curvature(SPoint2(u, _maxy));
        n->setPosition(nn.x(), nn.y(), nn.z());
    }
    else if (u <= _minx && v >= _miny && v <= _maxy)
    {
        p1 = _face->point(_minx + (_minx-u), v);
        p2 = _face->point(_minx, v);
        pt->setPosition(p2.x()+(p2.x()-p1.x()), p2.y()+(p2.y()-p1.y()), p2.z()+(p2.z()-p1.z()));
        SVector3 nn = _face->normal(SPoint2(_minx, v));
	curv = _face->curvature(SPoint2(_minx, v));
        n->setPosition(nn.x(), nn.y(), nn.z());
    }
    else if (u >= _maxx && v >= _miny && v <= _maxy)
    {
        p1 = _face->point(_maxx - (u-_maxx), v);
        p2 = _face->point(_maxx, v);
        pt->setPosition(p2.x()+(p2.x()-p1.x()), p2.y()+(p2.y()-p1.y()), p2.z()+(p2.z()-p1.z()));
        SVector3 nn = _face->normal(SPoint2(_maxx, v));
	curv = _face->curvature(SPoint2(_maxx, v));
        n->setPosition(nn.x(), nn.y(), nn.z());
    }
    else
    {
        p1 = _face->point(u, v);
        pt->setPosition(p1.x(), p1.y(), p1.z());
        SVector3 nn = _face->normal(SPoint2(u, v));
	curv = _face->curvature(SPoint2(u, v));
        n->setPosition(nn.x(), nn.y(), nn.z());
    }
}

DiscreteSurface::DiscreteSurface(GFace *face, int id, double tminx, double tmaxx, double tminy, double tmaxy, int max_step)
{
    _face = face;
    _minx = tminx;
    _maxx = tmaxx;
    _miny = tminy;
    _maxy = tmaxy;
    _lx = (_maxx-_minx);
    _ly = (_maxy-_miny);
    _enx = 0.0;
    _eny = 0.0;
    _size = max_step;
    _id = id;
     _center = NULL;
     _discrete = 1;
}

DiscreteSurface::DiscreteSurface(int id, double tminx, double tmaxx, double tminy, double tmaxy, int max_step)
{
    _minx = tminx;
    _maxx = tmaxx;
    _miny = tminy;
    _maxy = tmaxy;
    _lx = (_maxx-_minx);
    _ly = (_maxy-_miny);
    _enx = 0.0;
    _eny = 0.0;
    _size = max_step;
    _id = id;
     _center = NULL;
     _discrete = 1;
}

DiscreteSurface::DiscreteSurface(int id, void *implicit_surf)
{
     _discrete = 0;
     _implicit_surf = implicit_surf;
}

DiscreteSurface::DiscreteSurface(FILE* fp, int id)
{
     double surface_max_sample = 150;
     _size = surface_max_sample;
     _center = NULL;
     _discrete = 1;
     
     int error = 0;

    int ls_id;
    if (fscanf(fp, "%d", &ls_id) != 1)
      error++;
    _id = id;

    int nbvx;
    if (fscanf(fp, "%d", &nbvx) != 1)
      error++;  
        
    int i;
    Vertex *pt, *n;
    double ptx, pty, ptz, nx, ny, nz, c;
    for (i = 0; i<nbvx; i++)
    {
//         if (fscanf(fp, "%lf %lf %lf %lf %lf %lf ", &ptx, &pty, &ptz, &nx, &ny, &nz) != 6)
//           error++;
//         pt = new Vertex(ptx, pty, ptz);
//         n = new Vertex(nx, ny, nz);
//         add_point(pt, n, 0.0);
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf ", &ptx, &pty, &ptz, &nx, &ny, &nz, &c) != 7)
          error++;
	
// 	double eps = 0.001;
// 	if (c < eps)
// 	  c = 1.0/eps;
// 	else
// 	  c = 1.0/c;
        
	pt = new Vertex(ptx, pty, ptz);
        n = new Vertex(nx, ny, nz);
        add_point(pt, n, c);
    }
    assert(error == 0);
}

void DiscreteSurface::generate()
{
    if (!_discrete)
        return;

    double r = 1.0/(double)_size;

    for (int i = 0; i <= _size; i++)
        for (int j = 0; j <= _size; j++)
        {
            Vertex *pt = new Vertex;
            Vertex *n = new Vertex;
	    double curv;
            point((double)i*r, (double)j*r, pt, n, curv);
            pt->setSurf(_id);
            add_point(pt, n, curv);
        }
}

void DiscreteSurface::bucketize(double longuest_mesh_edge)
{
    if (!_discrete)
    return;
    
  double enlarge = 0.1;
#if defined(HAVE_ANN)
//   printf("--> using ANN\n");
  ANNpointArray nodes = annAllocPts(size(), 6);
    for (int i = 0; i < size()/*10000*/; i++)
    {
//       nodes[i][0] = 1000.*(double)rand()/(double)RAND_MAX;
//       nodes[i][1] = 1000.*(double)rand()/(double)RAND_MAX;
        Vertex *pt = NULL;
        Vertex *n = NULL;
        double curv;
        get_point(i, &pt, &n, &curv);
        nodes[i][0] = pt->x();
        nodes[i][1] = pt->y();
        nodes[i][2] = pt->z();
        nodes[i][3] = n->x();
        nodes[i][4] = n->y();
        nodes[i][5] = n->z();
    }
    _kdtree = new ANNkd_tree(nodes, size(), 3);  
    
    double ll[3] = {_bbmax[0]-_bbmin[0], _bbmax[1]-_bbmin[1], _bbmax[2]-_bbmin[2]};
//     _bbmin[0] -= enlarge*ll[0]/2.0;
//     _bbmin[1] -= enlarge*ll[1]/2.0;
//     _bbmin[2] -= enlarge*ll[2]/2.0;
//     _bbmax[0] += enlarge*ll[0]/2.0;
//     _bbmax[1] += enlarge*ll[1]/2.0;
//     _bbmax[2] += enlarge*ll[2]/2.0;
    
    _bbmin[0] -= 1.1*longuest_mesh_edge/2.0;
    _bbmin[1] -= 1.1*longuest_mesh_edge/2.0;
    _bbmin[2] -= 1.1*longuest_mesh_edge/2.0;
    _bbmax[0] += 1.1*longuest_mesh_edge/2.0;
    _bbmax[1] += 1.1*longuest_mesh_edge/2.0;
    _bbmax[2] += 1.1*longuest_mesh_edge/2.0;
    
//     printf("%lf %lf %lf -- %lf %lf %lf\n", _bbmin[0], _bbmin[1], _bbmin[2], _bbmax[0], _bbmax[1], _bbmax[2]);
//     filebuf fb;
//     fb.open ("test.dmp",ios::out);
//     ostream os(&fb);  
//     _kdtree->Dump(ANNtrue, os);
//     fb.close();
#else
    _bs = new Bucket(Vertex(_bbmin[0], _bbmin[1], _bbmin[2]), Vertex(_bbmax[0], _bbmax[1], _bbmax[2]), longuest_mesh_edge, floor(_size/5));
    for (int i = 0; i < size(); i++)
    {
        Vertex *pt = NULL;
        Vertex *n = NULL;
        get_point(i, &pt, &n);
        _bs->add_point(pt);
    }
//     _bs->infos();
#endif
}

void DiscreteSurface::get_closest(Vertex *pt, Vertex **cl)
{
  #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();
//    printf("pt --> %lf %lf %lf\n", xyz[0], xyz[1], xyz[2]);
    _kdtree->annkSearch(xyz, 1, index, dist);
    ANNpointArray pts = _kdtree->thePoints();
    ANNpoint pcl = pts[index[0]];
    if (*cl == NULL)     // WARNING : 2 utilisation differentes de get_closest a l'initialisation
      *cl = new Vertex;
    (*cl)->setPosition(pcl[0], pcl[1], pcl[2]);    
    (*cl)->setNormal(pcl[3], pcl[4], pcl[5]);
  #else
    if (_bs->contain(pt))
      *cl = _bs->get_closest(pt);
    else
    {
      Vertex *proj = _bs->get_projection(pt);
      *cl = _bs->get_closest(proj);
    }
    assert (*cl != NULL);
  #endif
}

void DiscreteSurface::distance(Vertex *pt, double &simpledist, double &projdist)
{
    if (!_discrete)
    {
      Plane *pl =  (Plane*)_implicit_surf;
      simpledist = pl->distance(pt);
      projdist = pl->distance(pt);
//       return pl->distance(pt);
    }
#if defined(HAVE_ANN)
//     ANNidxArray index = new ANNidx[2];
//     ANNdistArray dist = new ANNdist[2];
    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();
//     printf("pt --> %lf %lf %lf\n", xyz[0], xyz[1], xyz[2]);
    _kdtree->annkSearch(xyz, 1, index, dist);
    
    ANNpointArray pts = _kdtree->thePoints(); 
    ANNpoint pcl = pts[index[0]];
//     printf("kdsearch --> %lf %lf %lf %lf %lf %lf\n", pcl[0], pcl[1], pcl[2], pcl[3], pcl[4], pcl[5]);
    Vertex cl(pcl[0], pcl[1], pcl[2]);
    Vertex n(pcl[3], pcl[4], pcl[5]);
    Vertex ptcl;
    ptcl.setVector(pt, &cl);
    ptcl.dotproduct(&n);
    simpledist = ptcl.norm();
    projdist = ptcl.dotproduct(&n);    
//     return ptcl.dotproduct(&n);
#else
    Vertex *cl = NULL;
    get_closest(pt, &cl);
    Vertex *n = cl->getNormal();
    Vertex ptcl;
    ptcl.setVector(pt, cl);
    simpledist = ptcl.norm();
    projdist = ptcl.dotproduct(&n);
//     return ptcl.dotproduct(n);
#endif
}


void DiscreteSurface::dump(int num)
{
//     _bs->dump_vtk(num);
    char filename[100];
    sprintf(filename, "surf_point_%d.vtk", num);
    printf("output file %s\n", filename);

    FILE *fp = fopen(filename, "w");
    Vertex *pt;
    Vertex *n;
    double curv;

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

    fprintf (fp, "POINTS %d float\n", size());

    for (int i = 0; i < size(); i++)
    {
        get_point(i, &pt, &n, &curv);
        fprintf(fp, "%lf %lf %lf\n", pt->x(), pt->y(), pt->z());
    }

    fprintf(fp, "CELLS %d %d\n", size(), size()*2);
    for (int i = 0; i < size(); i++)
    {
        fprintf(fp, "%d %d\n", 1, i);
    }

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

    fprintf(fp, "POINT_DATA %d\n", size());
    fprintf(fp, "SCALARS curvature float 1\n");
    fprintf(fp, "LOOKUP_TABLE default\n");
    for (int i = 0; i < size(); i++)
    {
        get_point(i, &pt, &n, &curv);
        fprintf(fp, "%lf\n", curv);
    }
    
    fprintf(fp, "VECTORS normal float\n");
    for (int i = 0; i < size(); i++)
    {
        get_point(i, &pt, &n, &curv);
        fprintf(fp, "%lf %lf %lf\n", n->x(), n->y(), n->z());
    }
    
    fclose(fp);
}

void Mesh::bucketize()
{  
#if defined(HAVE_ANN)
    ANNpointArray nodes = annAllocPts(vertex_size(), 4);
    for (int i = 0; i < vertex_size(); i++)
    {
            Vertex *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
}

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;
}

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);
        Vertex *pt;
        Vertex *n;
        double curv;
        surf->get_point(i, &pt, &n, &curv);
        if (curv > 1e-5 )
        {
            Tetra *t = find_tetra_containing(pt);
            t->addInc(pt);
        }
    }
    pbar.end();
}

double Mesh::evaluate_params(int refmax, double error_threshold)
{
    double max_curv_mesh = 0.0;
    for (int i = 0; i < tetra_size(); i++)
    {
        Tetra *t = get_tetra(i);
        if (t->getState() == NOT_REFINED)
        {    
            for (int j = 0; j < t->inc_size(); j++)
            {
                Vertex *v = t->getInc(j);
		if (v->getCurvature() > max_curv_mesh)
		  max_curv_mesh = v->getCurvature();
            }
	}      
    }
    
    double max_lenght = get_longuest();
    double max_lenght_refined = max_lenght/pow(2.0, refmax);
    
    tprintf("min radius over mesh --> %lf\n", 1.0/max_curv_mesh);        
    tprintf("longuest --> %lf\n", get_longuest());    
    tprintf("refined longuest --> %lf\n", max_lenght_refined);
    
    double min_radius_allowed = sqrt((max_lenght_refined*max_lenght_refined)/(8.0*error_threshold-4.0*error_threshold*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++)
    {
        Tetra *t = get_tetra(i);
        if (t->getState() == NOT_REFINED)
        {    
            for (int j = 0; j < t->inc_size(); j++)
            {
                Vertex *v = t->getInc(j);
		if (v->getCurvature() > max_curv_allowed)
		  v->setCurvature(max_curv_allowed);
            }
	}      
    }
    return max_curv_allowed;
}


int Mesh::adapt(double max_curv_allowed, double error_threshold)
{
    printf("--> adapting tetras\n");
    
    int count = 0;
    
    for (int i = 0; i < tetra_size(); i++)
    {
        Tetra *t = get_tetra(i);
        if (t->getState() == NOT_REFINED)
        {
	    double max_curv_tetra = 0.0;
            for (int j = 0; j < t->inc_size(); j++)
            {
                Vertex *v = t->getInc(j);
		if (v->getCurvature() > max_curv_tetra)
		  max_curv_tetra = v->getCurvature();
            }
            
            if (max_curv_tetra > max_curv_allowed)
	      max_curv_tetra = max_curv_allowed;
            
            if (max_curv_tetra > 0.0)
            {
                int nrf = normalize_curv(1.0/max_curv_tetra, t->getlenght(), error_threshold);
		
                if (t->getRefDepth() < nrf)
                {
                    t->setRefDepth(nrf);
                    count++;
                }
            }
        }
    }
    printf("--> adapt = %d\n",count);
    return count;
}

Tetra* Mesh::find_tetra_containing(Vertex *pt)
{
    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]];
    Vertex cl(pcl[0], pcl[1], pcl[2]);
    int tetra_index = (int)pcl[3];
    Tetra *t = get_tetra(tetra_index);
       
    srand(0);  
    while (1)
    {
        if (t->contain(pt))
            return t;
        int dir = rand()%4;
        Tetra *adj = t->getAdj(dir);
        int opp = t->getOpp(dir);
        if (adj != NULL)
            t = adj;
    }        
}

int Mesh::intersected(DiscreteSurface *surf, Tetra *t, int *out)
{
    int minus = 0, plus = 0;
    double projdist;
    double dist;
    for (int i = 0; i < 4; i++)
    {
        Vertex *v = t->getVertex(i);
        if (!surf->bbox_contain(v))
            (*out)++;
        if (v->get_projdist() == INFINITY)
	{
	  surf->distance(v, dist, projdist);
// 	  printf("dist --> %lf %lf\n", dist, projdist);
            v->set_projdist(projdist);
	    v->set_dist(dist);
	}

        if (v->get_projdist() < 0.0)
            minus++;
        else if (v->get_projdist() > 0.0)
            plus++;
        else
        {
            minus++;
            plus++;
        }
    }
//   printf("intersection --> %lf %lf %lf %lf\n", t->getVertex(0)->get_projdist(), t->getVertex(1)->get_projdist(), t->getVertex(2)->get_projdist(), t->getVertex(3)->get_projdist());
    if (minus && plus)
        return 1;
    else return 0;
}

Tetra* Mesh::init_intersected(DiscreteSurface *surf, Tetra *init)
{
    std::vector<Tetra *> flagged;
    double min_dist = INFINITY;
    Tetra *min_tetra = NULL;
    
    Vertex *spt;
    Vertex *sptn;
    double curv;
    
    surf->get_point(0, &spt, &sptn, &curv);
    
    if (init->contain(spt))
      return init;    
       
    Tetra *t = init;
    
    srand(0);
	  
//     int count = 0;
    while (1)
    {
        int dir = rand()%4;
        Tetra *adj = t->getAdj(dir);
        int opp = t->getOpp(dir);
        if (adj != NULL)
        {	      
            if (adj->getFlag() == 0)
            {
                if (!init->contain(spt))
                {
                    t = adj;
// 		    count++;
                }
                else
                {
                    for (int j = 0; j < flagged.size(); j++)
                        flagged[j]->setFlag(0);
                    return adj;
                }
            }
            else
            {
                for (int j = 0; j < flagged.size(); j++)
                    flagged[j]->setFlag(0);
                return adj;
            }
        }
    }
}

Tetra* Mesh::search_intersected(DiscreteSurface *surf, Tetra *init)
{
    std::vector<Tetra *> flagged;
    double min_dist = INFINITY;
    Tetra *min_tetra = NULL;
    
    int out = 0;    
    if (intersected(surf, init, &out))
      return init;    
       
    Tetra *t = init;
	  
    while (1)
    {
        min_dist = INFINITY;
        for (int i = 0; i < 4; i++)
        {
	  Tetra *adj = t->getAdj(i);
	  int opp = t->getOpp(i);
            if (adj != NULL && adj->getFlag() == 0)
            {
	        out = 0;
                if (!intersected(surf, adj, &out))
                {
                    if (fabs(adj->getVertex(opp)->get_projdist()) < min_dist)
                    {
                        min_dist = fabs(adj->getVertex(opp)->get_projdist());
                        min_tetra = adj;
                    }
                }
                else
		{
                    for (int j = 0; j < flagged.size(); j++)
                        flagged[j]->setFlag(0);
                    return adj;
		}
            }
        }
	t->setFlag(1);
// 	printf("t --> %d\n", t->getIndex());
// 	printf("min dist --> %lf\n", min_dist);
// 	printf("min tetra --> %d\n", min_tetra->getIndex());
	flagged.push_back(t);
	t = min_tetra;	
    }
}

void Mesh::build_support_opt(DiscreteSurface *surf, std::vector<Tetra *> &tetra_support)
{
//     tprintf("--> building support ...\n");
    tetra_support.clear();
        
    for (int i = 0; i < vertex_size(); i++)
      get_vertex(i)->set_projdist(INFINITY);
    
    for (int i = 0; i < tetra_size(); i++)
      get_tetra(i)->setFlag(0);
    
    std::queue<Tetra *> tq;
    std::vector<Tetra *> flagged;
    double min_dist = INFINITY;
    Tetra *min_tetra = NULL;

    Tetra *init = get_vertex(0)->getTetra();
    
//     printf("init tetra --> %d\n", init->getIndex());
    
    init = search_intersected(surf, init);
//     init = init_intersected(surf, init);    
    
    tq.push(init);
    
    while (!tq.empty())
    {     
	Tetra *t = tq.front();
	tq.pop();
	for (int i = 0; i < 4; i++)
        {
	    Tetra *adj = t->getAdj(i);
            if (adj != NULL && adj->getFlag() == 0)
            {
	        int out = 0;
                if (intersected(surf, adj, &out))
                {
// 		    printf("out %d\n", out);
                    if (out < 4)
		    {
                        double d = 0.0;
                        for (int j = 0; j < 4; j++)
                        {
                            Vertex *v = t->getVertex(j);
                            if (v->get_dist() > d)
                                d = v->get_dist();
                        }
//			printf ( "longuest --> %lf / %lf\n", get_longuest(), d);
		
                      if (d <= 1.2*get_longuest())
                        {
                            tetra_support.push_back(adj);
                        }                        
		    }

                    tq.push(adj);
                    adj->setFlag(1);
                    flagged.push_back(adj);
                }
            }
        }
    }
    
//     int size = tq.size();
//     for (int i = 0; i < size; i++)
//     {
//       Tetra *t = tq[i];
//       double d = INFINITY;
//       for (int j = 0; j < 4; j++)
//       {
// 	Vertex *v = t->getVertex(j);
// 	if (v->get_dist() < d)
// 	  d = v->get_dist();
//       }
//       if (d > get_longuest())
//       {
// 	if (i < size-1)
// 	{
// 	  tq[i] = tq[size-1];
// 	}
// 	size--;
//       }      
//     }
//     tq
    // cleanup
    for (int i = 0; i < flagged.size(); i++)
        flagged[i]->setFlag(0);    
    
//     printf("--> %d tetra support\n\n", (int)tetra_support.size());
}

void Mesh::build_support(DiscreteSurface *surf, std::vector<Tetra *> &tetra_support)
{
//     tprintf("--> building support ...\n");
    tetra_support.clear();

    Vertex *cl = NULL;
    Vertex *n = NULL;
    Vertex vcl;
    double dist;

    std::vector<int> tetra_used;
    std::vector<Vertex*> vertex_closest;
    tetra_used.resize(tetra_size(), 0);
    vertex_closest.resize(vertex_size(), NULL);
    
    for (int i = 0; i < vertex_size(); i++)
      get_vertex(i)->set_projdist(INFINITY);

    for (int i = 0; i < vertex_size(); i++)
    {
        Vertex *v = get_vertex(i);
        surf->get_closest(v, &cl);
        if (cl != NULL)
        {
            vcl.setVector(v, cl);
            dist = vcl.norm();
            if (dist <= 1.1*get_longuest())
            {
                vertex_closest[i] = cl;
                std::vector<Tetra*> ball;
                if (isInBall(v, cl, ball))
                {
                    for (int j = 0; j < ball.size(); j++)
                    {
                        Tetra *t = ball[j];
                        if (!tetra_used[t->getIndex()])
                        {
                            tetra_used[t->getIndex()]++;
                            tetra_support.push_back(t);
                        }
                    }
                }
            }
        }
    }
    // cleanup
    int invalid = 0;
    for (int i = 0; i < tetra_support.size(); i++)
    {
        Tetra *t = tetra_support[i];
        if (t != NULL)
        {
            for (int j = 0; j < 4; j++)
            {
                Vertex *v = t->getVertex(j);
                cl = vertex_closest[v->getIndex()];
                if (cl != NULL)
                {
                    vcl.setVector(v, cl);
                    n = cl->getNormal();
                    dist = vcl.dotproduct(n);
                    v->set_projdist(dist);
                }
                else
                {
                    tetra_support[i] = tetra_support.back();
                    tetra_support.erase(tetra_support.end()-1);
                    invalid++;
                    i--;
                    break;
                }
            }
        }
    }
}


void Mesh::build_support_opt_walk(DiscreteSurface* surf, std::vector< Tetra* >& tetra_support)
{
    tetra_support.clear();

    for (int i = 0; i < vertex_size(); i++)
      get_vertex(i)->set_dist(INFINITY);

    for (int i = 0; i < tetra_size(); i++)
      get_tetra(i)->setFlag(0);
    
    srand(time(NULL));

    int middle_index = vertex_size()/2-1;
    Vertex* init_vertex = get_vertex(middle_index); // centre (ou au hazard...)
    
    Tetra *t = NULL;
    Vertex *cl = NULL;
    walk_to_suface(surf, init_vertex, &t, &cl);

    propagation(cl, t, surf, tetra_support);
}

Tetra* Mesh::walk_to_suface(DiscreteSurface* surf, Vertex* v, Tetra** init_tetra, Vertex** init_cl)
{
    double min[3], max[3];
    surf->get_bbox(min, max);

    // Calcul du centre de la bounding box
    Vertex *c = surf->get_center_box();

    Tetra *t = v->getTetra();

    Vertex cv;
    cv.setVector(c,v);
    double d_cv = cv.sq_norm();

    // Calcul du carre de la demi-diagonale de la bounding box
    Vertex vd( (max[0]-min[0])/2. , (max[1]-min[1])/2. , (max[2]-min[2])/2. );
    double d = vd.sq_norm();

    if ( d < get_longuest()*get_longuest() )
      d = get_longuest()*get_longuest();

//    tprintf("First walk\n");
    while( d_cv > d )
    {
      if ( !walk(c,&v,&t,d_cv) )
      {
        walk_with_ball(c, &v, &t);
        cv.setVector(c,v);
        d_cv = cv.sq_norm();
      }
    }

//    tprintf("Last walk\n");
    int i=0;
    int walking=1;
    Vertex *cl = NULL;
    surf->get_closest(v,&cl);
    if (cl == NULL)
      surf->get_closest(c,&cl);
    t = v->getTetra();

    while (walking)
    {
      walking = walk_coord_bary(cl,&t,&v);

      if ( walking == 1 )
      {
//        tprintf("  Walk %d is good.\n", i++);
      }
      else if ( walking == -1 )
      {
//        tprintf("  Walk %d is bad.\n", i++);
        walking = walk_with_ball(cl, &v, &t);
      }
    }
//    tprintf("  Walk %d is completed.\n", i++);
    *init_tetra = t;
    *init_cl = cl;
//    tprintf("Surface found\n");
}

int Mesh::walk (Vertex *cl, Vertex **v, Tetra **t, double &d_pv)
{
  Tetra *adj;
  int opp;
  int walking=0;

  Vertex pv;

  Vertex *v_new;
  double d_new = INFINITY;

  for (int i=0; i<4; i++)
  {
    (*t)->getAdjOpp(i,&adj,&opp);
    if (adj == NULL)
      continue;
    v_new = adj->getVertex(opp);
    pv.setVector(cl,v_new);
    d_new = pv.sq_norm();
    if ( d_new < d_pv )
    {
      d_pv = d_new;
      *t = adj;
      *v = v_new;
      walking++;
    }
  }
  return walking;
}

int Mesh::walk_with_ball(Vertex *cl, Vertex **v, Tetra **t)
{
    std::vector<Tetra*> ball;
    std::vector<Tetra*> potential_tetra;
    std::vector<Vertex*> potential_vertex;
    std::vector<int> v_in_ball;
    Tetra *tb, *temp;
    int i,eb;
    double b[4];
    int contain_cl;

    compute_ball(*v);
    for (i = 0; i < _bot->size(); i++)
    {
        _bot->get_tetra(i, &tb, &eb);
        ball.push_back(tb);
        v_in_ball.push_back(eb);
    }

    for (i = 0; i < ball.size(); i++)
    {   // On cherche si un tetra de la ball contient le point
        tb = ball[i];
        tb->barycentric(cl, b);
        contain_cl = 1;
        eb = v_in_ball[i];
        for (int j = 0; j < 4; j++)
        {
          if (b[j] < -THRESHOLD_FACE)
            contain_cl--;
        }
        if (contain_cl==1)
        {// tprintf("ball contains point\n");
          *t = tb;
          *v = tb->getVertex(rand() % 4);
          return 0;
        }
        else if ( b[eb]<0. && contain_cl==0 )
        {// tprintf("continue walk\n");
          tb->getAdjOpp(eb,&temp,&eb);
          if (temp == NULL)
          {
            *t = tb;
            *v = tb->getVertex(rand() % 4);
            continue;
          }
          *t = temp;
          *v = temp->getVertex(rand() % 4);
          return 1;
        }
    }

//     tprintf("Continue to test other random tetra\n");
    return -1;
}

int Mesh::walk_coord_bary (Vertex *cl, Tetra **t, Vertex **v)
{
  int next_t;
  int next_v;
  double b[4];
  std::vector<int> directions;
  Tetra *temp = NULL;
  bool walking=false;

  (*t)->barycentric(cl, b);

  for (int i = 0; i < 4; i++)
  {
    if (b[i] < -THRESHOLD_FACE)
      directions.push_back(i);
  }
  if (directions.size()==0)
  {// tprintf("tetra contains point\n");
    return 0;
  }
  else if ( directions.size() == 1 )
  {// tprintf("continue walk\n");
    (*t)->getAdjOpp(directions[0],&temp,&next_v);
    if (temp == NULL)
    {  printf("No adjacence in good direction\n");
      return -2;
    }
    *t = temp;
    *v = temp->getVertex(next_v);
    return 1;
  }
  else
  {
    int nb_test = 0;
    do
    {
      next_t = rand() % directions.size();
      (*t)->getAdjOpp(directions[next_t],&temp,&next_v);
      nb_test++;
    } while (temp == NULL && nb_test<directions.size());
    if (temp == NULL)
    { printf("No adjacence in random direction\n");
      return -2;
    }
    next_v = rand() % 4;
//    tprintf("random walk : %d, %d\n", next_t, next_v);
    *t = temp;
    *v = temp->getVertex(next_v);
    return -1;
  }
}

void Mesh::propagation (Vertex *init_cl, Tetra *init_tetra, DiscreteSurface *surf, std::vector< Tetra *>& tetra_support)
{
//    tprintf("Propagation BEGIN\n");
  
  std::queue<Tetra *> propagation_tetra; // voir si pour des raisons d'allocation memoir il n'est pass necessaire d'agrandir la map (utiliser un vecteur de la dimention du maillage augmenterais trop la taille)
  Tetra *t = NULL;
  Tetra *ti = NULL;
  Vertex *v = NULL;
  Vertex *cl = NULL;
  Vertex vcl;
  Vertex *n = NULL;
  int i;
  int opp;
  double d[4];
  double dist;

  init_tetra->setFlag(1);
  for (i = 0; i < 4; i++)
  {
    v = init_tetra->getVertex(i);
    surf->get_closest(v, &cl);
    if (cl == NULL)
      cl = init_cl;
    vcl.setVector(v,cl);
    n = cl->getNormal();
    dist = vcl.dotproduct(n);
    v->set_dist(dist);
  }
  tetra_support.push_back(init_tetra);
  propagation_tetra.push(init_tetra);

  while ( !propagation_tetra.empty() )
  {
    t = propagation_tetra.front();
    propagation_tetra.pop();

    for  (i = 0; i < 4; i++)
      d[i] = t->getVertex(i)->get_dist();
    
    for (i = 0; i < 4; i++)
    {
      t->getAdjOpp(i,&ti,&opp);
      if (ti == NULL)
        continue;

      if ( !ti->getFlag() )
      {
        ti->setFlag(1);

        v = ti->getVertex(opp);
        surf->get_closest(v, &cl);
        assert (cl != NULL);

        vcl.setVector(v,cl);
        if (vcl.norm() > get_longuest()*1.01)
          continue;

        n = cl->getNormal();
        dist = vcl.dotproduct(n);
        
        if( dist*d[(i+1)%4] <= 0. || dist*d[(i+2)%4] <= 0. || dist*d[(i+3)%4] <= 0. )
        {
          v->set_dist(dist);
          tetra_support.push_back(ti);
          propagation_tetra.push(ti);
        }
      }
    }
  }

//    tprintf("Propagation END\n");
}


void Mesh::longuest_edge(double dx, double dy, double dz)
{   // Diagonale du pave elementaire
    Vertex d(dx, dy, dz);
    _le = d.norm();
    tprintf("--> longuest edge = %lf\n", _le);
}


/*void Mesh::longuest_edge()
{
    _le = 0.0;
    for (int i = 0; i < tetra_size(); i++)
    {
        Tetra *t = get_tetra(i);
        for (int j = 0; j < 6; j++)
        {
            Vertex *v0 = t->getEdgeVertex(j, 0);
            Vertex *v1 = t->getEdgeVertex(j, 1);
            Vertex ab;
            ab.setVector(v0, v1);
            double l = ab.sq_norm();
            if (l > _le)
                _le = l;
        }
    }
    _le = sqrt(_le);
    tprintf("--> longuest edge = %lf\n", _le);
}
*/

void Mesh::renumber()
{
    int count = 0;
    for (int i = 0; i < tetra_size(); i++)
    {
      Tetra *t = get_tetra(i);
      if (t->getState() == NOT_REFINED)
	t->setIndex(count++);
    }
}

void Mesh::init_support_file(char *name)
{
    char filename[100];
#if defined(REFINE)
    sprintf(filename, "%s_refined.ls", name);
#else
    sprintf(filename, "%s.ls", name);
#endif
    _lsf = fopen(filename, "w");
    fprintf(_lsf, "%d\n", 0); // type de fichier
}

void Mesh::append_levelset_size(int size)
{
    int reset = 0;
    if (_lseek == 0)
        _lseek = ftell(_lsf);
    else
    {
        fseek(_lsf, _lseek, SEEK_SET);
        reset = 1;
    }
    fprintf(_lsf, "%10d\n", size); // %10d car laisse ainsi de la place pour des nombres eleves
    fseek(_lsf, 0, SEEK_END);
    if (reset)
        _lseek = 0;
}

void Mesh::append_header(int surf_id, int support_size)
{
    fprintf(_lsf, "%d %d\n", surf_id, support_size);
}

void Mesh::append_header(int surf_id, int st_1, int st_2, int support_size)
{
    fprintf(_lsf, "%d %d %d %d\n", surf_id, st_1, st_2, support_size);
}

void Mesh::append_support(std::vector< Tetra* >& tetra_support)
{
    for (int i = 0; i < tetra_support.size(); i++)
    {
        Tetra *t = tetra_support[i];
	if (t->getState() == NOT_REFINED)
        {
            fprintf(_lsf, "%d ", t->getIndex());
            for (int j = 0; j < 4; j++)
            {
                Vertex *v = t->getVertex(j);
                fprintf(_lsf, "%d %f ", v->getIndex(), v->get_projdist());
            }
            fprintf(_lsf, "\n");
        }
    }
}

void Mesh::append_topo_vertex_size(int size)
{
    fprintf(_lsf, "%d\n", size);
}

void Mesh::append_topo_vertex(int index, Vertex tv)
{
    fprintf(_lsf, "%d %lf %lf %lf\n", index, tv.x(), tv.y(), tv.z());
}

void Mesh::append_topo_edge_size(int size)
{
    fprintf(_lsf, "%d\n", size);
}

void Mesh::append_topo_edge(int index, std::vector<int> vertices, std::vector<int> surf)
{
    fprintf(_lsf, "%d %d", index, (int)vertices.size());
    for (int i = 0; i < vertices.size(); i++)
    {
        fprintf(_lsf, " %d", vertices[i]);
    }
    fprintf(_lsf, "\n");
    fprintf(_lsf, "%d", (int)surf.size());
    for (int i = 0; i < surf.size(); i++)
    {
        fprintf(_lsf, " %d ", surf[i]);
    }
    fprintf(_lsf, "\n");
}

void Mesh::close_support_file()
{
    fclose(_lsf);
}

void Mesh::dump_support(int surf_id, std::vector<Tetra *> &tetra_support)
{
    char filename[100];

    int nb_v = 0;
    std::map<int, int > map_v;
    std::map<int, int >::iterator it;
    std::vector<Vertex *> vx;
    for (int i = 0; i < tetra_support.size(); i++)
    {
        Tetra *t = tetra_support[i];
        for (int j = 0; j < 4; j++)
        {
            Vertex *v = t->getVertex(j);
            it = map_v.find(v->getIndex());
            if (it == map_v.end())
            {
                map_v.insert(std::pair<int, int>(v->getIndex(), nb_v++));
                vx.push_back(v);
            }
        }
    }

    sprintf(filename, "surf_support_%d.vtk", surf_id);
//     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", nb_v);
    for (int i = 0; i < nb_v; i++)
    {
        Vertex *v = vx[i];
        fprintf(fp, "%lf %lf %lf\n", v->x(), v->y(), v->z());
    }

    fprintf(fp, "CELLS %d %d\n", (int)tetra_support.size(), (int)tetra_support.size()*5);
    for (int i = 0; i < tetra_support.size(); i++)
    {
        Tetra *t = tetra_support[i];
        fprintf (fp, "%d ", 4);
        Vertex *v0 = t->getVertex(0);
        it = map_v.find(v0->getIndex());
        fprintf (fp, "%d ", it->second);
        Vertex *v1 = t->getVertex(1);
        it = map_v.find(v1->getIndex());
        fprintf (fp, "%d ", it->second);
        Vertex *v2 = t->getVertex(2);
        it = map_v.find(v2->getIndex());
        fprintf (fp, "%d ", it->second);
        Vertex *v3 = t->getVertex(3);
        it = map_v.find(v3->getIndex());
        fprintf (fp, "%d ", it->second);
        fprintf (fp, "\n");
    }

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

    fprintf(fp, "POINT_DATA %d\n", nb_v);
    fprintf(fp, "SCALARS scalars float 1\n");
    fprintf(fp, "LOOKUP_TABLE default\n");
    for (int i = 0; i < nb_v; i++)
    {
        Vertex *v = vx[i];
        fprintf(fp, "%lf\n", v->get_projdist());
    }
    fclose(fp);
}

void Mesh::exportVTK()
{
    char filename[100];
    
    static int countf = 0;

    sprintf(filename, "mesh_%d.vtk", countf++);
    printf("output --> %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", vertex_size());
    for (int i = 0; i < vertex_size(); i++)
    {
        Vertex *v = get_vertex(i);
        fprintf(fp, "%lf %lf %lf\n", v->x(), v->y(), v->z());
    }
    
    int count = 0;
    for (int i = 0; i < tetra_size(); i++)
    {
      if (get_tetra(i)->getState() == NOT_REFINED)
      {
	assert(get_tetra(i)->getChild(0) == NULL);
	count++;
      }
    }

    fprintf(fp, "CELLS %d %d\n", count, count*5);
    for (int i = 0; i < tetra_size(); i++)
    {
        Tetra *t = get_tetra(i);
        if (t->getState() == NOT_REFINED)
        {
            fprintf (fp, "%d ", 4);
            fprintf (fp, "%d ", t->getVertex(0)->getIndex());
            fprintf (fp, "%d ", t->getVertex(1)->getIndex());
            fprintf (fp, "%d ", t->getVertex(2)->getIndex());
            fprintf (fp, "%d ", t->getVertex(3)->getIndex());
            fprintf (fp, "\n");
        }
    }

    fprintf(fp, "CELL_TYPES %d\n", count);
    for (int i = 0; i < count; i++)
    {
        fprintf(fp, "%d\n", 10);
    }
    
    fprintf(fp, "CELL_DATA %d\n", count);
    fprintf(fp, "SCALARS ref_depth int 1\n");
    fprintf(fp, "LOOKUP_TABLE default\n");
    for (int i = 0; i < tetra_size(); i++)
    {
      Tetra *t = get_tetra(i);
      if (t->getState() == NOT_REFINED)
	fprintf(fp, "%d\n", t->getRefDepth());
    }
    
//     fprintf(fp, "SCALARS adj int 1\n");
//     fprintf(fp, "LOOKUP_TABLE default\n");
//     for (int i = 0; i < tetra_size(); i++)
//     {
//       Tetra *t = get_tetra(i);
//       if (t->getState() == NOT_REFINED)
//       {
// 	int adjc = 0;
// 	for (int j = 0; j < 4; j++)
// 	  if (t->getAdj(j) != NULL)
// 	     adjc++;
// 	fprintf(fp, "%d\n", adjc);
//       }
//     }
    
    fprintf(fp, "SCALARS inc int 1\n");
    fprintf(fp, "LOOKUP_TABLE default\n");
    for (int i = 0; i < tetra_size(); i++)
    {
      Tetra *t = get_tetra(i);
      if (t->getState() == NOT_REFINED)
      {
	fprintf(fp, "%d\n", t->inc_size());
      }
    }
    
    fprintf(fp, "SCALARS ratio float 1\n");
    fprintf(fp, "LOOKUP_TABLE default\n");
    for (int i = 0; i < tetra_size(); i++)
    {
      Tetra *t = get_tetra(i);
      if (t->getState() == NOT_REFINED)
      {
	double curv = 0.0;
	for (int j = 0; j < t->inc_size(); j++)
        {
                Vertex *v = t->getInc(j);
		if (v->getCurvature() > curv)
		  curv = v->getCurvature();
        }
	fprintf(fp, "%f\n", t->getlenght()*curv);
      }
    }

    
    fclose(fp);
}

/*Tetra *Mesh::locate_vertex(Vertex *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}};
    Vertex *mv = _bm->get_closest(v);
//     printf("closest --> %p\n", mv);
    std::vector<Tetra*> tf;
    Tetra *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]->setFlag(-1);
            return t;
        }
    }
    assert(0);
}
*/

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

    _min = new Vertex;
    _max = new Vertex;

//   _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));
    _min->setPosition(center.x() - semidiag.x() - enlarge, center.y() - semidiag.y() - enlarge, center.z() - semidiag.z() - enlarge);
    _max->setPosition(center.x() + semidiag.x() + enlarge, center.y() + semidiag.y() + enlarge, center.z() + semidiag.z() + enlarge);
//   printf("%lf %lf %lf -- %lf %lf %lf\n", min.x(), min.y(), min.z(), max.x(), max.y(), max.z());
//   printf("%lf %lf %lf -- %lf %lf %lf\n", _min->x(), _min->y(), _min->z(), _max->x(), _max->y(), _max->z());

    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;

//     printf("ref size --> %lf\n", _ref_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);
    _center = new Vertex( (_max->x()+_min->x())/2. , (_max->y()+_min->y())/2. , (_max->z()+_min->z())/2. );

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

void Bucket::infos()
{
    printf("Bucket --> %d * %d * %d = %d buckets\n", _nx, _ny, _nz, (int)_bucket.size());
    printf(" --> contains %d vertices\n", _nbv);
}

void Bucket::dump_vtk(int num)
{
    char filename[100];
    sprintf(filename, "bucket_dump_%d.vtk", num);
    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", (_nx+1)*(_ny+1)*(_nz+1));
    for (int k = 0; k < _nz+1; k++)
        for (int j = 0; j < _ny+1; j++)
            for (int i = 0; i < _nx+1; i++)
                fprintf(fp, "%lf %lf %lf\n", _min->x()+i*_ref_size, _min->y()+j*_ref_size, _min->z()+k*_ref_size);

    fprintf(fp, "CELLS %d %d\n", _nx*_ny*_nz, 9*_nx*_ny*_nz);
    for (int k = 0; k < _nz; k++)
        for (int j = 0; j < _ny; j++)
            for (int i = 0; i < _nx; i++)
            {
                fprintf (fp, "%d", 8);
                fprintf (fp, " %d", (i) + (j)*(_nx+1) + (k)*(_nx+1)*(_ny+1));
                fprintf (fp, " %d", (i+1) + (j)*(_nx+1) + (k)*(_nx+1)*(_ny+1));
                fprintf (fp, " %d", (i+1) + (j+1)*(_nx+1) + (k)*(_nx+1)*(_ny+1));
                fprintf (fp, " %d", (i) + (j+1)*(_nx+1) + (k)*(_nx+1)*(_ny+1));
                fprintf (fp, " %d", (i) + (j)*(_nx+1) + (k+1)*(_nx+1)*(_ny+1));
                fprintf (fp, " %d", (i+1) + (j)*(_nx+1) + (k+1)*(_nx+1)*(_ny+1));
                fprintf (fp, " %d", (i+1) + (j+1)*(_nx+1) + (k+1)*(_nx+1)*(_ny+1));
                fprintf (fp, " %d", (i) + (j+1)*(_nx+1) + (k+1)*(_nx+1)*(_ny+1));
                fprintf (fp, "\n");
            }

    fprintf(fp, "CELL_TYPES %d\n", _nx*_ny*_nz);
    for (int i = 0; i < _nx*_ny*_nz; i++)
        fprintf(fp, "%d\n", 12);

}

int Bucket::contain(Vertex *pt)
{
//   printf("%lf %lf %lf\n", pt->x(), pt->y(), pt->z());
//   printf("%lf %lf %lf -- %lf %lf %lf\n", _min->x(), _min->y(), _min->z(), _max->x(), _max->y(), _max->z());
    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;
}

Vertex *Bucket::get_projection(Vertex *pt)
{
    Vertex v, vabs;
    Vertex vx(INFINITY,INFINITY,INFINITY);
    Vertex vy(INFINITY,INFINITY,INFINITY);
    Vertex vz(INFINITY,INFINITY,INFINITY);

    double ratiox = (_max->x()-_min->x())/2.;
    double ratioy = (_max->y()-_min->y())/2.;
    double ratioz = (_max->z()-_min->z())/2.;

    v.setVector(_center,pt);
    vabs.setPosition( fabs(v.x()) , fabs(v.y()) , fabs(v.z()) );
    if (v.x()!=0.) vx.setPosition( ratiox*v.x()/vabs.x() , ratiox*v.y()/vabs.x() , ratiox*v.z()/vabs.x() );
    if (v.y()!=0.) vy.setPosition( ratioy*v.x()/vabs.y() , ratioy*v.y()/vabs.y() , ratioy*v.z()/vabs.y() );
    if (v.z()!=0.) vz.setPosition( ratioz*v.x()/vabs.z() , ratioz*v.y()/vabs.z() , ratioz*v.z()/vabs.z() );

    if (( fabs(vx.y()) <= ratioy  &&  fabs(vx.z()) <= ratioz ) )
      return new Vertex(_center->x()+vx.x(), _center->y()+vx.y(), _center->z()+vx.z());
    if (( fabs(vy.x()) <= ratiox  &&  fabs(vy.z()) <= ratioz ) )
      return new Vertex(_center->x()+vy.x(), _center->y()+vy.y(), _center->z()+vy.z());
    if (( fabs(vz.x()) <= ratiox  &&  fabs(vz.y()) <= ratioy ) )
      return new Vertex(_center->x()+vz.x(), _center->y()+vz.y(), _center->z()+vz.z());

    printf("get_Projection error");
    return NULL;
}

void Bucket::get_coord(Vertex *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::get_indices(int index, int &x, int &y, int &z)
{
    x = index%_nx;
    z = floor(index/(_nx*_ny));
    y = (index-x-z*_nx*_ny)/_ny;
}

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

////////////////////////////////////////////////////////////
////////////// recherche le point le plus proche ///////////
////////////////////////////////////////////////////////////
Vertex *Bucket::get_closest_in_bucket(int index, Vertex *pt, double *sq_dist)
{
    Vertex *found = NULL;
//     printf("general lenght --> %lf\n", sq_dist);
    for (int i = 0; i < _bucket[index].size(); i++)
    {
        Vertex *pb = _bucket[index][i];
        Vertex ab;
        ab.setVector(pt, pb);
        double d = ab.sq_norm();
        if (d < *sq_dist)
        {
            found = pb;
            *sq_dist = d;
// 	    printf("new d = %lf\n", d);
        }
    }
    _flag[index] = _request_id;

    return found;
}

Vertex *Bucket::get_closest(Vertex *pt)
{
    Vertex *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;
    Vertex *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)
                    {
//                         printf("Bucket %d %d %d --> %d --> found = %p\n", i, j, k, index, found);
                        tmp = get_closest_in_bucket(index, pt, &dist);
                        if (tmp != NULL)
                            found = tmp;
                    }
                }
        level++;
    }

#if 0
    Vertex *cf = check(pt);
    if (cf != found)
    {
        printf("vertex %d != vertex %d\n", cf->getIndex(), found->getIndex());
        Vertex l1, l2;
        l1.setVector(pt, cf);
        l2.setVector(pt, found);
        printf("l1 = %lf != l2 = %lf\n", l1.norm(), l2.norm());
        getchar();
    }
#endif

    return found;
}

Vertex *Bucket::check(Vertex *pt)
{
    double dd = INFINITY;
    Vertex *found = NULL;
    int b;
    for (int i = 0; i < _bucket.size(); i++)
    {
        for (int j = 0; j < _bucket[i].size(); j++)
        {
            Vertex *pb = _bucket[i][j];
            Vertex ab;
            ab.setVector(pt, pb);
            double d = ab.sq_norm();
            if (d < dd)
            {
                found = pb;
                dd = d;
                b = i;
            }
        }
    }
    int x, y, z;
    get_coord(pt, x, y, z);
    int index = get_index(x, y, z);
//     printf("found was in bucket %d, pt was in %d\n", b, index);
    return found;
}

// ////////////////////////////////////////////////////////////
// //////////  recherche le point le plus proche    ///////////
// //////////      a une distance donnee pour       ///////////
// //////////      pour chacune des couleurs        ///////////
// ////////////////////////////////////////////////////////////
// std::vector<Vertex*> Bucket::get_closests_in_bucket(int index, Vertex *pt, double sq_dmax)
// {
//     std::map<int, double>::iterator it;
//     std::pair<std::map<int, double>::iterator, bool> ret;
//     std::vector<Vertex*> found;
//     for (int i = 0; i < _bucket[index].size(); i++)
//     {
//         Vertex *pb = _bucket[index][i];
//         int surf = pb->getSurf();
//         double sq_dmin = INFINITY;
//         it = _cps.find(surf);
//         if (it != _cps.end())
//             sq_dmin = it->second;
//         else
//             ret = _cps.insert(std::pair<int, double>(surf, sq_dmin));
//         Vertex ab;
//         ab.setVector(pt, pb);
//         double d = ab.sq_norm();
//         if (d < sq_dmin)
//         {
// 	    _found.insert(std::pair<int, Vertex *>(surf, pb));
// 	    ret.first->second = d;
//         }
//     }
//     _flag[index] = _request_id;
//
//     return found;
// }
//
// std::vector<Vertex*> Bucket::get_closest_in_ball(Vertex *pt, double dmax)
// {
//     Vertex *found = NULL;
//     double dist = INFINITY;
//
//     _request_id++;
//
//     int x, y, z;
//     get_coord(pt, x, y, z);
//     int index = get_index(x, y, z);
//
//     _cps.clear();
//     _found.clear();
//
//     int level = 0;
//     Vertex *tmp;
//
//     int imax;
//     int imin;
//     int jmax;
//     int jmin;
//     int kmax;
//     int kmin;
//
//     while ((found == NULL) || (level < 2) && (level < _maxn))
//     {
// //         printf("level --> %d\n", level);
//         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)
//                     {
// //                         printf("Bucket %d %d %d --> %d --> found = %p\n", i, j, k, index, found);
//                         tmp = get_closest_in_bucket(index, pt, &dist);
//                         if (tmp != NULL)
//                             found = tmp;
//                     }
//                 }
//         level++;
//     }
// //     return found;
// }

////////////////////////////////////////////////////////////
//////////  recherche le point le plus proche    ///////////
//////////      a une distance donnee pour       ///////////
//////////      pour chacune des couleurs        ///////////
//////////             indiquees                 ///////////
////////////////////////////////////////////////////////////
void Bucket::get_closests_in_bucket(int index, Vertex *pt)
{
    std::map<int, double>::iterator itd;
    std::map<int, Vertex *>::iterator itv;
    std::pair<std::map<int, double>::iterator, bool> ret;
//     std::vector<Vertex*> found;
    for (int i = 0; i < _bucket[index].size(); i++)
    {
        Vertex *pb = _bucket[index][i];
        int surf = pb->getSurf();
        double sq_dmin = INFINITY;
        itd = _cps.find(surf);
        if (itd != _cps.end())
        {
            sq_dmin = itd->second;
            Vertex ab;
            ab.setVector(pt, pb);
            double d = ab.sq_norm();
            if (d < sq_dmin)
            {
                itv = _found.find(surf);
                itv->second = pb;
                itd->second = d;
            }
        }
    }
    _flag[index] = _request_id;
}

std::vector<Vertex*> Bucket::get_closests(Vertex *pt, std::vector<int> surf)
{
    _request_id++;
    int x, y, z;
    get_coord(pt, x, y, z);
    int index = get_index(x, y, z);

    _cps.clear();
    _found.clear();

    for (int i = 0; i < surf.size(); i++)
    {
        _cps.insert(std::pair<int, double>(surf[i], INFINITY));
        Vertex* v = NULL;
        _found.insert(std::pair<int, Vertex* >(surf[i], v));
    }

    int found = 0;
    int level = 0;
    int imax;
    int imin;
    int jmax;
    int jmin;
    int kmax;
    int kmin;

    while ((!found) || (level < 2) && (level < _maxn))
    {
//         printf("level --> %d\n", level);
        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)
                        get_closests_in_bucket(index, pt);
                }
        level++;

        found = 1;
        for (int i = 0; i < surf.size(); i++)
            if (_found.find(surf[i])->second == NULL)
                found = 0;
    }

    assert(found);
    std::vector<Vertex*> results;
    for (int i = 0; i < surf.size(); i++)
    {
        Vertex *v = _found.find(surf[i])->second;
        results.push_back(v);
    }
    return results;
}

//////////////////////////////////////////////////////////
/////////////////         End bucket         /////////////
//////////////////////////////////////////////////////////

void cadLevelset::compute_edge_lenght(GEdge *edge, double tol, std::list<Vertex>& vlist, std::list<double>& plist)
{
    std::list<double> llist;
    std::queue< std::list<Vertex>::iterator > itvlist;
    std::queue< std::list<double>::iterator > itplist;
    std::queue< std::list<double>::iterator > itllist;
    std::list<Vertex>::iterator itv;
    std::list<double>::iterator itp;
    std::list<double>::iterator itl;
    double t0 = dynamic_cast<OCCEdge *>(edge)->parBounds(1).low();
    double t1 = dynamic_cast<OCCEdge *>(edge)->parBounds(1).high();
    GPoint pt;
    Vertex d;
//     printf("t0 = %lf ---> t1 = %lf\n", t0, t1);
    pt = edge->point(t0);
    vlist.push_back(Vertex(pt.x(), pt.y(), pt.z()));
    plist.push_back(t0);
    pt = edge->point(t1);
    vlist.push_back(Vertex(pt.x(), pt.y(), pt.z()));
    plist.push_back(t1);
    d.setVector(&vlist.front(), &vlist.back());
    double tl = d.norm();
//     printf("Initial lenght --> %lf\n", tl);
    llist.push_back(tl);
    itvlist.push(vlist.begin());
    itplist.push(plist.begin());
    itllist.push(llist.begin());
    while (!itvlist.empty())
    {
        itv = itvlist.front();
        Vertex v0 = *itv;
        std::advance(itv, 1);
        Vertex v1 = *itv;
        itp = itplist.front();
        t0 = *itp;
        std::advance(itp, 1);
        t1 = *itp;
// 	printf("t0 = %lf ---> t1 = %lf\n", t0, t1);
        itl = itllist.front();
        double ol = *itl;
// 	printf("old lenght --> %lf\n", ol);
        double tm = (t0+t1)/2.0;
        pt = edge->point(tm);
        Vertex vm(pt.x(), pt.y(), pt.z());
        d.setVector(&v0, &vm);
        double nl1 = d.norm();
        d.setVector(&vm, &v1);
        double nl2 = d.norm();
// 	printf("l1 = %lf -- l2 = %lf\n", nl1, nl2);
        assert(nl1+nl2 - ol >= -tol);
        if (nl1+nl2 - ol > tol*ol)
        {
            itv = vlist.insert(itv, vm);
            itvlist.push(itv);
            itp = plist.insert(itp, tm);
            itplist.push(itp);
            *itl = nl1;
            itl = llist.insert(itl, nl2);
            itllist.push(itl);
        }
        else
        {
            itvlist.pop();
            itplist.pop();
            itllist.pop();
        }
//         tl = 0.0;
//         for (itl = llist.begin(); itl != llist.end(); itl++)
//             tl += *itl;
// 	printf("Temp lenght --> %lf\n", tl);
    }
    tl = 0.0;
    for (itl = llist.begin(); itl != llist.end(); itl++)
        tl += *itl;
    edge->setLength(tl);

//     for (itp = plist.begin(); itp != plist.end(); itp++)
//         printf("--> %lf\n", *itp);
//     printf("Final lenght --> %lf\n", edge->length());
    return;
}

/*Vertex *cadLevelset::linearize(GEdge *edge, Tetra *t, int face, Vertex *v0, Vertex *v1, double t0, double t1, double tol)
{
  // compute plane
    Vertex *p1 = t->getFaceVertex(face, 0);
    Vertex *p2 = t->getFaceVertex(face, 1);
    Vertex *p3 = t->getFaceVertex(face, 2);
    Vertex u, v, n;
    u.setVector(p1, p2);
    v.setVector(p1, p3);
    n.crossproduct(&v, &n);
    n.normalize();
    Vertex pv, vv, inter;
    inter = v0;
    while (0)
    {
        vv.setVector(inter, v1);
        double tl = vv.dotproduct(&n);
        pv.setVector(inter, p1);
        double p = pv.dotproduct(&n)/tl;
        inter.setPosition(v0->x()+p*(v1->x()-v0->x()), v0->y()+p*(v1->y()-v0->y()), v0->z()+p*(v1->z()-v0->z()));
    }

}
*/

/*void cadLevelset::cut_edge_with_mesh(GEdge *edge, Mesh *m, double tol, std::list<Vertex>& vlist_inter, std::list<double>& plist_inter)
{
    std::list<Vertex> vlist;
    std::list<double> plist;
    std::list<Vertex>::iterator itvlist;
    std::list<double>::iterator itplist;

    // discretisation des edges en fonction de la courbure
    compute_edge_lenght(edge, tol, vlist, plist);
    assert(vlist.size() != 0);
    Vertex v0, v1;
    double p0, p1;
    double b[4];
    b[0] = b[1] = b[2] = b[3] = INFINITY;
    // recherche le tetra contenant le sommet
    v0 = vlist.front();
    v1 = vlist.front();
    p0 = plist.front();
    p1 = plist.front();
    vlist_inter.push_back(Vertex(v0.x(), v0.y(), v0.z()));
    plist_inter.push_back(p0);
    Tetra *t = m->locate_vertex(&v0, b);
//     printf("tetra %d\n", t->getIndex());
    assert(t != NULL);
    int seek = 0;
//     printf("vlist size --> %d\n", (int)vlist.size());
    itplist = plist.begin();
    for (itvlist = vlist.begin(); itvlist != vlist.end(); itvlist++, itplist++)
    {
        v0 = v1;
        v1 = *(itvlist);
// 	printf("position --> %d\n", seek);
        p0 = p1;
        p1 = *(itplist);
        while (t->contain(&v1))
        {
            v0 = v1;
            v1 = *(++itvlist);
            p0 = p1;
            p1 = *(++itplist);
            seek++;
//             printf("position --> %d\n", seek);
            if (itvlist == vlist.end())
            {
                vlist_inter.push_back(Vertex(v1.x(), v1.y(), v1.z()));
                plist_inter.push_back(p1);
                return;
            }
        }
        // vecteur du segment
        Vertex vtotal;
        vtotal.setVector(&v0, &v1);
        Vertex vpartiel;
//         printf("v0 --> %lf %lf %lf\n", v0.x(), v0.y(), v0.z());
//         printf("v1 --> %lf %lf %lf\n", v1.x(), v1.y(), v1.z());
        // v0 est le dernier vertex avant changement de tetra
        std::vector<Tetra*> tl;
        std::vector<int> fl;
        Vertex *init = &v0;
        Vertex *inter = NULL;
        Tetra *tb = NULL;
        int eb = -1;
        int face;
        int inside = 0;
        int vertex = -1;
        int edge = -1;
        inter = init;

        for (int i = 0; i < 4; i++)
        {
            tl.push_back(t);
            fl.push_back(i);
        }
        do
        {
// 	    printf("tetra list size --> %d\n", (int)tl.size());
            for (int i = 0; i < tl.size(); i++)
            {
                t = tl[i];
                face = fl[i];
// 		printf("tetra %d -- face %d\n", t->getIndex(), face);
                if (t->visibility(face, init, &v1, b, &inter, &inside, &vertex, &edge))
                {
                    tl.clear();
                    fl.clear();
                    // calcul des parametres
                    vpartiel.setVector(&v0, inter);
                    double tpartiel = vpartiel.norm()/vtotal.norm();
                    vlist_inter.push_back(Vertex(inter->x(), inter->y(), inter->z()));
                    plist_inter.push_back(p0+tpartiel*(p1-p0));
                    // une correction eventuelle serait a inserer ici !
                    //
                    //
                    // determination des elements a analyser ensuite
                    if (vertex != -1)
                    {
                        _m->compute_ball(inter);
                        for (int j = 0; j < _m->ball_size(); j++)
                        {
                            _m->get_tetra_ball(j, &tb, &eb);
                            tl.push_back(tb);
                            fl.push_back(eb);
                        }
                    }
                    else if (edge != -1)
                    {
                        _m->compute_shell(t, edge);
                        for (int j = 0; j < _m->shell_size(); j++)
                        {
                            _m->get_tetra_shell(j, &tb, &eb);
                            tl.push_back(tb);
                            fl.push_back(tb->getEdgeVertexIndex(eb, 0));
                            tl.push_back(tb);
                            fl.push_back(tb->getEdgeVertexIndex(eb, 1));
                        }
                    }
                    else
                    {
                        for (int j = 0; j < 4; j++)
                        {
                            if (j != t->getOpp(face))
                            {
                                tl.push_back(t->getAdj(face));
                                fl.push_back(j);
                            }
                        }
                    }
                    init = inter;
//                     printf("init = %lf %lf %lf\n", init->x(), init->y(), init->z());
                    break;
                }
                if (inside == 1)
                {
                    t = tl[i];
                    tl.clear();
                    fl.clear();
                    break;
                }
            }
        }
        while (!inside);

//         printf("tetra %d\n", t->getIndex());
        seek++;
//         getchar();
    }
    vlist_inter.push_back(Vertex(v1.x(), v1.y(), v1.z()));
    plist_inter.push_back(p1);
}
*/

DiscreteSurface *cadLevelset::add_levelset_if_tangetial_edge(GEdge *edge, double curv_tol, double dotmin, int max_step)
{
    DiscreteSurface *surf = NULL;

    std::list<Vertex> vlist;
    std::list<double> plist;
    std::list<Vertex>::iterator itvlist;
    std::list<double>::iterator itplist;
    
    std::vector<GFace *> faces = edge->faces();
    std::vector<GFace *>::iterator itface = faces.begin();
    
    int surf0 = _gface_map.find(*itface)->second;
    itface++;
    int surf1 = _gface_map.find(*itface)->second;

    compute_edge_lenght(edge, curv_tol, vlist, plist);
    assert(vlist.size() >= 2);

    int tangent = 0;
    int nb_step = 5;

    Vertex v0, v1;
    double p0, p1;
    double tstep;
    double dotmean = 0.0;

    Vertex uv;

    v0 = vlist.front();
    v1 = *(++vlist.begin());
//     v1 = vlist.front();
    p0 = plist.front();
    p1 = *(++plist.begin());
//     p1 = plist.front();
    itvlist = vlist.begin();
    itplist = plist.begin();
    tstep = (plist.back() - plist.front())/(double)max_step;
//     printf("tstep = %lf\n", tstep);

    progress_bar pbar;

    for (int i = 0; i < nb_step-1; i++)
    {
        if (tangent)
            pbar.progress(i);

        double t = plist.front()+(double)i*tstep;
        while (t >= p1)
        {
            v0 = v1;
            v1 = *(++itvlist);
            p0 = p1;
            p1 = *(++itplist);
        }

        uv.setVector(&v0, &v1);
        double lt = p1-p0;
        double l = t-p0;
        double tt = l/lt;
        Vertex *pt = new Vertex(v0.x()+tt*(v1.x()-v0.x()), v0.y()+tt*(v1.y()-v0.y()), v0.z()+tt*(v1.z()-v0.z()));
        // on recupere les sommets les plus proches des surfaces formant l'arete
	
//         std::vector<Vertex *> results = _gb->get_closests(pt, surf_id);
	//         assert(results.size() == 2);
	Vertex *cl0 = new Vertex;
	_lsurf[surf0]->get_closest(pt, &cl0);
	Vertex *cl1 = new Vertex;
	_lsurf[surf1]->get_closest(pt, &cl1);

        // on analyse
        Vertex *n1 = cl0->getNormal();
        Vertex *n2 = cl1->getNormal();
        double dot = fabs(n1->dotproduct(n2));
        dotmean += dot;
// 	printf("dot --> %lf\n", dot);
        if (tangent || dot > dotmin)
        {
            if (!tangent)
            {
                v0 = vlist.front();
                v1 = vlist.front();
                p0 = plist.front();
                p1 = plist.front();
                i = -1;
                tangent = 1;
                surf = new DiscreteSurface(NULL, _gface_map.size(), plist.front(), plist.back(), 0.0, 0.0, max_step);
                nb_step = max_step;
                pbar.start(nb_step);;
            }
            else
            {
                Vertex *n = new Vertex;
		double curv = 0.0;
                n1->crossproduct(&uv, n);
                pt->setNormal(n);
                surf->add_point(pt, pt->getNormal(), curv);
            }
        }
        else
            delete pt;
    }
    if (surf)
        pbar.end();

    return surf;
}

void cadLevelset::dump_surfaces()
{
      char filename[100];
    sprintf(filename, "surfaces.vtk");
    printf("output file %s\n", filename);

    FILE *fp = fopen(filename, "w");
    Vertex *pt;
    Vertex *n;
    double curv;
    
    int count = 0;
    for(int i = 0; i < _lsurf.size(); i++)
      count += _lsurf[i]->size();

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

    fprintf (fp, "POINTS %d float\n", count);

    for (int i = 0; i < _lsurf.size(); i++)
    {
        for (int j = 0; j < _lsurf[i]->size(); j++)
        {
            _lsurf[i]->get_point(j, &pt, &n, &curv);
            fprintf(fp, "%lf %lf %lf\n", pt->x(), pt->y(), pt->z());
        }
    }

    fprintf(fp, "CELLS %d %d\n", count, count*2);
    for (int i = 0; i < count; i++)
	fprintf(fp, "%d %d\n", 1, i);

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

    for (int i = 0; i < count; i++)
            fprintf(fp, "%d\n", 1);

    fprintf(fp, "POINT_DATA %d\n", count);
    fprintf(fp, "SCALARS curvature float 1\n");
    fprintf(fp, "LOOKUP_TABLE default\n");
    for (int i = 0; i < _lsurf.size(); i++)
    {
        for (int j = 0; j < _lsurf[i]->size(); j++)
        {
            _lsurf[i]->get_point(j, &pt, &n, &curv);
            fprintf(fp, "%lf\n", curv);
        }
    }
    
    fprintf(fp, "VECTORS normal float\n");
    for (int i = 0; i < _lsurf.size(); i++)
    {
        for (int j = 0; j < _lsurf[i]->size(); j++)
        {
            _lsurf[i]->get_point(j, &pt, &n, &curv);
	    fprintf(fp, "%lf %lf %lf\n", n->x(), n->y(), n->z());
        }
    }
    
    fclose(fp);
}

void cadLevelset::process(int max_mesh_size)
{
    char filename[100];
    
    // variables d'ajustement
    double relative_enlarge = 0.05;
    double surface_max_sample = 60;
    double tangent_dotproduct_threshold = 0.99;
    double curvature_threshold = 1e-3;
    // relative to refinement
    int max_refinement_level = 5;
    double error_threshold = 1e-2;
    // fin des variables d'ajustement

    std::map<GVertex*, int>::iterator itgv;
    std::map<GFace*, int>::iterator itgf;

    int surf_id = 0;
    progress_bar pbar(100);

    tprintf("Surface building START\n");
    
    if ( _fileType == 0)
    {
        int nb_face = _pModel->getNumFaces();
// 	printf("nb faces --> %d\n", nb_face);
        pbar.start(nb_face);
        for (GModel::fiter it = _pModel->firstFace(); it != _pModel->lastFace(); ++it)
        {
            GFace *gf = *it;
    //         printf("face %d --> %p\n", surf_id, gf);
            _gface_map.insert(std::pair<GFace*, int>(gf, surf_id));
            pbar.progress(surf_id);
            double umin = dynamic_cast<OCCFace *>(gf)->parBounds(0).low();
            double umax = dynamic_cast<OCCFace *>(gf)->parBounds(0).high();
            double vmin = dynamic_cast<OCCFace *>(gf)->parBounds(1).low();
            double vmax = dynamic_cast<OCCFace *>(gf)->parBounds(1).high();
            DiscreteSurface *surf = new DiscreteSurface(gf, surf_id, umin, umax, vmin, vmax, surface_max_sample);
            surf->generate();
            _lsurf.push_back(surf);
            surf_id++;
    // 	getchar();
        }
        pbar.end();
    }
    
    double global_bbmin[3] = {INFINITY, INFINITY, INFINITY};
    double global_bbmax[3] = {-INFINITY, -INFINITY, -INFINITY};
    for(int i = 0; i < _lsurf.size(); i++)
    {
        double surf_bbmin[3], surf_bbmax[3];
        _lsurf[i]->get_bbox(surf_bbmin, surf_bbmax);

//         printf("bound --> %lf %lf %lf %lf %lf %lf\n", surf_bbmin[0], surf_bbmin[1], surf_bbmin[2], surf_bbmax[0], surf_bbmax[1], surf_bbmax[2]);
        
        if (global_bbmin[0] > surf_bbmin[0])
            global_bbmin[0] = surf_bbmin[0];
        if (global_bbmin[1] > surf_bbmin[1])
            global_bbmin[1] = surf_bbmin[1];
        if (global_bbmin[2] > surf_bbmin[2])
            global_bbmin[2] = surf_bbmin[2];
        if (global_bbmax[0] < surf_bbmax[0])
            global_bbmax[0] = surf_bbmax[0];
        if (global_bbmax[1] < surf_bbmax[1])
            global_bbmax[1] = surf_bbmax[1];
        if (global_bbmax[2] < surf_bbmax[2])
            global_bbmax[2] = surf_bbmax[2];
    }

    Vertex min, max;
    min.setPosition(global_bbmin[0], global_bbmin[1], global_bbmin[2]);
    max.setPosition(global_bbmax[0], global_bbmax[1], global_bbmax[2]);
    
    dump_surfaces();

    tprintf("Surface building END\n");
        
#ifndef RECUT
//     if ( _fileType == 0)
    {
        _m = new Mesh;
        _m->create(&min, &max, max_mesh_size, relative_enlarge);
        _m->write(_basename);
    }
#endif

#if defined(REFINE)
    double max_curv_allowed = _m->evaluate_params(max_refinement_level, error_threshold);

    // refinement part
    _m->bucketize();
    for (int i = 0; i < _lsurf.size(); i++)
    {
        DiscreteSurface *surf = _lsurf[i];
        _m->add_included(surf);
    }
    
    int loop_ref = 0;    
    
    while (_m->adapt(max_curv_allowed, error_threshold))
    {
        printf("refinement loop --> %d\n", loop_ref++);
        _m->refine_triangulation();
    }

    char refinedname[100];
    sprintf(refinedname, "%s_refined", _basename);
    _m->write(refinedname);
#endif
    
//     if ( _fileType == 0)
//     {
//         tprintf("Bucket points START\n");
//         _gb = new Bucket(min, max, relative_enlarge*1.01, max_mesh_size/2);
// 
//         for (int i = 0; i < _lsurf.size(); i++)
//         {
//             Surface *surf = _lsurf[i];
//             for (int i = 0; i < surf->size(); i++)
//             {
//                 Vertex *v, *n;
//                 surf->get_point(i, &v, &n);
//                 v->setNormal(n);
//                 _gb->add_point(v);
//             }
//         }
//         tprintf("Bucket points END\n");
//     }

    
#if 1
    tprintf("Surface support building START\n");
    _m->renumber();
    _m->init_support_file(_basename);
    tprintf("Model contains %d surfaces\n", (int)_lsurf.size());

    _m->append_levelset_size(0);

    pbar.start(_lsurf.size());
    for (int i = 0; i < _lsurf.size(); i++)
    {
        pbar.progress(i);
        DiscreteSurface *surf = _lsurf[i];
        surf->bucketize(_m->get_longuest());
//  	surf->dump(i);
        std::vector<Tetra*> tlist;
        _m->build_support_opt(surf, tlist);
        //_m->build_support_opt_walk(surf, tlist);
//         _m->build_support(surf, tlist);
        _m->append_header(i, tlist.size());
        _m->append_support(tlist);
        _m->dump_support(i, tlist);
    }
    pbar.end();

    _m->append_levelset_size(_lsurf.size());
    tprintf("Surface support building END\n");
#endif

#if 1
    if ( _fileType == 0)
    {
        tprintf("Tangent support building START\n");
        _m->append_levelset_size(0);
        int nb_edge = _pModel->getNumEdges();
        tprintf("Model contains %d edges\n", nb_edge);
        int tanget_edge_count = 0;
        int edge_id = 0;
        pbar.start(nb_edge);
        for (GModel::eiter it = _pModel->firstEdge(); it != _pModel->lastEdge(); ++it)
        {
            pbar.progress(edge_id);
            // l'arete courante
            GEdge *edge = *it;
            std::list<Vertex> vlist;
            std::list<double> plist;
            std::list<Vertex>::iterator itvlist;
            std::list<double>::iterator itplist;
            DiscreteSurface *surf = add_levelset_if_tangetial_edge(edge, curvature_threshold, tangent_dotproduct_threshold, surface_max_sample);
            if (surf)
            {
                surf->bucketize(_m->get_longuest());
    // 	    surf->dump(-surf_id);
                std::vector<Tetra*> tlist;
                _m->build_support_opt(surf, tlist);
                //_m->build_support_opt_walk(surf, tlist);
                //_m->build_support(surf, tlist);
                int s1 = _gface_map.find(edge->faces().front())->second;
                int s2 = _gface_map.find(edge->faces().back())->second;
                _m->append_header(-surf_id, s1, s2, tlist.size()); // par convention, les levelsets "virtuelles" sont d'indice negatif !
                _m->append_support(tlist);
    //             _m->dump_support(-surf_id, tlist);
                surf_id++;
                tanget_edge_count++;
    //             getchar();
            }
            edge_id++;
        }
        pbar.end();
        // retour en arriere pour inscrire le nombre d'arete
        _m->append_levelset_size(tanget_edge_count);
        tprintf("Added %d tangent edges\n", tanget_edge_count);
        tprintf("Tangent support building END\n");
    }
    else if ( _fileType == 1)
        _m->append_levelset_size(0);
#endif

  

#if 0
    std::vector<Vertex> vertices;
    std::vector<std::vector<int> > edges;
    std::vector<std::vector<int> > edge_faces;

    for (GModel::viter it = _pModel->firstVertex(); it != _pModel->lastVertex(); ++it)
    {
        GVertex *gv = *it;
        _gvertex_map.insert(std::pair<GVertex*, int>(gv, vertex_id));
        vertices.push_back(Vertex(gv->x(), gv->y(), gv->z()));
        vertex_id++;
    }

    edges.resize(_pModel->getNumEdges());
    edge_faces.resize(_pModel->getNumEdges());

    for (GModel::eiter it = _pModel->firstEdge(); it != _pModel->lastEdge(); ++it)
    {
        // l'arete courante
        GEdge *edge = (*it);
        // le premier vertex
        GVertex *gv0 = edge->getBeginVertex();
        itgv = _gvertex_map.find(gv0);
        edges[edge_id].push_back(itgv->second);
        // les suivants
        std::list<Vertex> vlist;
        std::list<double> plist;
// 	compute_edge_lenght(edge, 1e-3, vlist, plist);
        cut_edge_with_mesh(edge, _m, 1e-3, vlist, plist);
        vlist.erase(vlist.begin());
        vlist.erase(--vlist.end());
        for (std::list<Vertex>::iterator itvx = vlist.begin(); itvx != vlist.end(); itvx++)
        {
            vertices.push_back(*itvx);
            edges[edge_id].push_back(vertex_id);
            vertex_id++;
        }
        // le dernier vertex
        GVertex *gv1 = edge->getEndVertex();
        itgv = _gvertex_map.find(gv1);
        edges[edge_id].push_back(itgv->second);

        std::list<GFace*> lf = edge->faces();
        for ( std::list<GFace*>::iterator itf = lf.begin(); itf != lf.end(); itf++)
        {
            itgf = _gface_map.find(*itf);
            edge_faces[edge_id].push_back(itgf->second);
        }
        edge_id++;
    }

    _m->append_topo_vertex_size(vertices.size());
    for (int i = 0; i < vertices.size(); i++)
    {
        _m->append_topo_vertex(i, vertices[i]);
    }

    _m->append_topo_edge_size(edges.size());
    for (int i = 0; i < edges.size(); i++)
    {
        _m->append_topo_edge(i, edges[i], edge_faces[i]);
    }
#endif

#if 0
    _m->close_support_file();
#endif
    tprintf("Support building END\n");
    
    if ( _fileType == 0)
    {
      tprintf("Topology file START\n");
      write_topo();
      tprintf("Topology file END\n");
    }
}

void cadLevelset::write_topo()
{
    char filename[100];
    sprintf(filename, "%s.topo", _basename);
    FILE *fp = fopen(filename, "w");
    fprintf(fp, "%d\n", _pModel->getNumVertices());
    int num = 0;
    for (GModel::viter it = _pModel->firstVertex(); it != _pModel->lastVertex(); it++)
    {
        GVertex *gv = *it;
        _gvertex_map.insert(std::pair<GVertex*, int>(*it, num));
        fprintf(fp, "%d\n", num);
        fprintf(fp, "%lf %lf %lf\n", gv->x(), gv->y(), gv->z());
        std::set<int> flist;
        std::vector<GEdge*> gle = gv->edges();
        for (std::vector<GEdge*>::iterator itle = gle.begin(); itle != gle.end(); itle++)
        {
            GEdge* ge = *itle;
            std::vector<GFace*> glf = ge->faces();
            for (std::vector<GFace*>::iterator itlf = glf.begin(); itlf != glf.end(); itlf++)
            {
                std::map<GFace*, int>::iterator itmf = _gface_map.find(*itlf);
                assert(itmf != _gface_map.end());
                flist.insert(itmf->second);
            }
        }
//         while (!flist.empty())
	for(int i = 0; i < 3; i++)
        {
            fprintf(fp, "%d ", *flist.begin());
            flist.erase(flist.begin());
        }
        fprintf(fp, "\n");
        num++;
    }
    fprintf(fp, "%d\n", _pModel->getNumEdges());
    num = 0;
    for (GModel::eiter it = _pModel->firstEdge(); it != _pModel->lastEdge(); it++)
    {
        GEdge *ge = *it;
        _gedge_map.insert(std::pair<GEdge*, int>(*it, num));
        fprintf(fp, "%d\n", num);
        std::vector<GFace*> glf = ge->faces();
        std::vector<GFace*>::iterator itlf;
        for (itlf = glf.begin(); itlf != glf.end(); itlf++)
        {
            std::map<GFace*, int>::iterator itmf = _gface_map.find(*itlf);
            assert(itmf != _gface_map.end());
            fprintf(fp, "%d ", itmf->second);
        }
        fprintf(fp, "\n");
        GVertex *gv = ge->getBeginVertex();
        std::map<GVertex*, int>::iterator itmv = _gvertex_map.find(gv);
        assert(itmv != _gvertex_map.end());
        fprintf(fp, "%d ", itmv->second);
        gv = ge->getEndVertex();
        itmv = _gvertex_map.find(gv);
        assert(itmv != _gvertex_map.end());
        fprintf(fp, "%d\n", itmv->second);
        num++;
    }
    fprintf(fp, "%d\n", _pModel->getNumFaces());
    num = 0;
    for (GModel::fiter it = _pModel->firstFace(); it != _pModel->lastFace(); it++)
    {
        GFace *gf = *it;
        _gface_map.insert(std::pair<GFace*, int>(*it, num));
        std::vector<GEdge*> gle = gf->edges();
        fprintf(fp, "%d %d\n", num, (int)gle.size());
        std::vector<GEdge*>::iterator itlfe;
        for (itlfe = gle.begin(); itlfe != gle.end(); itlfe++)
        {
            std::map<GEdge*, int>::iterator itme = _gedge_map.find(*itlfe);
            assert(itme != _gedge_map.end());
            fprintf(fp, "%d ", itme->second);
        }
        fprintf(fp, "\n");
        num++;
    }
    fclose(fp);
}

// void cadLevelset::createCutModel(char *lsfilename)
// {
//     GModel *_pModelCut = _pModel->buildCutGModel(lsfilename, 1);
//     _pModelCut->writeMSH("test_cut.msh",2.2,false,false);
// }