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

#include "discreteSurface.h"
#include "mesh.h"

DiscreteSurface::~DiscreteSurface()
{
#if defined(HAVE_ANN)
    delete _kdtree;
#else
    delete _bs;
#endif
}


void DiscreteSurface::point(double uu, double vv, LVertex *pt, LVertex *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)
{
     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 = ls_id;

    int nbvx = 0;
    if (fscanf(fp, "%d", &nbvx) != 1)
      error++;  
        
    int i;
    LVertex *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 %lf", &ptx, &pty, &ptz, &nx, &ny, &nz, &c) != 7)
	  error++;
        pt = new LVertex(ptx, pty, ptz);
        n = new LVertex(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++)
        {
            LVertex *pt = new LVertex;
            LVertex *n = new LVertex;
	    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(); i++)
    {
        LVertex *pt = NULL;
        LVertex *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;
#else
    _bs = new Bucket(LVertex(_bbmin[0], _bbmin[1], _bbmin[2]), LVertex(_bbmax[0], _bbmax[1], _bbmax[2]), longuest_mesh_edge, floor(_size/5));
    for (int i = 0; i < size(); i++)
    {
        LVertex *pt = NULL;
        LVertex *n = NULL;
        double d;
        get_point(i, &pt, &n,&d);
        _bs->add_point(pt);
    }
#endif
}

void DiscreteSurface::get_closest(LVertex *pt, LVertex **cl, LVertex **cln)
{
  #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 LVertex;
    (*cl)->setPosition(pcl[0], pcl[1], pcl[2]);    
    (*cln)->setPosition(pcl[3], pcl[4], pcl[5]);
  #else
    if (_bs->contain(pt))
      *cl = _bs->get_closest(pt);
    else
    {
//      LVertex *proj = _bs->get_projection(pt);
      LVertex *proj = pt;
      *cl = _bs->get_closest(proj);
    }
    assert (*cl != NULL);
  #endif
}

void DiscreteSurface::distance(LVertex *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;
    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]];
    LVertex cl(pcl[0], pcl[1], pcl[2]);
    LVertex n(pcl[3], pcl[4], pcl[5]);
    LVertex ptcl;
    ptcl.setVector(pt, &cl);
    ptcl.dotproduct(&n);
    simpledist = ptcl.norm();
    projdist = ptcl.dotproduct(&n);    
//     return ptcl.dotproduct(&n);
#else
    LVertex *cl = NULL;
    get_closest(pt, &cl);
    LVertex *n = cl->getNormal();
    LVertex 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");
    LVertex *pt;
    LVertex *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);
}