%module multisdf
%feature ("kwargs");
%feature("compactdefaultargs");
%init %{
  import_array(); /* load the Numeric PyCObjects */
%}

%{

#include "numpy/arrayobject.h"

static PyArrayObject *contiguous_typed_array(PyObject *obj, int typecode,
                                      int expectnd, int *expectdims)
{
  PyArrayObject *arr;
  int i;
  char buf[255];

  /* if the shape and type are OK, this function increments the reference
     count and arr points to obj */
  if((arr = (PyArrayObject *)PyArray_ContiguousFromObject(obj,
                                                          typecode, 0,
                                                          10)) == NULL)
    {
      sprintf(buf,"Failed to make a contiguous array of type %d\n", typecode);
      PyErr_SetString(PyExc_ValueError, buf);
      return NULL;
    }

  if(expectnd>0)
    {
      if(arr->nd > expectnd + 1 || arr->nd < expectnd)
        {
          Py_DECREF((PyObject *)arr);
          PyErr_SetString(PyExc_ValueError,
                          "Array has wrong number of dimensions");
          return NULL;
        }
      if(arr->nd == expectnd + 1)
        {
          if(arr->dimensions[arr->nd - 1] != 1)
            {
              Py_DECREF((PyObject *)arr);
              PyErr_SetString(PyExc_ValueError,
                              "Array has wrong number of dimensions");
              return NULL;
            }
        }
      if(expectdims)
        {
          for(i = 0; i < expectnd; i++)
            if(expectdims[i]>0)
              if(expectdims[i] != arr->dimensions[i])
                {
                  Py_DECREF((PyObject *)arr);
                  sprintf(buf,"The extent of dimension %d is %d while %d was expected\n",
                          i, arr->dimensions[i], expectdims[i]);
                  PyErr_SetString(PyExc_ValueError, buf);
                  return NULL;
                }
                  
        }
    }

  return arr;
}

#include "mesh_io.h"
#include "mds.h"
#include "sdf.h"
#include "kdtree.h"
#include "matrix.h"
#include "dt.h"


double BB_SCALE = 0.2;

 void make_labeled_mesh(Mesh &mesh, int nv, float* verts, int nf, int* faces, int *label)
   {
     int i;
     //cout << "nverts: " << nv << endl;
     mesh.set_nv(nv);
     mesh.set_nf(nf);
     for(i = 0; i < nv; i ++)
       {
	 float x,y,z;
	 x = verts[3*i+0];
	 y = verts[3*i+1];	
	 z = verts[3*i+2];
	 mesh.vert_list.push_back(MVertex(Point(x,y,z)));
	 mesh.vert_list[mesh.vert_list.size() - 1].id = i;
	 mesh.vert_list[mesh.vert_list.size() - 1].set_iso(true);
       }
     int ne = 0;
     //cout << "nfaces: " << nf << endl;
     for (i = 0; i <nf; i++)
       {
	 int v1, v2, v3;
	 v1 = faces[3*i+0];
	 v2 = faces[3*i+1];
	 v3 = faces[3*i+2];
	 mesh.face_list.push_back(MFace(v1,v2,v3));
	 mesh.face_list[mesh.face_list.size() - 1].id = i;
	 mesh.face_list[mesh.face_list.size() - 1].label = label[i];
	 
	 // add the incident faces in all three vertices.
	 mesh.vert_list[v1].add_inc_face(i);
	 mesh.vert_list[v1].set_iso(false);
	 mesh.vert_list[v2].add_inc_face(i);
	 mesh.vert_list[v2].set_iso(false);
	 mesh.vert_list[v3].add_inc_face(i);
	 mesh.vert_list[v3].set_iso(false);
	 // book keeping for incident vertices for a vertex.
	 if(!mesh.vert_list[v1].is_inc_vert(v2))
	   {
	     mesh.vert_list[v1].add_inc_vert(v2);
	     assert(!mesh.vert_list[v2].is_inc_vert(v1));
	     mesh.vert_list[v2].add_inc_vert(v1);
	     // here create an edge between v1 and v2.
	     // add the face to the edge
	     MEdge me = MEdge(v1,v2);
	     me.add_inc_face(i);
	     mesh.edge_list.push_back(me);
	     ne ++;
	     // insert the id of the incident edge in the vertices
	     mesh.vert_list[v1].add_inc_edge(mesh.edge_list.size() - 1);
	     mesh.vert_list[v2].add_inc_edge(mesh.edge_list.size() - 1);
	   }
	 else
	   {
	     // the edge is already there.
	     // find the edge id using the vertex indices.
	     MVertex mv = mesh.vert_list[v1];
	     int eid = -1;
#ifndef NDEBUG 
	     assert(mv.get_eid(v2, eid));
#else
	     mv.get_eid(v2, eid); 
#endif
	     assert(eid != -1);

	     // add the face to the edge.
	     if(mesh.edge_list[eid].num_inc_face <= 1)
	       {
		 assert(mesh.edge_list[eid].num_inc_face == 1);
		 mesh.edge_list[eid].add_inc_face(i);
	       }
	     else
	       mesh.edge_list[eid].num_inc_face ++; // nonmanifold.
	   }
	 
	 if(!mesh.vert_list[v2].is_inc_vert(v3))
	   {
	     mesh.vert_list[v2].add_inc_vert(v3);
	     assert(!mesh.vert_list[v3].is_inc_vert(v2));
	     mesh.vert_list[v3].add_inc_vert(v2);
	     // here create an edge between v2 and v3.
	     // add the face to the edge
	     MEdge me = MEdge(v2,v3);
	     me.add_inc_face(i);
	     mesh.edge_list.push_back(me);
	     ne ++;
	     
	     // insert the id of the incident edge in the vertices
	     mesh.vert_list[v2].add_inc_edge(mesh.edge_list.size() - 1);
	     mesh.vert_list[v3].add_inc_edge(mesh.edge_list.size() - 1);
	   }
	 else
	   {
	     // the edge is already there.
	     // find the edge id using the vertex indices.
	     MVertex mv = mesh.vert_list[v2];
	     int eid = -1;
#ifndef NDEBUG 
	     assert(mv.get_eid(v3, eid));
#else
	     mv.get_eid(v3, eid);
#endif
	     assert(eid != -1);
	     // add the face to the edge.
	     if(mesh.edge_list[eid].num_inc_face <= 1)
	       {
		 assert(mesh.edge_list[eid].num_inc_face == 1);
		 mesh.edge_list[eid].add_inc_face(i);
	       }
	     else
	       mesh.edge_list[eid].num_inc_face ++; // nonmanifold.
	   }
	 
	 if(!mesh.vert_list[v3].is_inc_vert(v1))
	   {
	     mesh.vert_list[v3].add_inc_vert(v1);
	     assert(!mesh.vert_list[v1].is_inc_vert(v3));
	     mesh.vert_list[v1].add_inc_vert(v3);
	     // here create an edge between v3 and v1.
	     // add the face to the edge
	     MEdge me = MEdge(v3,v1);
	     me.add_inc_face(i);
	     mesh.edge_list.push_back(me);
	     ne ++;
	     // insert the id of the incident edge in the vertices
	     mesh.vert_list[v3].add_inc_edge(mesh.edge_list.size() - 1);
	     mesh.vert_list[v1].add_inc_edge(mesh.edge_list.size() - 1);
	   }
	 else
	   {
	     // the edge is already there.
	     // find the edge id using the vertex indices.
	     MVertex mv = mesh.vert_list[v3];
	     int eid = -1;
#ifndef NDEBUG 
	     assert(mv.get_eid(v1, eid));
#else
	     mv.get_eid(v1, eid);
#endif
	     assert(eid != -1);
	     // add the face to the edge.
	     if(mesh.edge_list[eid].num_inc_face <= 1)
	       {
		 assert(mesh.edge_list[eid].num_inc_face == 1);
		 mesh.edge_list[eid].add_inc_face(i);
	       }
	     else
	       mesh.edge_list[eid].num_inc_face ++; // nonmanifold.
	   }
       }
     mesh.set_ne(ne);
     // add the edges in the face so that
     // if v1 is the ith corner of a face 
     // then v2<->v3 is the ith edge.
     for(int i = 0; i < nf; i ++)
       {
	 for(int j = 0; j < 3; j ++)
	   {
	     int u = mesh.face_list[i].get_corner((j+1)%3);
	     int w = mesh.face_list[i].get_corner((j+2)%3);
	     // find the edge id connecting u and w.
	     int eid = -1;
#ifndef NDEBUG 
	     assert(mesh.vert_list[u].get_eid(w, eid));
#else
	     mesh.vert_list[u].get_eid(w, eid);
#endif
	     assert(eid != -1);
	     // this edge should be the jth edge of the face.
	     mesh.face_list[i].set_edge(j, eid);
	   }
       }
     
   }

void
construct_bbox(const Mesh& mesh, vector<double>& bbox)
{
   double x_min = HUGE, x_max = -HUGE,
          y_min = HUGE, y_max = -HUGE,
          z_min = HUGE, z_max = -HUGE;
   for(int i = 0; i < mesh.get_nv(); i ++)
   {
      if( mesh.vert_list[i].iso() ) continue;
      Point p = mesh.vert_list[i].point();
      
      // check x-span
      if(CGAL::to_double(p.x()) < x_min) 
         x_min = CGAL::to_double(p.x());
      if(CGAL::to_double(p.x()) > x_max) 
         x_max = CGAL::to_double(p.x());
      // check y-span
      if(CGAL::to_double(p.y()) < y_min) 
         y_min = CGAL::to_double(p.y());
      if(CGAL::to_double(p.y()) > y_max) 
         y_max = CGAL::to_double(p.y());
      // check z-span
      if(CGAL::to_double(p.z()) < z_min) 
         z_min = CGAL::to_double(p.z());
      if(CGAL::to_double(p.z()) > z_max) 
         z_max = CGAL::to_double(p.z());
   }
   //cout << "scale: " << BB_SCALE << endl;
   bbox.push_back(x_min - BB_SCALE*(x_max-x_min));
   bbox.push_back(y_min - BB_SCALE*(y_max-y_min));
   bbox.push_back(z_min - BB_SCALE*(z_max-z_min));

   bbox.push_back(x_max + BB_SCALE*(x_max-x_min));
   bbox.push_back(y_max + BB_SCALE*(y_max-y_min));
   bbox.push_back(z_max + BB_SCALE*(z_max-z_min));
}

void
assign_sdf_weight(Mesh& mesh, vector<double>& weights)
{
   // map the color of each facet to a weight (scalar).
   // for the time being the information is in file called "weights".
   ifstream fin;
   fin.open("weights");
   istream_iterator<double> input(fin);
   istream_iterator<double> beyond;
   double tw = 0;
   while(input != beyond) { tw += *input; weights.push_back(*input); input++; }  
   for(int i = 0; i < (int)weights.size(); i ++) weights[i] /= tw;
}


float* computeSDF(int nverts, float* verts, int nfaces, int* faces, int*labels, 
		  int dimx, int dimy, int dimz, double maxval=-10, double minval=10)
  {
    Mesh mesh;
    cerr << "Creating input mesh ";
    make_labeled_mesh(mesh, nverts,verts, nfaces, faces, labels); 
    cerr << "done." << endl;
    
    // build a bounding box around the input and store the
    // origin, span etc.
    vector<double> bbox; bbox.clear();
    construct_bbox(mesh, bbox);
  
    // construct a kd-tree of all the non-isolated mesh_vertices.
    vector<VECTOR3> points;
    vector<Point> pts;
    //cout << "mesh nv: " << mesh.get_nv() << endl;
    for(int i = 0; i < mesh.get_nv(); i ++)
      {
	if( mesh.vert_list[i].iso() ) continue;
	Point p = mesh.vert_list[i].point();
	pts.push_back(p);
	points.push_back(VECTOR3(CGAL::to_double(p.x()),
				 CGAL::to_double(p.y()),
				 CGAL::to_double(p.z())));
      }
    KdTree kd_tree(points, 20);
    kd_tree.setNOfNeighbours(1);
    //Now perform a reconstruction to build a tetrahedralized solid
    // with in-out marked.
    Triangulation triang;
    recon(pts, triang);
    
    // assign weight to each triangle.
    vector<double> weights; weights.clear();
    // assign_sdf_weight(mesh, weights); // comment out for uniform weight.
    
    cerr << "SDF ";
    double xmin, ymin, zmin, xmax, ymax, zmax, x, y, z;
    xmin = bbox[0]; ymin = bbox[1]; zmin = bbox[2];
    xmax = bbox[3]; ymax = bbox[4]; zmax = bbox[5];

    double XSpan = (dimx-1 == 0) ? 0.0 : (xmax-xmin)/(dimx-1); 
    double YSpan = (dimy-1 == 0) ? 0.0 : (ymax-ymin)/(dimy-1);
    double ZSpan = (dimz-1 == 0) ? 0.0 : (zmax-zmin)/(dimz-1);
    
    int ind, jj, kk;
    
    float* sdfValues = (float*)(malloc(sizeof(float)*dimx*dimy*dimz));
    
    for(unsigned int k=0; k<dimz; k++)
      {
	kk = k*dimx*dimy; 
	z = zmin + k*ZSpan;
	for(unsigned int j=0; j<dimy; j++)
	  {
	    jj = j*dimx + kk;
	    y = ymin + j*YSpan;
	    for(unsigned int i=0; i<dimx; i++)
	      {
		x = xmin + i*XSpan;
		//ind = i + j*dimx + k*dimx*dimy;
		ind = i + jj;
		//cout << "i=" << i << ", j=" << j << ", k=" << k << ", ind=" << ind ;
		sdfValues[ind] = sdf(Point(x,y,z), mesh, weights, kd_tree, triang, maxval, minval);
		//cout << ", val= " << sdfValues[ind] << endl;
		//cout << sdfValues[ind] << " ";
	      }
	    //cout << endl;
	  }
        cerr << ".";
      }
    
    cerr << "done." << endl;
    
    return sdfValues;
  }
%}


// typemap to input an array of verts and triangles
%typemap(in) (int nverts, float* verts) (PyArrayObject *array, 
					 int expected_dims[2]) 
%{
  if ($input != Py_None)
  {
    expected_dims[0] = 0;
    expected_dims[1] = 3;		
    array = contiguous_typed_array($input, PyArray_FLOAT, 2, expected_dims);
    if (! array) return NULL;
    $1 = ((PyArrayObject *)(array))->dimensions[0];
    $2 = (float *)array->data;
    
  }
  else
  {
    array = NULL;
    $1 = 0;
    $2 = NULL;
  }
%}

%typemap(freearg) (int nverts, float* verts)
%{
   if (array$argnum)
      Py_DECREF((PyObject *)array$argnum);
%}

%typemap(in) (int nfaces, int* faces) (PyArrayObject *array, 
					 int expected_dims[2]) 
%{
  if ($input != Py_None)
  {
    expected_dims[0] = 0;
    expected_dims[1] = 3;		
    array = contiguous_typed_array($input, PyArray_INT, 2, expected_dims);
    if (! array) return NULL;
    $1 = ((PyArrayObject *)(array))->dimensions[0];
    $2 = (int *)array->data;
    
  }
  else
  {
    array = NULL;
    $1 = 0;
    $2 = NULL;
    
  }
%}

%typemap(freearg) (int nfaces, int* faces)
%{
   if (array$argnum)
      Py_DECREF((PyObject *)array$argnum);
%}

%typemap(in) (int* labels) (PyArrayObject *array, 
					 int expected_dims[1]) 
%{
  if ($input != Py_None)
  {
    expected_dims[0] = 0;
    array = contiguous_typed_array($input, PyArray_INT, 1, expected_dims);
    if (! array) return NULL;
    $1 = (int *)array->data;
    
  }
  else
  {
    array = NULL;
    $1 = NULL;
  }
%}

%typemap(freearg) ( int* labels)
%{
   if (array$argnum)
      Py_DECREF((PyObject *)array$argnum);
%}


%native(createNumArr)PyObject *Py_createNumArr(PyObject *self, PyObject *args);
%{
static PyObject *Py_createNumArr(PyObject *self, PyObject *args)
{
  PyObject * swigPt  = 0 ;
  int size;
  npy_intp dim[1];
  float *data;
  PyArrayObject *out;

  if(!PyArg_ParseTuple(args, "Oi", &swigPt, 
		       &size))
    return NULL;
  //cout << "size: " << size << endl;
  dim[0] = (npy_intp)size;
  if (swigPt) 
    {
        swig_type_info *ty = SWIG_TypeQuery("float *");
        if ((SWIG_ConvertPtr(swigPt, (void **)&data, ty,1)) == -1)
      {
         printf("utsdf,createNumArr: failed to convert a pointer\n");
         return NULL;
      }
    }
  //cout << "dim[0]: " << dim[0] << endl;
  out = (PyArrayObject *)PyArray_SimpleNewFromData(1, dim,
						 PyArray_FLOAT, 
						 (char *) data);
  //cout << "array dims: " << out->dimensions[0] << endl;
  
  if (!out) 
    {
      PyErr_SetString(PyExc_RuntimeError,
		      "Failed to allocate memory for normals");
      return NULL;
    }

#ifdef _MSC_VER
  switch ( WinVerMajor() )
  {
    case 6: break; // Vista
	default: out->flags |= NPY_OWNDATA;
  }
#else
  // so we'll free this memory when this
  // array will be garbage collected
  out->flags |= NPY_OWNDATA; 
#endif

  return Py_BuildValue("O", (PyObject *)out);
}
%}

extern double BB_SCALE;

float* computeSDF(int nverts, float* verts, int nfaces, int* faces, int*labels, 
		  int dimx, int dimy, int dimz, double maxval=10, double minval=-10);
