/* This software and supporting documentation are distributed by
 *     Institut Federatif de Recherche 49
 *     CEA/NeuroSpin, Batiment 145,
 *     91191 Gif-sur-Yvette cedex
 *     France
 *
 * This software is governed by the CeCILL-B license under
 * French law and abiding by the rules of distribution of free software.
 * You can  use, modify and/or redistribute the software under the
 * terms of the CeCILL-B license as circulated by CEA, CNRS
 * and INRIA at the following URL "http://www.cecill.info".
 *
 * As a counterpart to the access to the source code and  rights to copy,
 * modify and redistribute granted by the license, users are provided only
 * with a limited warranty  and the software's author,  the holder of the
 * economic rights,  and the successive licensors  have only  limited
 * liability.
 *
 * In this respect, the user's attention is drawn to the risks associated
 * with loading,  using,  modifying and/or developing or reproducing the
 * software by the user in light of its specific status of free software,
 * that may mean  that it is complicated to manipulate,  and  that  also
 * therefore means  that it is reserved for developers  and  experienced
 * professionals having in-depth computer knowledge. Users are therefore
 * encouraged to load and test the software's suitability as regards their
 * requirements in conditions enabling the security of their systems and/or
 * data to be ensured and,  more generally, to use and operate it in the
 * same conditions as regards security.
 *
 * The fact that you are presently reading this means that you have had
 * knowledge of the CeCILL-B license and that you accept its terms.
 */

// activate deprecation warning
#ifdef AIMSDATA_CLASS_NO_DEPREC_WARNING
#undef AIMSDATA_CLASS_NO_DEPREC_WARNING
#endif

#include <cstdlib>
#include <aims/mesh/curv.h>
#include <set>
#include <algorithm>
#include <float.h>
#define use_boost
#ifdef use_boost
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#if BOOST_VERSION < 103300
typedef boost::numeric::ublas::sparse_matrix<float> boost_sparse_matrix;
#else
typedef boost::numeric::ublas::mapped_matrix<float> boost_sparse_matrix;
#endif
#endif

using namespace std;

//All the functions for
//Curvature and Laplacian estimations

float fsign(float x)
{
  if (x>0) return(1);
  else
    if (x<0) return(-1);
    else
      return(0);
}

// Cross product between two 3D points
Point3df cross(Point3df a, Point3df b)
{

  Point3df n;
  n[0] = a[1]*b[2] - a[2]*b[1];
  n[1] = a[2]*b[0] - a[0]*b[2];
  n[2] = a[0]*b[1] - a[1]*b[0];
  return (n);
}

TimeTexture<float> AimsMeshCurvature( const AimsSurface<3,Void> & mesh)
{
  TimeTexture<float>				tex;
  const vector<Point3df>			& vert = mesh.vertex(), & normal = mesh.normal() ;
  const vector< AimsVector<uint,3> >		& poly = mesh.polygon();
  unsigned					i, n = vert.size();
  vector<float>					bary(n), bary2(n);
  map<unsigned, set<unsigned> >			neighbours;
  set<unsigned>::iterator 	                inei,enei;
  unsigned					v1, v2, v3;
  Point3df					a;

  //  neighbours map

  for( i=0; i<poly.size(); ++i )
    {
      v1 = poly[i][0];
      v2 = poly[i][1];
      v3 = poly[i][2];

      neighbours[v1].insert( v2 );
      neighbours[v2].insert( v1 );
      neighbours[v1].insert( v3 );
      neighbours[v3].insert( v1 );
      neighbours[v2].insert( v3 );
      neighbours[v3].insert( v2 );

    }

  //Barycenter and sign
  for (i=0; i<n; ++i)
    {
      a = Point3df(0,0,0) ;
      bary[i] = 0 ;
      for (inei = neighbours[i].begin(), enei = neighbours[i].end(); inei != enei; ++inei)
	a += vert[*inei];
      a /= neighbours[i].size(); //barycenter
      a = vert[i] - a;
      bary[i] = a.norm() * fsign( a.dot(normal[i]) );
      bary2[i] = a.dot(normal[i] );
      tex[0].push_back( bary[i] ); //barycenter
      //tex[1].push_back( bary[i] );

    }

  return(tex);
}

Texture<float> AimsMeshBoixCurvature( const AimsSurface<3,Void> & mesh,
				      const vector<float> & ALPHA,
				      const vector<float> & BETA,
				      const vector<list <float> > & SURFACE)
{
  const vector<Point3df>			& vert = mesh.vertex() ;
  unsigned					i, n = vert.size();
  Texture<float>				tex(n);
  float                                         H,nume,deno,aire;
  list <float>::const_iterator              il,el;

  for (i=0; i<n; ++i)
    {
      nume = BETA[i];
      aire = 0;
      for (il = SURFACE[i].begin(), el = SURFACE[i].end(); il != el; ++il)
	aire += *il;

      deno = (aire * 0.5 + ALPHA[i] );
      H = nume / deno;
      tex.item(i) = H;
    }

  return(tex);
}

Texture<float> AimsMeshFiniteElementCurvature( const AimsSurface<3,Void> & mesh,
					       const vector< list<unsigned> > & neighbourso,
					       const vector< list<float> > & PHI,
					       const vector< list<float> > & THETA,
					       const vector< list<float> > & SURFACE,
					       const vector< list<float> > & DOT)
{
  const vector<Point3df>			& vert = mesh.vertex() ;
  unsigned					i, n = vert.size();
  Texture<float>				tex(n);
  list<unsigned>::const_iterator		ilist,elist;
  list<float>::const_iterator		        iphi,idot,itheta,isurf;
  unsigned					/*nb,*/s,p,d,t;
  float 					K,surface;

  for (i=0; i<n; ++i)
    {
      s = SURFACE[i].size();
      p = PHI[i].size();
      t = THETA[i].size();
      d = DOT[i].size();
      //nb = neighbourso[i].size() ;
      if ( !( s == p && s == t && s == d && p==t && p==d && t==d ) )
	{
	  cout << "Problem with the mesh features..." << endl;
	  ASSERT(0);
	}
      K = 0;
      surface = 0;
      for ( elist =  neighbourso[i].end(),ilist = neighbourso[i].begin(),
	      iphi = PHI[i].begin(),idot = DOT[i].begin(),
	      isurf = SURFACE[i].begin(), itheta = THETA[i].begin();
	    ilist != elist;++ilist, ++iphi,++itheta,++idot,++isurf )
	{
	  K +=(*iphi + *itheta)*(*idot);
	  surface += *isurf;
	  //cout << *iphi << " " << *itheta << " " <<  *idot << " " << *isurf << endl;
	}

     if ( surface != 0 )
       K = K/(surface * 4);
     else
       {
	 cout << "Triangle with null surface\n";
	 K = 0;
       }
     tex.item(i) = K;
    }

  return(tex);
}

vector< list<float> > AimsMeshFiniteElementPhi( const AimsSurface<3,Void> & mesh,
						const vector< list<unsigned> > & neighbourso,
						const vector< list<float> > & surf)
{
  const vector<Point3df>			& vert = mesh.vertex() ;
  unsigned					i, n = vert.size();
  vector< list<float> >                         PHI(n);//id but triangle are put in order
  list<unsigned>::iterator        		ilist,elist;
  list<unsigned>::const_iterator        	el;
  list<float>::const_iterator        	        efl,iflist;
  unsigned					v1, v2;
  float						phi,t;
  vector< list<unsigned> >                     neigho = neighbourso;
  vector< list<float> >                        surfl = surf;

  for (i=0; i<n; ++i)
    {
      el = neighbourso[i].end();
      --el;
      neigho[i].push_front(*el);
      efl = surf[i].end();
      --efl;
      surfl[i].push_front(*efl);
    }

  for (i=0; i<n; ++i)
    {
      phi = 0;
      ASSERT(neigho[i].size() == surfl[i].size() );
      iflist = surfl[i].begin();
      elist = neigho[i].end();
      --elist;
      ilist = neigho[i].begin();
      while ( ilist != elist )
	{
	  v1 = *ilist;
	  ++ilist;
	  v2 = *ilist;
	  t = *iflist;
	  ++iflist;
	  if (fabs(t) != 0 )
	      phi =   ( ( vert[v1] - vert[i] ).dot( vert[v1] - vert[v2]  ) )/(2 * t);
	  else
	    {
	      cout << "Triangle with a  null surface \n";
	      phi = 0;
	    }
	  PHI[i].push_back(phi);
	}
    }

  return(PHI);
}

vector<float> AimsMeshFiniteElementBeta( const AimsSurface<3,Void> & mesh,
					 const vector< list<unsigned> > & neighbourso)
{
  const vector<Point3df>			& vert = mesh.vertex(), &normal = mesh.normal() ;
  unsigned					i, n = vert.size();
  vector<float>                                 BETA(n);
  list<unsigned>::iterator       		ilist,elist;
  list<unsigned>::const_iterator       		il,el;
  unsigned					v1, v2, v3;
  float						b,nj1n,nj2n;
  Point3df                                      nj1,nj2;
  vector< list<unsigned> >                      neigho = neighbourso;

  for (i=0; i<n; ++i)
    {
      il = neighbourso[i].begin();
      el = neighbourso[i].end();
      --el;
      neigho[i].push_front(*el);
      neigho[i].push_back(*il);
    }

  for (i=0; i<n; ++i)
    {
      b = 0;
      elist =  neigho[i].end();
      --elist;
      --elist;
      ilist = neigho[i].begin();
      while ( ilist != elist )
	{
	  v1 = *ilist;
	  ++ilist;
	  v2 = *ilist;
	  ++ilist;
	  v3 = *ilist;
	  --ilist;
	  nj1 = (normal[i] + normal[v1] + normal[v2]);
	  nj2 = (normal[i] + normal[v3] + normal[v2]);
	  nj1n = nj1.norm();
	  nj2n = nj2.norm();
	  if (nj1n  != 0 &&  nj2n != 0)
	    {
	      nj2 /= nj2n;
	      nj1 /= nj1n;
	      if (nj1.dot(nj2) != 1)
		b += (vert[v2] - vert[i]).norm() * acos( (double)nj1.dot(nj2) );

	    }
	  else
	    cout << "Triangle of size null\n";

	}
      BETA[i] = 0.25 * b;
    }

  return(BETA);
}

vector<float> AimsMeshFiniteElementAlpha( const AimsSurface<3,Void> & mesh,
					  const vector< list<unsigned> > & neighbourso)
{
  const vector<Point3df>			& vert = mesh.vertex() ;
  unsigned					i, n = vert.size();
  vector<float>                                 ALPHA(n);
  list<unsigned>::iterator       		ilist,elist;
  unsigned					v1, v2;
  float						a,t;
  Point3df                                      nj1,nj2;
  vector< list<unsigned> >                      neigho = neighbourso;

  for (i=0; i<n; ++i)
    neigho[i].push_front(*neigho[i].rbegin());//the first element is the last one (circular)

  for (i=0; i<n; ++i)
    {
      a = 0;
      elist =  neigho[i].end();
      --elist;
      ilist = neigho[i].begin();
      while ( ilist != elist )
	{
	  v1 = *ilist;
	  ++ilist;
	  v2 = *ilist;
	  nj1 = (vert[i] - vert[v2]);
	  nj2 = (vert[i] - vert[v1]);
	  t = 0.5 * fabs(cross(nj1,nj2).norm());
	  if ( t != 0)
	    a +=(vert[v1] - vert[v2]).norm2() * nj1.dot(nj2)/(2*t);
	}
       ALPHA[i] = -a/8;
    }

  return(ALPHA);
}

vector< list<float> > AimsMeshFiniteElementDot( const AimsSurface<3,Void> & mesh,
						const vector< list<unsigned> > & neighbourso)
{

  const vector<Point3df>			& vert = mesh.vertex(), & normal = mesh.normal() ;
  unsigned					i, n = vert.size();
  vector< list<float> >                         DOT(n);
  list<unsigned>::const_iterator		ilist,elist;

  for (i=0; i<n; ++i)
    for (ilist = neighbourso[i].begin(),elist =  neighbourso[i].end(); ilist != elist ; ++ilist)
      DOT[i].push_back(normal[i].dot( - vert[*ilist] + vert[i]) );
  //DOT[i].push_back(normal[i].dot( vert[*ilist] - vert[i]) ); //BUG in CHUNG paper ?! YES : SEE ref. Desbrun99

  return(DOT);
}

vector< list<float> > AimsMeshFiniteElementTheta( const AimsSurface<3,Void> & mesh,
						  const vector< list<unsigned> > & neighbourso,
						  const vector< list<float> > & surf)
{
  const vector<Point3df>			& vert = mesh.vertex() ;
  unsigned					i, n = vert.size();
  vector< list<float> >                         THETA(n);
  list<float>::const_iterator		        il;
  list<unsigned>::iterator       		ilist,elist;
  unsigned					v1, v2;
  float						theta,t;
  vector< list<unsigned> >                      neigho = neighbourso;
  vector< list<float> >                         surfl=surf;

  for (i=0; i<n; ++i)//the first element is the same as the last one (circular)
    {
      neigho[i].push_back(*neighbourso[i].begin());
      surfl[i].push_back(*surf[i].begin());
    }

  for (i=0; i<n; ++i)
    {
      theta = 0;
      il = surfl[i].begin();
      elist =  neigho[i].end();
      --elist;
      ilist = neigho[i].begin();
      ASSERT(neigho[i].size() == surfl[i].size() );
      while ( ilist != elist )
	{
	  v1 = *ilist;
	  ++ilist;
	  v2 = *ilist;
	  t = *il;
	  ++il;
	  if (fabs(t)  != 0)
	    theta = ( ( vert[v2] - vert[i] ).dot( vert[v2] - vert[v1]  ) )/(2 * t);
	  else
	    {
	      cout << "Triangle with null surface \n";
	      theta = 0;
	    }
	  THETA[i].push_back(theta);
	}
    }

  return(THETA);
}

//ie the sum of all the neighbouring triangle surface
vector< list<float> > AimsMeshFiniteElementSurface( const AimsSurface<3,Void> & mesh,
						    const vector< list<unsigned> > & neighbourso)
{
  const vector<Point3df>			& vert = mesh.vertex() ;
  unsigned					i, n = vert.size();
  vector< list<float> >                         SURFACE(n);
  list<unsigned>::iterator			ilist,elist;
  unsigned					v1, v2;
  float						t;
  vector< list<unsigned> >                      neigho = neighbourso;

  for (i=0; i<n; ++i)
    neigho[i].push_back(*neighbourso[i].begin());//the first element is the last one (circular)

  for (i=0; i<n; ++i)
    {
      elist =  neigho[i].end();
      --elist;
      ilist = neigho[i].begin();
      while ( ilist != elist )
	{
	  v1 = *ilist;
	  ++ilist;
	  v2 = *ilist;
	  t = 0.5 * ( cross(vert[v2] - vert[i], vert[v1] - vert[i]).norm() );
	  SURFACE[i].push_back(t);
	}
    }

  return(SURFACE);
}

vector< list<unsigned> > AimsMeshOrderNode(const AimsSurface<3,Void> & mesh)
{
  const vector<Point3df>			& vert = mesh.vertex(), & normal = mesh.normal() ;
  const vector< AimsVector<uint,3> >		& poly = mesh.polygon();
   unsigned					i,n = vert.size();
   vector<list<unsigned> >                       neighbourso(n);//id but nodes are put in order
  vector< list<unsigned> >                      trineigho(n);//id but triangle are put in order
  list<unsigned>::iterator			ilist,elist,jlist;

  trineigho = AimsMeshOrderTriangle(mesh);

  for ( i=0; i<n; ++i)
    {
      set<unsigned> setinter;
      insert_iterator<set<unsigned> > ii( setinter,setinter.begin() );
      elist=trineigho[i].end();
      --elist;
      ilist= trineigho[i].begin();
      set<unsigned> tn1,tn2;
      tn1.insert(poly[*ilist][0]);
      tn1.insert(poly[*ilist][1]);
      tn1.insert(poly[*ilist][2]);
      //cout << poly[*ilist][0] << " " << poly[*ilist][1] << " "<< poly[*ilist][2]<< endl;
      tn1.erase(i);
      ++ilist;
      tn2.insert(poly[*ilist][0]);
      tn2.insert(poly[*ilist][1]);
      tn2.insert(poly[*ilist][2]);
      //cout << poly[*ilist][0] << " " << poly[*ilist][1] << " "<< poly[*ilist][2]<< endl;
      tn2.erase(i);
      set_intersection( tn1.begin(),tn1.end(),tn2.begin(),tn2.end(),ii );
      tn1.erase(*setinter.begin());
      tn2.erase(*setinter.begin());
      //cout << "setinter size: " << setinter.size() << endl;
      neighbourso[i].push_back(*tn1.begin() );
      neighbourso[i].push_back(*setinter.begin());
      neighbourso[i].push_back(*tn2.begin() );

      while (ilist != elist)
	{
	  //cout << "in loop" << endl;
	  tn1.clear();
	  tn2.clear();
	  tn1.insert(poly[*ilist][0]);
	  tn1.insert(poly[*ilist][1]);
	  tn1.insert(poly[*ilist][2]);
	  //cout << poly[*ilist][0] << " " << poly[*ilist][1] << " "<< poly[*ilist][2]<< endl;
	  tn1.erase(i);
	  ++ilist;
	  tn2.insert(poly[*ilist][0]);
	  tn2.insert(poly[*ilist][1]);
	  tn2.insert(poly[*ilist][2]);
	  //cout << poly[*ilist][0] << " " << poly[*ilist][1] << " "<< poly[*ilist][2]<< endl;
	  tn2.erase(i);

	  setinter.clear();
	  insert_iterator<set<unsigned> > jj( setinter,setinter.begin() );
	  set_intersection( tn1.begin(),tn1.end(),tn2.begin(),tn2.end(),jj );
	  //cout << "setinter size: " << setinter.size() << endl;
	  tn2.erase(*setinter.begin());
	  neighbourso[i].push_back(*tn2.begin() );
	}
    }

  for (i=0; i<n;++i)
    neighbourso[i].pop_front();//remove the repeated first element

  for (i=0; i<n;++i)
    {
      //cout << i << " " << neighbourso[i].size() << endl;
      Point3df nor = normal[i],a,b;
      //elist=neighbourso[i].end();
      ilist=neighbourso[i].begin();
      a = vert[*ilist] - vert[i];
      jlist = ilist;
      ++jlist;
      b = vert[*jlist] - vert[*ilist];
      if ( cross(a,b).dot(nor) < 0 )//clockwise ordering
	neighbourso[i].reverse();
    }

  return(neighbourso);
}

vector< list<unsigned> > AimsMeshOrderTriangle(const AimsSurface<3,Void> & mesh)
{
  const vector<Point3df>			& vert = mesh.vertex() ;
  const vector< AimsVector<uint,3> >		& poly = mesh.polygon();
  unsigned					i,j,n = vert.size();
  unsigned					v1, v2, v3;
  map<unsigned, set<unsigned> >			neighbours;//give for each vertex a list of the  adjacent triangles
  vector< set <unsigned> >                      trineigh(n); //give for each vertex a list of the  adjacent triangles
  vector< list<unsigned> >                      trineigho(n);//id but triangle are put in order
  set<unsigned>::iterator 	                inei,enei;

   //  neighbours map

  for( i=0; i<poly.size(); ++i )
    {
      v1 = poly[i][0];
      v2 = poly[i][1];
      v3 = poly[i][2];

      neighbours[v1].insert( v2 );
      neighbours[v2].insert( v1 );
      neighbours[v1].insert( v3 );
      neighbours[v3].insert( v1 );
      neighbours[v2].insert( v3 );
      neighbours[v3].insert( v2 );

      trineigh[v1].insert( i );
      trineigh[v2].insert( i );
      trineigh[v3].insert( i );
    }

  //order neighbour nodes
  set<unsigned>	        nodes;
  set<unsigned>		tmpNodes;
  bool			stopLoop=false;
  for ( i=0; i<n; ++i)
    {
      // cout << i << " -> " ;
      tmpNodes = trineigh[i];
      inei = tmpNodes.begin();
      trineigho[i].push_back( *inei );
      tmpNodes.erase( inei );
      while ( !tmpNodes.empty() )
	{
	  enei = tmpNodes.end();
	  stopLoop = false;
	  for ( inei=tmpNodes.begin(); inei!=enei && !stopLoop; ++inei )
	    {
	      nodes.clear();
	      j = trineigho[i].back();
	      nodes.insert(poly[*inei][0]);
	      nodes.insert(poly[*inei][1]);
	      nodes.insert(poly[*inei][2]);
	      nodes.insert(poly[j][0]);
	      nodes.insert(poly[j][1]);
	      nodes.insert(poly[j][2]);
	      if (nodes.size() == 4) // 2 adjacent triangles ?
		{
		  trineigho[i].push_back(*inei);
		  tmpNodes.erase( inei );
		  stopLoop = true;
		  //cout << *inei << " ";
		}
	    }
          if( !stopLoop )
          {
            cout << "warning, mesh is not closed.\n";
            tmpNodes.clear();
          }
	}
      // cout << endl;
    }
  return(trineigho);
}

Texture<float> AimsMeshLaplacian( const Texture<float> &inittex,
                                  const map<unsigned, set< pair<unsigned,float> > > &lapl)
{
  unsigned					n =inittex.nItem();
  Texture<float>				tex(n);
  AimsMeshLaplacian( inittex.data(), tex.data(), lapl );

  return(tex);
}

template <typename T>
void AimsMeshLaplacian( const vector<T> &inittex, vector<T> & outtex,
                        const map<unsigned, set< pair<unsigned,float> > > &lapl)
{
  unsigned neigh,node, n =inittex.size();
  map<unsigned, set< pair<unsigned,float> > >::const_iterator il,el;
  set< pair<unsigned,float> >::iterator      ip,ep;
  float                                        L,weight;
  ASSERT ( lapl.size() == n);
  // resize/clear output texture
  if( outtex.size() != n )
    outtex.resize( n );

  for (il = lapl.begin(), node=0, el = lapl.end(); il != el; ++il)
    {
      node = il->first;
      L = 0;

      //Weighted sum on the neighbour of the node
      for ( ip = (il->second).begin(), ep = (il->second).end(); ip != ep; ++ip    )
        {
          neigh = ip->first;
          weight = ip->second;
          L += weight * (- inittex[node] + inittex[neigh]);
        }

      outtex[node] = L;
    }
}

template void AimsMeshLaplacian( const vector<float> &inittex,
  vector<float> & outtex,
  const map<unsigned, set< pair<unsigned,float> > > &lapl);
template void AimsMeshLaplacian( const vector<double> &inittex,
  vector<double> & outtex,
  const map<unsigned, set< pair<unsigned,float> > > &lapl);

map<unsigned, set< pair<unsigned,float> > >  AimsMeshWeightFiniteElementLaplacian( const AimsSurface<3,Void> & mesh,
                                      const float Wmax)

{
  unsigned					neigh,i, n =mesh.vertex().size();
  list<unsigned>::const_iterator		ilist,elist;
  list<float>::const_iterator		        iphi,itheta,isurf,esurf;
  float						weight,s,p,t,surface;
  map<unsigned, set< pair<unsigned,float> > > lapl;
  map<unsigned, set< pair<unsigned,float> > >::iterator il,el;
  vector< list<unsigned> > neighbourso;
  vector< list<float> >  PHI;
  vector< list<float> > THETA;
  vector< list<float> >  SURFACE;
  set<float> values;

  cout << "Node ordering..." << flush;
  neighbourso = AimsMeshOrderNode(mesh);
  cout << "done \n";
  cout << "Extract mesh features..." <<flush;
  SURFACE =  AimsMeshFiniteElementSurface(mesh,neighbourso);
  PHI = AimsMeshFiniteElementPhi(mesh,neighbourso,SURFACE);
  THETA = AimsMeshFiniteElementTheta(mesh,neighbourso,SURFACE);
  cout << "done \n";

  for (i=0; i<n; ++i)
  {
    s = SURFACE[i].size()  ;
    p = PHI[i].size();
    t = THETA[i].size();
    if ( !( s == p && s == t && p==t ) )
    {
      cout << "Problem with the mesh features..." << endl;
      ASSERT(0);
    }
    elist =  neighbourso[i].end();
    ilist = neighbourso[i].begin();
    iphi = PHI[i].begin();
    isurf = SURFACE[i].begin();
    esurf = SURFACE[i].end();
    itheta = THETA[i].begin();
    surface = 0;
    while ( isurf != esurf )
    {
      surface += *isurf;
      ++isurf;
    }

    while ( ilist != elist )
    {
      neigh = *ilist;

      if (surface != 0 )
        weight = (*iphi + *itheta) / surface;
      else
      {
        cerr << "Triangle(s) with null surface on vertex " << i << endl;
        // ASSERT (0);
        weight = 0.;
      }

      values.insert(weight);
      lapl[i].insert( pair<unsigned,float>(neigh,weight) );
      ++ ilist;
      ++iphi;
      ++itheta;
    }
  }

  //Threshold definition
  set<float>::iterator iv;
  set<float>::reverse_iterator riv;
  unsigned   nbValues,inc = 0 ;
  set< pair<unsigned,float> >::iterator isp,esp;
  float Wm, WM;
  nbValues = (unsigned) rint ( (1 - Wmax) * values.size() );
  cout << "Thresholding  " << 100*(1 - Wmax) << "% of the Laplacian weights (" << nbValues << "/" <<  values.size() << ")" << endl;
  iv = values.begin();
  riv = values.rbegin();
  cout << "Weights:" << endl << "Before: min: " << *iv << " max: " << *riv << endl;
  while ( inc < nbValues)
  {
    ++inc;
    ++iv;
  }
  Wm = *iv;
  inc = 0;
  while ( inc < nbValues)
  {
    ++inc;
    ++riv;
  }
  WM = *riv;
  cout << "After: min: " << Wm << " max: " << WM << endl;
  for (il = lapl.begin(), el = lapl.end(); il != el; ++il)
    for ( isp = (il->second).begin(), esp = (il->second).end(); isp != esp; ++isp)
    {
      neigh = isp->first;
      weight = isp->second;
      if (weight > WM)
      {
        il->second.erase(pair<unsigned,float>(neigh,weight) );
        il->second.insert(pair<unsigned,float>(neigh,WM) );
      }
      else
        if (weight < Wm)
        {
          il->second.erase(pair<unsigned,float>(neigh,weight) );
          il->second.insert(pair<unsigned,float>(neigh,Wm) );
        }
    }

  return(lapl);
}

float AimsMeshFiniteElementDt(  const Texture<float> &tex,
			       const Texture<float> &lapl,
			       float tmax)
{
  set<float>      s1,s2;
  float           maxv,minv,dt;
  unsigned        i,n=tex.nItem();

  for (i=0; i<n; ++i)
    s1.insert(tex.item(i));

  maxv = *s1.rbegin();
  minv = *s1.begin();

  for (i=0; i<n; ++i)
    if (lapl.item(i) != 0 && fabs(tex.item(i)) < tmax)
      {
      s2.insert( fabs((maxv - tex.item(i)) / lapl.item(i)));
      s2.insert( fabs((minv - tex.item(i)) / lapl.item(i)));
      }
  s2.erase(0);
  dt = *s2.begin();

  return(dt);
}

//Regularization of the  curvature map
//Threshold outlier curvature values
//From mean and std deviation
//The value of the thresholded node
//is then the average of its neighbourhood
Texture<float> AimsRegularizeTexture( const Texture<float> &tex,
				      const AimsSurface<3,Void> & mesh,
				      float alpha)
{

  const vector<Point3df>		& vert = mesh.vertex();
  const vector< AimsVector<uint,3> >	& poly = mesh.polygon();
  unsigned				i, n = vert.size();
  map<unsigned, set<unsigned> >		neighbours;
  set<unsigned>::iterator		il,el;
  unsigned				v1, v2, v3;
  Texture<float>			outTex;

  ASSERT( tex.nItem() == n );

  //Neighbouring
  for( i=0; i<poly.size(); ++i )
    {
      v1 = poly[i][0];
      v2 = poly[i][1];
      v3 = poly[i][2];
      neighbours[v1].insert( v2 );
      neighbours[v2].insert( v1 );
      neighbours[v1].insert( v3 );
      neighbours[v3].insert( v1 );
      neighbours[v2].insert( v3 );
      neighbours[v3].insert( v2 );
    }

  float max=-FLT_MAX, min=FLT_MAX,mean = 0, std = 0, Hmax, nval;

  for (i = 0; i < tex.nItem(); ++i)
    {
      if (tex.item(i)>max)
	max = tex.item(i);
      if (tex.item(i)<min)
	min = tex.item(i);
      outTex.push_back( tex.item(i) );
    }

  for (i = 0; i < tex.nItem(); ++i)
    mean += tex.item(i);
  mean = mean/tex.nItem();

  for (i = 0; i < tex.nItem(); ++i)
    std += (tex.item(i) - mean) * (tex.item(i) - mean) ;
  std = sqrt(std / tex.nItem() );

  Hmax = mean +  alpha * std;

  cout << "min: " << min << " max: " << max
       << " mean:" << mean << " std: " << std << " Hmax:" << Hmax << endl;

  unsigned inc1 = 0, inc2 = 0;
  Point3df pos;
  float    d;

  for (i = 0; i < tex.nItem(); ++i)
    {
      if ( (tex.item(i) > Hmax) || (tex.item(i)< -Hmax) )
	{
	  ++inc1;
	  nval = 0;
	  mean = 0;
	  for ( il=neighbours[i].begin(), el=neighbours[i].end(); il!=el; ++il )
	    if( (tex.item(*il) < Hmax) && (tex.item(*il) > -Hmax) )
	      {
		pos = vert[i] - vert[*il];
		d = sqrt(pos[0]*pos[0] + pos[2]*pos[2] + pos[1]*pos[1]) ;
		mean += tex.item(*il) / d  ;
		nval += 1/d ;
	      }
	  if (nval != 0)
	  outTex.item(i) = mean / nval;
	  else
	    {
	      ++inc2;
	      outTex.item(i) = fsign(tex.item(i)) * Hmax;
	    }
	}
    }
  cout << "Nb of regularized points: "<< inc1 << endl;
  cout << "Nb of non regularized points: " << inc2 <<  endl;

  return(outTex);

}

Texture<float> AimsRegularizeTexture(const Texture<float> & tex,
				     float ratio )
{

  Texture<float>                otex = tex;
  set<float>                    values;
  unsigned 			 i, n = tex.nItem();
  set<float>::iterator iv;
  set<float>::reverse_iterator riv;
  unsigned   nbValues,inc = 0 ;
  float Wm, WM;

  for ( i=0; i<n; ++i )
    values.insert(tex.item(i));

  ASSERT(ratio <= 1 && ratio >= 0);
  nbValues = (unsigned) rint ( (1 - ratio) * values.size() );
  cout <<  "Nb of rejected values: " << nbValues << "/" <<  values.size() <<  endl;
  iv = values.begin();
  riv = values.rbegin();
  cout << "Values before: min: " << *iv << " max: " << *riv << endl;
  while ( inc < nbValues)
    {
      ++inc;
      ++iv;
    }
  Wm = *iv;
  inc = 0;
  while ( inc < nbValues)
    {
      ++inc;
      ++riv;
    }
  WM = *riv;
  cout << "Values after: min: " << Wm << " max: " << WM << endl;

  for ( i=0; i<n; ++i )
    if (tex.item(i) > WM)
      otex.item(i) = WM;
    else
      if (tex.item(i) < Wm)
	otex.item(i) = Wm;

  return(otex);
}

namespace aims
{

  void makeLaplacianMatrix( const LaplacianWeights & weights,
                            LaplacianWeights & lmat, float dt )
  {
    /* make weights matrix A = I + dt * W
       W diagonal being substracted the sum of weights of its line.
       so that applying the laplacian smoothing is a matricial operation:
       X(t+1) = A.X(t)
       and X(t) = W^t.X(0)
    */
    cout << "makeLaplacianMatrix...\n";
    float w = 0;
    unsigned i, j;
    LaplacianWeights::const_iterator iw, ew = weights.end();

    for( iw=weights.begin(); iw!=ew; ++iw )
    {
      set<pair<unsigned int, float> >::const_iterator
        ij, ej = iw->second.end();
      w = 0;
      i = iw->first;
      set<pair<unsigned int, float> > & ow = lmat[ i ];

      for( ij=iw->second.begin(); ij!=ej; ++ij )
      {
        j = ij->first;
        if( i == j )
        {
          cout << "problem: ii found: " << i << endl;
        }
        else
        {
          w += ij->second;
          ow.insert( make_pair( j, ij->second * dt ) );
        }
      }
      ow.insert( make_pair( i, 1. - dt * w ) );
    }
    cout << "makeLaplacianMatrix done.\n";
  }

  void makeLaplacianMatrix( const LaplacianWeights & weights,
                            boost_sparse_matrix & lmat,
                            float dt )
  {
    /* make weights matrix A = I + dt * W
       W diagonal being substracted the sum of weights of its line.
       so that applying the laplacian smoothing is a matricial operation:
       X(t+1) = A.X(t)
       and X(t) = W^t.X(0)
    */
    cout << "makeLaplacianMatrix... " << weights.size() << endl;
    float w = 0;
    unsigned i, j;
    lmat.resize( weights.size(), weights.size(), false );
    LaplacianWeights::const_iterator iw, ew = weights.end();

    for( iw=weights.begin(); iw!=ew; ++iw )
    {
      set<pair<unsigned int, float> >::const_iterator
        ij, ej = iw->second.end();
      w = 0;
      i = iw->first;

      for( ij=iw->second.begin(); ij!=ej; ++ij )
      {
        j = ij->first;
        if( i == j )
        {
          cout << "problem: ii found: " << i << endl;
        }
        else
        {
          w += ij->second;
          lmat( i, j ) = ij->second * dt;
        }
      }
      lmat( i, i ) = 1. - dt * w;
    }
    cout << "makeLaplacianMatrix done: " << lmat.size1() << " x " << lmat.size2() << endl;
  }

  void laplacianMatrixThreshold( LaplacianWeights & lmat, float threshold )
  {
    /* remove weights under threshold (in absolute value) on non-diagonal
       coefs, and re-norm diagonal to ensure conservation of the total weight
    */
    cout << "laplacianMatrixThreshold...\n";
    float w = 0, t;
    unsigned i, j;
    LaplacianWeights::iterator iw, ew = lmat.end();

    for( iw=lmat.begin(); iw!=ew; ++iw )
    {
      set<pair<unsigned int, float> > & line = iw->second;
      set<pair<unsigned int, float> >::const_iterator
        ij = line.begin(), ej = line.end(), ti, ii = ej;
      w = 0;
      i = iw->first;
      while( ij!=ej )
      {
        j = ij->first;
        if( i == j )
        {
          ii = ij;
          ++ij;
        }
        else
        {
          t = ij->second;
          if( fabs( t ) <= threshold )
          {
            ti = ij;
            ++ij;
            // remove matrix entry here
            line.erase( ti );
          }
          else
          {
            w += t;
            ++ij;
          }
        }
      }
      // diagonal coef: 1 - sum( line weights )
      if( ii != ej )
        line.erase( ii );
      line.insert( make_pair( i, 1. - w ) );
    }
    cout << "laplacianMatrixThreshold done.\n";
  }

  void laplacianMatrixThreshold( boost_sparse_matrix & lmat, float threshold )
  {
    /* remove weights under threshold (in absolute value) on non-diagonal
       coefs, and re-norm diagonal to ensure conservation of the total weight
    */
    cout << "laplacianMatrixThreshold...\n";
    float w = 0, t;
    unsigned i, j;
    boost_sparse_matrix::iterator1 iw, ew = lmat.end1();
    boost_sparse_matrix::iterator2 jw, ejw;

    for( iw=lmat.begin1(); iw!=ew; ++iw )
    {
      boost_sparse_matrix::iterator2
        ij = iw.begin(), ej = iw.end();
      w = 0;
      i = iw.index1();
      while( ij!=ej )
      {
        j = ij.index2();
        if( i == j )
          ++ij;
        else
        {
          t = *ij;
          if( fabs( t ) <= threshold )
          {
            ++ij;
            // remove matrix entry here
#if BOOST_VERSION < 103300
            lmat.erase( i, j );
#else
            lmat.erase_element( i, j );
#endif
          }
          else
          {
            w += t;
            ++ij;
          }
        }
      }
      // diagonal coef: 1 - sum( line weights )
      lmat( i, i ) =1. - w;
    }
    cout << "laplacianMatrixThreshold done.\n";
  }

  LaplacianWeights* sparseMult( const LaplacianWeights & in1,
                                const LaplacianWeights & in2,
                                float sparseThresh )
  {
#ifdef use_boost
    cout << "sparseMult...\n";
    LaplacianWeights *out = new LaplacianWeights;
    unsigned ncoef = 0; // debug

    LaplacianWeights::const_iterator iin1, ein1 = in1.end(),
      iin2, ein2 = in2.end();
    set<pair<unsigned int, float> >::const_iterator ik, ek, ik2, ek2;
    cout << "convert to boost sparse matrix...\n";
    boost_sparse_matrix
      mat1( in1.size(), in1.size() ), mat2( in2.size(), in2.size() );
    unsigned i;
    for( iin1=in1.begin(); iin1!=ein1; ++iin1 )
    {
      i = iin1->first;
      for( ik=iin1->second.begin(), ek=iin1->second.end(); ik!=ek; ++ik )
        mat1( i, ik->first ) = ik->second;
    }
    for( iin2=in2.begin(); iin2!=ein2; ++iin2 )
    {
      i = iin2->first;
      for( ik=iin2->second.begin(), ek=iin2->second.end(); ik!=ek; ++ik )
        mat2( i, ik->first ) = ik->second;
    }
    cout << "convert done. Sizes: " << mat1.size1() << " x " << mat1.size2()
      << ", " << mat2.size1() << " x " << mat1.size2() << endl;
    boost_sparse_matrix mat3( mat1.size1(), mat2.size2() );
    boost::numeric::ublas::sparse_prod( mat1, mat2, mat3 );
    cout << "mult done. converting back...\n";
    boost_sparse_matrix::iterator1 il, el = mat3.end1();
    boost_sparse_matrix::iterator2 ic, ec;
    for( il=mat3.begin1(); il!=el; ++il )
    {
      set<pair<unsigned int, float> > & line = (*out)[ il.index1() ];
      for( ic=il.begin(), ec=il.end(); ic!=ec; ++ic )
      {
        line.insert( make_pair( ic.index2(), *ic ) );
        ++ncoef;
      }
    }

    sparseThresh = sparseThresh; // compilation warning...
    cout << "out size: " << mat3.size1() << " x " << mat3.size2() << " / " << out->size() << endl;
    cout << "\nsparseMult done, weights num: " << ncoef << endl;
    return out;

#else

    cout << "sparseMult...\n";
    LaplacianWeights *out = new LaplacianWeights;
    unsigned ncoef = 0; // debug

    vector<map<unsigned, float> > lin2( in2.size() );
    LaplacianWeights::const_iterator iin1, ein1 = in1.end(),
      iin2, ein2 = in2.end();
    set<pair<unsigned int, float> >::const_iterator ik, ek, ik2, ek2;
    unsigned i, j, k, n = in1.size();
    map<unsigned, float>::iterator ic, ec;

    // transpose in2 for faster access
    for( iin2=in2.begin(); iin2!=ein2; ++iin2 )
    {
      i = iin2->first;
      for( ik2=iin2->second.begin(), ek2=iin2->second.end(); ik2!=ek2;
          ++ik2 )
        lin2[ ik2->first ][ i ] = ik2->second;
    }
    cout << "transposition done.\n";

    // out(i,j) = S_k( in1(i,k) * in2(k,j) )
    // with in2(k,j) = lin2(j,k)

    for( iin1=in1.begin(); iin1!=ein1; ++iin1 )
    {
      i = iin1->first;
      if( i % 1000 == 0 )
        cout << "\rline: " << i << flush;
      set<pair<unsigned int, float> > & oline = (*out)[i];
      const set<pair<unsigned int, float> > & iline = iin1->second;
      ek = iline.end();

      for( j=0; j<n; ++j )
      {
        map<unsigned, float> & col2 = lin2[j];
        ec = col2.end();
        float oij = 0.F;
        if( col2.size() <= iline.size() )
        {
          // iterate on in2 cols
          for( ic=col2.begin(), ec=col2.end(); ic!=ec; ++ic )
          {
            k = ic->first;
            ik = iline.lower_bound( make_pair( k, -FLT_MAX ) );
            if( ik != ek && ik->first == k )
              oij += ik->second * ic->second;
          }
        }
        else
        {
          // iterate on in1 lines
          ic = col2.begin();
          for( ik=iline.begin(); ik!=ek; ++ik )
          {
            k = ik->first;
            while( ic != ec && ic->first < k )
              ++ic;
            if( ic != ec && ic->first == k )
            {
              oij += ik->second * ic->second;
            }
          }
        }
//         for( ic=col2.begin(), ec=col2.end(); ic!=ec; ++ic )
//         {
//           iin2 = in2.find( ik->first );
//           if( iin2 != ein2 )
//           {
//             ik2 = iin2->second.lower_bound( make_pair( j, -FLT_MAX ) );
//             if( ik2 != iin2->second.end() && ik2->first == j )
//               oij += ik->second * ik2->second;
//           }
//         }
        if( fabs( oij ) > sparseThresh )
        {
          oline.insert( make_pair( j, oij ) );
          ++ncoef;
        }
      }
    }

    cout << "\nsparseMult done, weights num: " << ncoef << endl;
    return out;
#endif
  }

  LaplacianWeights*
    makeLaplacianSmoothingCoefficients( const LaplacianWeights & weights,
                                        unsigned niter, float dt,
                                        float sparseThresh )

  {
#ifdef use_boost
    boost_sparse_matrix weightLaplMat;
    makeLaplacianMatrix( weights, weightLaplMat, dt ); // matricial representation
    laplacianMatrixThreshold( weightLaplMat, sparseThresh );
    boost_sparse_matrix
      *weightLaplPow = &weightLaplMat,
      *weightLapl2, *weightLapl3 = 0;

    unsigned t, iterbin = niter;
    cout << "size: " << weightLaplMat.size1() << " x " << weightLaplMat.size2() << ", niter: " << niter << endl;

    if( iterbin & 1 ) // small bit is set: keep weightLaplMat
    {
      weightLapl3 = &weightLaplMat;
    }
    iterbin = iterbin >> 1;

    for( t=0; iterbin!=0; ++t, iterbin=iterbin>>1 )
    {
      //if (t%10 == 0)
      {
        cout << "                ";
        cout << "\r" << rint(100.*t/log(float(niter))*log(2.)) << "%" << flush;
      }
      weightLapl2 = new boost_sparse_matrix(
        weightLaplMat.size1(), weightLaplMat.size2() );
      // multiply the weights matrix to power niter
      sparse_prod( *weightLaplPow, *weightLaplPow, *weightLapl2 );
      // renormalize diagonal
      laplacianMatrixThreshold( *weightLapl2, sparseThresh );
      if( weightLaplPow != & weightLaplMat && weightLaplPow != weightLapl3 )
        delete weightLaplPow;
      weightLaplPow = weightLapl2;
      // weightLapl2 is weightLaplMat ^ (2^t)
      if( iterbin & 1 )
      {
        // keep weightLaplPow * weightLapl3 in weightLapl3
        if( weightLapl3 )
        {
          weightLapl2 = weightLapl3;
          weightLapl3 = new boost_sparse_matrix(
          weightLaplMat.size1(), weightLaplMat.size2() );
          sparse_prod( *weightLaplPow, *weightLapl2, *weightLapl3 );
          if( weightLapl2 != & weightLaplMat )
            delete weightLapl2;
          laplacianMatrixThreshold( *weightLapl3, sparseThresh );
        }
        else
          weightLapl3 = weightLaplPow;
      }
    }
    weightLaplPow = weightLapl3; // result
    if( !weightLaplPow ) // zero iterations
      weightLaplPow = &weightLaplMat; // take initial matrix

    // convert to LaplacianWeights type
    LaplacianWeights *weightLaplPowL = new LaplacianWeights;
    boost_sparse_matrix::iterator1 il, el = weightLaplPow->end1();
    boost_sparse_matrix::iterator2 ic, ec;
    for( il=weightLaplPow->begin1(); il!=el; ++il )
    {
      set<pair<unsigned int, float> >
        & line = (*weightLaplPowL)[ il.index1() ];
      for( ic=il.begin(), ec=il.end(); ic!=ec; ++ic )
        line.insert( make_pair( ic.index2(), *ic ) );
    }
    cout << "makeLaplacianSmoothingCoefficients done, size: " << weightLaplPow->size1() << " x " << weightLaplPow->size2() << endl;
    if( weightLaplPow != &weightLaplMat )
      delete weightLaplPow;
    return weightLaplPowL;

#else

    LaplacianWeights weightLaplMat;
    makeLaplacianMatrix( weights, weightLaplMat, dt ); // matricial representation
    laplacianMatrixThreshold( weightLaplMat, sparseThresh );
    LaplacianWeights *weightLaplPow = &weightLaplMat, *weightLapl2;

    unsigned t;

    for( t=0; t<niter; ++t )
    {
      //if (t%10 == 0)
      {
        cout << "                ";
        cout << "\r" << rint(100*t/niter) << "%" << flush;
      }
      // multiply the weights matrix to power niter
      weightLapl2 = sparseMult( *weightLaplPow, weightLaplMat,
                                sparseThresh );
      // renormalize diagonal
      laplacianMatrixThreshold( *weightLapl2, sparseThresh );
      if( t != 0 )
        delete weightLaplPow;
      weightLaplPow = weightLapl2;
    }
    return weightLaplPow;
#endif
  }

  template <typename T>
  void applyLaplacianMatrix( const vector<T> &inittex, vector<T> & outtex,
                             const LaplacianWeights &lapl )
  {
    unsigned neigh,node, n =inittex.size();
    map<unsigned, set< pair<unsigned,float> > >::const_iterator il,el;
    set< pair<unsigned,float> >::iterator      ip,ep;
    float                                      L,weight;
    ASSERT ( lapl.size() == n);
    // resize/clear output texture
    if( outtex.size() != n )
      outtex.resize( n );

    for( il = lapl.begin(), el = lapl.end(); il != el; ++il )
    {
      node = il->first;
      L = 0;

      //Weighted sum on the neighbour of the node
      for( ip = (il->second).begin(), ep = (il->second).end(); ip != ep;
           ++ip )
      {
        neigh = ip->first;
        weight = ip->second;
        L += weight * inittex[neigh];
      }

      outtex[node] = L;
    }
  }

  // --- template instanciations ---

  template void applyLaplacianMatrix( const vector<float> &inittex,
    vector<float> & outtex, const LaplacianWeights &lapl);
  template void applyLaplacianMatrix( const vector<double> &inittex,
    vector<double> & outtex, const LaplacianWeights &lapl);

}


#include "problem_multi.h"

#include <algorithm>
#include <iostream>
#include <fstream>
#include <functional>
#include <map>
#include <numeric>
#include <utility>

#include <boost/graph/copy.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include "macros.h"

#define GET_DISTANCE_MAP(graph, source, map) \
	boost::dijkstra_shortest_paths(graph, source, boost::distance_map(boost::make_iterator_property_map(map.begin(), boost::get(boost::vertex_index, graph))))

using namespace std;
using namespace boost;
using json = nlohmann::json;

problem_multi::problem_multi(const vector<graph_type>& _graphs, const vector<commodity>& commodities) :
	graphs(_graphs), commodities(commodities) {
	update();
}

problem_multi::problem_multi(vector<graph_type>&& _graphs, std::vector<commodity>&& commodities) :
	graphs(std::move(_graphs)), commodities(std::move(commodities))
{
	update();
}

problem_multi::problem_multi(const problem_multi& prob) : problem_multi(prob.graphs, prob.commodities) { }

problem_multi::problem_multi(problem_multi&& prob) :
	graphs(std::move(prob.graphs)), commodities(std::move(prob.commodities)),
	big_m(std::move(prob.big_m)),
	big_n(std::move(prob.big_n))
{
	update_graph_data();
	update_indices();
}

problem_multi::problem_multi(const problem& prob) :
	graphs(prob.commodities.size(), prob.graph), commodities(prob.commodities)
{
	update();
}

problem_multi problem_multi::parse_json(const json& json_obj) {
	// Read commodities
	vector<commodity> commodities(json_obj["K"].size());
	for (const json& comm : json_obj["K"]) {
		// Data from Json
		int index = comm["index"].get<int>() - 1;
		int orig = comm["orig"].get<int>() - 1;
		int dest = comm["dest"].get<int>() - 1;
		demand_type demand = comm["demand"].get<demand_type>();

		commodities[index] = commodity{ orig, dest, demand };
	}

	// Read number of vertices
	int num_vertices = json_obj["V"].get<int>();
	vector<graph_type> graphs;
	LOOP(k, commodities.size())
		graphs.push_back(graph_type(num_vertices));

	// Read arcs
	LOOP (k, commodities.size()) {
		for (const json& edge : json_obj["A"][k]) {
			// Data from Json
			int src = edge["src"].get<int>() - 1;
			int dst = edge["dst"].get<int>() - 1;
			cost_type cost = edge["cost"].get<cost_type>();
			bool tolled = edge["toll"].get<bool>();

			// Edge properties (cost, tolled)
			edge_property_type prop;
			get_property_value(prop, edge_weight) = cost;
			get_property_value(prop, edge_tolled) = tolled;

			// Add edge to graph
			add_edge(src, dst, prop, graphs[k]);
		}
	}

	// Add the problem into the list
	return problem_multi(std::move(graphs), std::move(commodities));
}

json problem_multi::get_json(const problem_multi& prob) {
	json json_obj;

	// Number of vertices
	json_obj["V"] = num_vertices(prob.graphs[0]);

	// Edges
	json all_edges_obj;

	LOOP(k, prob.commodities.size()) {
		json edges_obj;

		auto cost_map = get(edge_weight, prob.graphs[k]);
		auto tolled_map = get(edge_tolled, prob.graphs[k]);
		graph_traits<problem_multi::graph_type>::edge_iterator ei, ei_end;

		for (tie(ei, ei_end) = edges(prob.graphs[k]); ei != ei_end; ++ei) {
			// Edge data
			int src = source(*ei, prob.graphs[k]);
			int dst = target(*ei, prob.graphs[k]);
			cost_type cost = cost_map[*ei];
			bool tolled = tolled_map[*ei];

			// Edge ptree node
			json edge;
			edge["src"] = src + 1;
			edge["dst"] = dst + 1;
			edge["cost"] = cost;
			edge["toll"] = tolled;

			edges_obj.push_back(edge);
		}

		all_edges_obj.push_back(edges_obj);
	}

	json_obj["A"] = all_edges_obj;

	// Commodities
	json commodities_obj;

	LOOP(k, prob.commodities.size()) {
		const commodity& c = prob.commodities[k];

		json commodity_obj;
		commodity_obj["index"] = k + 1;
		commodity_obj["orig"] = c.origin + 1;
		commodity_obj["dest"] = c.destination + 1;
		commodity_obj["demand"] = c.demand;

		commodities_obj.push_back(commodity_obj);
	}

	json_obj["K"] = commodities_obj;

	// Add to root node
	return json_obj;
}

json problem_multi::get_json() const {
	return get_json(*this);
}

void problem_multi::update()
{
	update_graph_data();
	update_indices();
	update_big_mn();
}

void problem_multi::update_graph_data() {
	LOOP(k, commodities.size()) {
		is_tolled_maps.push_back(boost::get(edge_tolled, graphs[k]));
		cost_maps.push_back(boost::get(boost::edge_weight, graphs[k]));
		tollfree_graphs.push_back(tollfree_graph_type(graphs[k], edge_tollfree_predicate<edge_tolled_map_type>(is_tolled_maps[k])));
	}
}

void problem_multi::update_indices()
{
	using namespace std;
	using namespace boost;
	using edge_index_rel = edge_index_bimap_type::value_type;
	using src_dst_index_rel = src_dst_index_bimap_type::value_type;

	// Reset
	int K = commodities.size();
	tolled_index_common_map.clear();
	tolled_index_maps = vector<edge_index_bimap_type>(K);
	tollfree_index_maps = vector<edge_index_bimap_type>(K);
	alledges_index_maps = vector<edge_index_bimap_type>(K);

	// Common tolled arcs mapping
	int total_num_tolled = 0;
	max_edge_index = 0;

	// Mapping
	LOOP(k, commodities.size()) {
		edge_iterator ei, ei_end;
		int tollfree_index = 0;

		for (tie(ei, ei_end) = edges(graphs[k]); ei != ei_end; ++ei) {
			bool is_tolled = is_tolled_maps[k][*ei];
			if (is_tolled) {
				int src = source(*ei, graphs[k]);
				int dst = target(*ei, graphs[k]);
				auto src_dst = make_pair(src, dst);

				// New tolled arc
				if (tolled_index_common_map.right.find(src_dst) == tolled_index_common_map.right.end()) {
					tolled_index_common_map.insert(src_dst_index_rel(total_num_tolled++, src_dst));
				}

				tolled_index_maps[k].insert(edge_index_rel(tolled_index_common_map.right.at(src_dst), *ei));
			}
			else {
				tollfree_index_maps[k].insert(edge_index_rel(tollfree_index++, *ei));
			}
		}

		max_edge_index = max(max_edge_index, tollfree_index);
	}

	max_edge_index += total_num_tolled;

	// All edges mapping
	LOOP(k, commodities.size()) {
		auto alledges_inserter = inserter(alledges_index_maps[k], alledges_index_maps[k].begin());

		copy(tolled_index_maps[k].begin(), tolled_index_maps[k].end(), alledges_inserter);
		transform(tollfree_index_maps[k].begin(), tollfree_index_maps[k].end(), alledges_inserter,
				  [total_num_tolled](const edge_index_rel& entry) {
					  return edge_index_rel(entry.left + total_num_tolled, entry.right);
				  });
	}
}

void problem_multi::update_big_mn()
{
	int V = num_vertices(graphs[0]);
	int K = commodities.size();
	int A1 = tolled_index_common_map.size();

	big_m = cost_matrix(K, cost_array(A1));
	big_n = cost_array(A1);

	cost_matrix tollfree_o(K, cost_array(V)), nulltoll_o(K, cost_array(V));
	LOOP(k, K) {
		int o = commodities[k].origin;
		GET_DISTANCE_MAP(graphs[k], o, nulltoll_o[k]);
		GET_DISTANCE_MAP(tollfree_graphs[k], o, tollfree_o[k]);
	}

	LOOP(a, A1) {
		big_n[a] = 0;

		LOOP(k, K) {
			auto map_it = tolled_index_maps[k].left.find(a);
			if (map_it == tolled_index_maps[k].left.end()) {
				big_m[k][a] = 0;
				continue;
			}

			auto edge = map_it->get_right();
			int i = source(edge, graphs[k]);
			int j = target(edge, graphs[k]);
			cost_type c = cost_maps[k][edge];

			cost_array tollfree_i(V), nulltoll_j(V);
			GET_DISTANCE_MAP(graphs[k], j, nulltoll_j);
			GET_DISTANCE_MAP(tollfree_graphs[k], i, tollfree_i);

			int o = commodities[k].origin;
			int d = commodities[k].destination;

			cost_type m1, m2, m3, m4;
			m1 = tollfree_i[j] - c;
			m2 = tollfree_o[k][j] - nulltoll_o[k][i] - c;
			m3 = tollfree_i[d] - c - nulltoll_j[d];
			m4 = tollfree_o[k][d] - nulltoll_o[k][i] - c - nulltoll_j[d];

			big_m[k][a] = max((cost_type)0, min({ m1, m2, m3, m4 }));
			big_n[a] = max(big_n[a], big_m[k][a]);
		}
	}
}

cost_type problem_multi::get_obj_upper_bound() const
{
	int V = num_vertices(graphs[0]);
	int K = commodities.size();

	cost_type sum_obj = 0;
	LOOP(k, K) {
		int o = commodities[k].origin;
		int d = commodities[k].destination;

		cost_array tollfree(V), nulltoll(V);
		GET_DISTANCE_MAP(graphs[k], o, nulltoll);
		GET_DISTANCE_MAP(tollfree_graphs[k], o, tollfree);

		sum_obj += (tollfree[d] - nulltoll[d]) * commodities[k].demand;
	}

	return sum_obj;
}


#include "vio-common/pose-lookup-buffer.h"

#include <mutex>

#include <Eigen/Core>
#include <aslam/common/pose-types.h>
#include <aslam/common/time.h>
#include <maplab-common/interpolation-helpers.h>
#include <maplab-common/temporal-buffer.h>
#include <vio-common/imu-forward-propagation.h>
#include <vio-common/imu-measurements-buffer.h>
#include <vio-common/vio-types.h>

namespace vio_common {
PoseLookupBuffer::PoseLookupBuffer(
    int64_t pose_buffer_length_nanoseconds,
    int64_t max_allowed_propagation_time_ns)
    : max_allowed_propagation_time_ns_(max_allowed_propagation_time_ns),
      buffer_rovio_estimate_(pose_buffer_length_nanoseconds),
      imu_buffer_(max_allowed_propagation_time_ns_),
      gravity_provider_(
          common::locations::kAltitudeZurichMeters,
          common::locations::kLatitudeZurichDegrees) {
  CHECK(aslam::time::isValidTime(pose_buffer_length_nanoseconds));
  CHECK(aslam::time::isValidTime(max_allowed_propagation_time_ns));
}

PoseLookupBuffer::ResultStatus PoseLookupBuffer::getPoseAt(
    int64_t timestamp_ns, aslam::Transformation* T_M_I) const {
  CHECK_NOTNULL(T_M_I);
  std::lock_guard<std::mutex> lock(mutex_);

  // Early exit if the timestamp_ns is older than the buffered values.
  vio::ViNodeState oldest_rovio_estimate;
  if (buffer_rovio_estimate_.getOldestValue(&oldest_rovio_estimate) &&
      timestamp_ns < oldest_rovio_estimate.getTimestamp()) {
    return ResultStatus::kFailed;
  }

  // First try to interpolate the pose.
  vio::ViNodeState estimate_interpolated;
  if (buffer_rovio_estimate_.interpolateAt(
          timestamp_ns, &estimate_interpolated)) {
    *T_M_I = estimate_interpolated.get_T_M_I();
    return ResultStatus::kSuccessInterpolated;
  }

  // If that failed, propagate the pose using IMU data.
  vio::ViNodeState most_recent_estimate;
  CHECK(buffer_rovio_estimate_.getNewestValue(&most_recent_estimate));
  CHECK_GT(timestamp_ns, most_recent_estimate.getTimestamp());

  const int64_t propagation_time_nanoseconds =
      most_recent_estimate.getTimestamp() - timestamp_ns;
  if (propagation_time_nanoseconds > max_allowed_propagation_time_ns_) {
    return ResultStatus::kFailed;
  }

  vio::ViNodeState estimate_propagated;
  if (!vio::propagatePoseUsingImu(
          most_recent_estimate, gravity_provider_.getGravityMagnitude(),
          timestamp_ns, imu_buffer_, &estimate_propagated)) {
    return ResultStatus::kFailed;
  }
  *T_M_I = estimate_propagated.get_T_M_I();
  return ResultStatus::kSuccessImuForwardPropagation;
}

void PoseLookupBuffer::bufferImuMeasurement(const vio::ImuMeasurement& imu) {
  std::lock_guard<std::mutex> lock(mutex_);
  imu_buffer_.addMeasurement(imu.timestamp, imu.imu_data);
}

void PoseLookupBuffer::bufferRovioEstimate(const vio::ViNodeState& estimate) {
  std::lock_guard<std::mutex> lock(mutex_);
  buffer_rovio_estimate_.addValue(estimate.getTimestamp(), estimate);
}
}  //  namespace vio_common


#include <iostream>
#include <numeric>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/viz/vizcore.hpp>
#include <kfusion/kinfu.hpp>
#include <kfusion/cuda/marching_cubes.hpp>
//#include <io/capture.hpp>
//#include <io/bin_grabber.hpp>
#include <string>
#include <thread>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>

using namespace kfusion;
using namespace std;

void outputMeshAsPly(const std::string& filename, const cv::viz::Mesh& mesh, const Affine3f last_pose, int file_type)
{
    if (file_type == 0){
    	std::ofstream stream(filename.c_str());
    	size_t num_points = mesh.cloud.cols; // the number of points in the mesh
    	stream << "ply" << std::endl;
    	stream << "format ascii 1.0" << std::endl;
    	stream << "element vertex " << num_points << std::endl;
    	stream << "property float x" << std::endl;
    	stream << "property float y" << std::endl;
    	stream << "property float z" << std::endl;

    	stream << "property uchar red" << std::endl;
    	stream << "property uchar green" << std::endl;
    	stream << "property uchar blue" << std::endl;

        stream << "element face " << mesh.cloud.cols / 3 << std::endl;
        stream << "property list uchar int vertex_index" << std::endl;

        stream << "end_header" << std::endl;

    	char temp[100];
		for (int i = 0 ; i < mesh.cloud.cols ; i++) {
			cv::Affine3f::Vec3 point(mesh.cloud.at<float>(4*i),mesh.cloud.at<float>(4*i+1),mesh.cloud.at<float>(4*i+2));
			point = last_pose*point;

			stream << point(0) << " " << point(1) << " " << point(2) << " ";

			sprintf(temp, "%d %d %d", mesh.colors.at<unsigned char>(4*i+2),
									  mesh.colors.at<unsigned char>(4*i+1),
									  mesh.colors.at<unsigned char>(4*i));
			stream << temp << endl;
		}
		for (int i = 0 ; i < mesh.cloud.cols/3 ; i++) {
			sprintf(temp, "3 %d %d %d",  3*i+2, 3*i+1, 3*i+0);
			stream << temp << endl;
		}
		stream.close();
    }
    else {
    	std::ofstream stream(filename.c_str() , ios::binary);
		size_t num_points = mesh.cloud.cols; // the number of points in the mesh
		stream << "ply" << std::endl;
		stream << "format binary_little_endian 1.0" << std::endl;
		stream << "element vertex " << num_points << std::endl;
		stream << "property float x" << std::endl;
		stream << "property float y" << std::endl;
		stream << "property float z" << std::endl;

		stream << "property uchar red" << std::endl;
		stream << "property uchar green" << std::endl;
		stream << "property uchar blue" << std::endl;

		stream << "element face " << mesh.cloud.cols / 3 << std::endl;
		stream << "property list uchar int vertex_index" << std::endl;

		stream << "end_header" << std::endl;

		char temp[100];
		for (int i = 0 ; i < mesh.cloud.cols ; i++) {
			cv::Affine3f::Vec3 point(mesh.cloud.at<float>(4*i),
									 mesh.cloud.at<float>(4*i+1),
									 mesh.cloud.at<float>(4*i+2));
			point = last_pose*point;
			stream.write(reinterpret_cast<const char*>(&point(0)), sizeof(float));
			stream.write(reinterpret_cast<const char*>(&point(1)), sizeof(float));
			stream.write(reinterpret_cast<const char*>(&point(2)), sizeof(float));

			stream.write(reinterpret_cast<const char*>(&mesh.colors.at<unsigned char>(4*i+2)), sizeof(char));
			stream.write(reinterpret_cast<const char*>(&mesh.colors.at<unsigned char>(4*i+1)), sizeof(char));
			stream.write(reinterpret_cast<const char*>(&mesh.colors.at<unsigned char>(4*i)), sizeof(char));
		}
		for (int i = 0 ; i < mesh.cloud.cols/3 ; i++) {
			stream.write(reinterpret_cast<const char*>(&mesh.polygons.at<int>(4*i)), sizeof(char));
			stream.write(reinterpret_cast<const char*>(&mesh.polygons.at<int>(4*i+3)), sizeof(int));
			stream.write(reinterpret_cast<const char*>(&mesh.polygons.at<int>(4*i+2)), sizeof(int));
			stream.write(reinterpret_cast<const char*>(&mesh.polygons.at<int>(4*i+1)), sizeof(int));
		}
		stream.close();
    }
}

class ConfigParser
{
	boost::property_tree::ptree pt;

public:
	string dataset_dir;
	double magic_factor;
	int seq_n;
	int idx;
	int gpu_id;
	int start_frame;
	double volume_size;
	int img_cols;
	int img_rows;
	double focal_length;

	ConfigParser(string config_file)
	{
    	boost::property_tree::ini_parser::read_ini(config_file, pt);

    	gpu_id = atoi(pt.get<std::string>("parameters.gpu_id").c_str());
    	start_frame = atof(pt.get<std::string>("parameters.start_frame").c_str());
    	dataset_dir = pt.get<std::string>("parameters.dataset_dir");
    	magic_factor = atof(pt.get<std::string>("parameters.magic_factor").c_str());
    	volume_size = atof(pt.get<std::string>("parameters.volume_size").c_str());
    	img_cols = atoi(pt.get<std::string>("parameters.img_cols").c_str());
    	img_rows = atoi(pt.get<std::string>("parameters.img_rows").c_str());
    	focal_length = atoi(pt.get<std::string>("parameters.focal_length").c_str());

    	cout << "[Parameters]" << endl;
    	cout << "GPU_ID : \t" << gpu_id << endl;
    	cout << "Start frame : \t" <<  start_frame << endl;
    	cout << "Dataset dir : \t" <<  dataset_dir << endl;
    	cout << "Magic factor: \t" <<  magic_factor << endl;
    	cout << "Volume size : \t" <<  volume_size << endl;
    	cout << "Img cols : \t" <<  img_cols << endl;
    	cout << "Img rows : \t" <<  img_rows << endl;
    	cout << "Focal length : \t" <<  focal_length << endl;
	}
};

class SequenceSource
{
    string dataset_dir;
    string asso_file;
    vector<string> color_files;
    vector<string> depth_files;
    vector<string> semantic_files;
    vector<string> timestamps;
    float magic_factor;
    int seq_n;
    int idx;

public:
    vector<string> split(const char *str, char c = ' ')
    {
        vector<string> result;

        do
        {
            const char *begin = str;

            while(*str != c && *str)
                str++;

            result.push_back(string(begin, str));
        } while (0 != *str++);

        return result;
    }

    SequenceSource(string _dataset_dir, float _magic_factor, int start_frame)
    {
        dataset_dir = _dataset_dir;
        asso_file = _dataset_dir + "/associations.txt";
        magic_factor = _magic_factor;

        ifstream openFile(asso_file.c_str());
        if( openFile.is_open() ){
            string line;
            while(getline(openFile, line)){
                // cout << line << endl;
                vector<string> tokens = split(line.c_str(), ' ');
                color_files.push_back(dataset_dir+tokens[1]);
                depth_files.push_back(dataset_dir+tokens[3]);
                semantic_files.push_back(dataset_dir+tokens[5]);
                timestamps.push_back(tokens[0]);
            }
            openFile.close();
        }

        idx=start_frame;
        seq_n = color_files.size();
    }

    bool grab(cv::Mat& depth, cv::Mat& color, cv::Mat& semantic)
    {
    	// end of frames
        if (idx >= seq_n)
        {
            idx = 0;
            return false;
        }

        std::string depth_file_name;
        std::string color_file_name;
        std::string semantic_file_name;
        color_file_name = color_files[idx];
        depth_file_name = depth_files[idx];
        semantic_file_name = semantic_files[idx];

        // std::cout << color_file_name << std::endl;
        // std::cout << depth_file_name << std::endl;
        // std::cout << semantic_file_name << std::endl;

        depth = cv::imread(depth_file_name, CV_LOAD_IMAGE_ANYDEPTH);
        for (int y = 0 ; y < depth.rows ; y++)
        for (int x = 0 ; x < depth.cols ; x++)
        {
            depth.at<ushort>(y,x) *= magic_factor;
        }

        color = cv::imread(color_file_name);
        cv::cvtColor(color, color, CV_BGR2BGRA, 4);

        semantic = cv::imread(semantic_file_name);
        cv::cvtColor(semantic, semantic, CV_BGR2BGRA, 4);

        // cv::imshow("color",color);
        // cv::imshow("depth",depth);
        // cv::imshow("semantic",semantic);

        idx++;

        return true;
    }
    void reset()
    {
        idx = 0;
    }

    string current_timestamp()
    {
    	return timestamps[idx-1];
    }
};

struct KinFuApp
{
  /**
   * @name KeyboardCallback
   * @fn static void KeyboardCallback(const cv::viz::KeyboardEvent& event, void* pthis)
   * @brief Define keyboard callback for the Kinfu app
   * @param[in] event
   * @param[in] pthis, void pointer on itself
   */
  static void KeyboardCallback(const cv::viz::KeyboardEvent& event, void* pthis)
  {
      KinFuApp& kinfu = *static_cast<KinFuApp*>(pthis);

      if(event.action != cv::viz::KeyboardEvent::KEY_DOWN)
          return;

      if(event.code == 't' || event.code == 'T')
          kinfu.take_cloud(*kinfu.kinfu_);

      if(event.code == 'i' || event.code == 'I')
          kinfu.interactive_mode_ = !kinfu.interactive_mode_;

      if(event.code == 'm')
          kinfu.take_mesh(*kinfu.kinfu_, false);
      if(event.code == 'M')
    	  kinfu.take_mesh(*kinfu.kinfu_, true, "mesh/color_mesh " + kinfu.capture_.current_timestamp() + ".ply");

      if(event.code == 's')
          kinfu.take_semantic_mesh(*kinfu.kinfu_, false);
      if(event.code == 'S')
          kinfu.take_semantic_mesh(*kinfu.kinfu_, true, "mesh/semantic_mesh " + kinfu.capture_.current_timestamp() + ".ply");
  }

  KinFuApp(SequenceSource& source, const KinFuParams& params) : exit_ (false), capture_(source), interactive_mode_(false), pause_(false) {
    kinfu_ = KinFu::Ptr( new KinFu(params) );

    // capture_.setRegistration(true);

    cv::viz::WCube cube(cv::Vec3d::all(0), cv::Vec3d(params.volume_size), true, cv::viz::Color::apricot());
    viz.showWidget("cube", cube, params.volume_pose);
    viz.showWidget("coor", cv::viz::WCoordinateSystem(0.1));
    viz.registerKeyboardCallback(KeyboardCallback, this);
  }

  /**
   * @name show_depth
   * @fn void show_depth(const cv::Mat& depth)
   * @brief Display the depth stream, after normalization
   * @param[in] depth, the depth image to normalize and display
   */
  void show_depth(const cv::Mat& depth) {
      cv::Mat display;
      cv::normalize(depth, display, 0, 255, cv::NORM_MINMAX, CV_8U);
      depth.convertTo(display, CV_8U, 255.0/4000);
      cv::imshow("Depth", display);
  }

  /**
   * @name show_raycasted
   * @fn void show_raycasted(KinFu& kinfu)
   * @brief Show the reconstructed scene (using raycasting)
   * @param[in] kinfu instance
   */
  void show_raycasted(KinFu& kinfu) {
      const int mode = 3;
      if (interactive_mode_)
          kinfu.renderImage(view_device_, viz.getViewerPose(), mode);
      else
          kinfu.renderImage(view_device_, mode);

      view_host_.create(view_device_.rows(), view_device_.cols(), CV_8UC4);
      view_device_.download(view_host_.ptr<void>(), view_host_.step);
      cv::imshow("Scene", view_host_);
  }

  /**
   * @name take_cloud
   * @fn void take_cloud(KinFu& kinfu)
   * @brief Fetch cloud and display it
   * @param[in] kinfu instance
   */
  void take_cloud(KinFu& kinfu)
  {
      cuda::DeviceArray<Point> cloud = kinfu.tsdf().fetchCloud(cloud_buffer_);

      cv::Mat cloud_host(1, (int)cloud.size(), CV_32FC4);

      if (kinfu.params().integrate_color)
      {
          kinfu.color_volume()->fetchColors(cloud, color_buffer_);
          cv::Mat color_host(1, (int)cloud.size(), CV_8UC4);
          cloud.download(cloud_host.ptr<Point>());
          color_buffer_.download(color_host.ptr<RGB>());
          viz.showWidget("cloud", cv::viz::WCloud(cloud_host, color_host));
      } else
      {
          cloud.download(cloud_host.ptr<Point>());
          // viz.showWidget("cloud", cv::viz::WCloud(cloud_host));
          viz.showWidget("cloud", cv::viz::WPaintedCloud(cloud_host));
      }
  }

  /**
   * @name take_mesh
   * @fn void take_mesh(KinFu& kinfu)
   * @brief Run marching cubes on the volume and construct the mesh
   * @param[in] kinfu instance
   */
  void take_mesh(KinFu& kinfu, bool save_mesh, string mesh_string = "color_mesh.ply", bool last_frame=false)
  {
      if (!marching_cubes_)
          marching_cubes_ = cv::Ptr<cuda::MarchingCubes>(new cuda::MarchingCubes());

      cuda::DeviceArray<Point> triangles = marching_cubes_->run(kinfu.tsdf(), triangles_buffer_);
      int n_vert = triangles.size();

      cv::viz::Mesh mesh;
      mesh.cloud.create(1, n_vert, CV_32FC4);
      mesh.polygons.create(1, 4*n_vert/3, CV_32SC1);

      for (int i = 0; i < n_vert/3; ++i) {
          mesh.polygons.at<int>(4*i) = 3;
          mesh.polygons.at<int>(4*i+1) = 3*i;
          mesh.polygons.at<int>(4*i+2) = 3*i+1;
          mesh.polygons.at<int>(4*i+3) = 3*i+2;
      }

      cv::Mat mesh_colors(1, n_vert, CV_8UC4);

      if (kinfu.params().integrate_color)
      {
          kinfu.color_volume()->fetchColors(triangles, color_buffer_);
          color_buffer_.download(mesh_colors.ptr<RGB>());
          mesh.colors = mesh_colors;
      }

      triangles.download(mesh.cloud.ptr<Point>());

      viz.showWidget("cloud", cv::viz::WMesh(mesh));

      if (save_mesh)
      {
		  std::thread myThread(outputMeshAsPly, mesh_string, mesh, kinfu.getLastSucessPose(),1);
		  if (!last_frame)
			  myThread.detach();
		  else // if it is the last frame, we should wait
			  myThread.join();
      }

      // cv::imshow("mesh_colors", mesh_colors);
      // cv::waitKey(0);
  }

  void take_semantic_mesh(KinFu& kinfu, bool save_mesh, string mesh_string = "semantic_mesh.ply")
  {
      if (!marching_cubes_)
          marching_cubes_ = cv::Ptr<cuda::MarchingCubes>(new cuda::MarchingCubes());

      cuda::DeviceArray<Point> triangles = marching_cubes_->run(kinfu.tsdf(), triangles_buffer_);
      int n_vert = triangles.size();

      cv::viz::Mesh mesh;
      mesh.cloud.create(1, n_vert, CV_32FC4);
      mesh.polygons.create(1, 4*n_vert/3, CV_32SC1);

      for (int i = 0; i < n_vert/3; ++i) {
          mesh.polygons.at<int>(4*i) = 3;
          mesh.polygons.at<int>(4*i+1) = 3*i;
          mesh.polygons.at<int>(4*i+2) = 3*i+1;
          mesh.polygons.at<int>(4*i+3) = 3*i+2;
      }

      cv::Mat mesh_colors(1, n_vert, CV_8UC4);

      if (kinfu.params().integrate_color)
      {
          kinfu.semantic_volume()->fetchSemantics(triangles, color_buffer_);
          color_buffer_.download(mesh_colors.ptr<RGB>());
          mesh.colors = mesh_colors;
      }

      triangles.download(mesh.cloud.ptr<Point>());

      viz.showWidget("cloud", cv::viz::WMesh(mesh));

      if (save_mesh)
      {
		  std::thread myThread(outputMeshAsPly, mesh_string, mesh, kinfu.getLastSucessPose(),1);
		  myThread.detach();
      }

      // cv::imshow("mesh_colors", mesh_colors);
      // cv::waitKey(0);
  }

  /**
   * @name execute
   * @fn bool execute()
   * @brief Run the main loop of the app
   * @return true if no error, false otherwise
   */
  bool execute()
  {
      KinFu& kinfu = *kinfu_;
      cv::Mat depth, image, semantic;
      double time_ms = 0;
      int kinfu_return_val= 0;

      for (int i = 0; !exit_ && !viz.wasStopped(); ++i)
      {
          if (!capture_.grab(depth, image, semantic))
          {
               std::cout << "[End of frames]" << std::endl;
               take_mesh(kinfu, true, "mesh/color_mesh [finish].ply", true);
               kinfu.saveEstimatedTrajectories();
               return false;
          }

          depth_device_.upload(depth.data, depth.step, depth.rows, depth.cols);
          color_device_.upload(image.data, image.step, image.rows, image.cols);
          semantic_device_.upload(semantic.data, semantic.step, semantic.rows, semantic.cols);

		  SampledScopeTime fps(time_ms); (void)fps;
		  kinfu_return_val = kinfu(depth_device_, color_device_, semantic_device_, capture_.current_timestamp());

          // volume save
          if (kinfu_return_val == 1)		// tracking success
          {
        	  show_raycasted(kinfu);
//        	  if (kinfu.getFrameCounter() % 100 == 0)
//        	  {
//        		  take_mesh(kinfu, true, "mesh/color_mesh " + capture_.current_timestamp() + "[split].ply");
//        		  kinfu.storeSubvolume();
//        		  kinfu.storePoseVector();
//        		  kinfu.clearVolumes();
//        	  }
          }
          else if (kinfu_return_val == 2)	// tracking failure,
          {
        	  if (kinfu.getFrameCounter() > 50)	// the number of frame until the failure point should be bigger than 50.
        	  {
        		  take_mesh(kinfu, true, "mesh/color_mesh " + capture_.current_timestamp() + "[faliure].ply");
				  kinfu.storeSubvolume();
				  kinfu.storePoseVector();
        	  }
        	  kinfu.reset();
          }
          // else if (inconsistent model)

          if (!interactive_mode_)
              viz.setViewerPose(kinfu.getCameraPose());

          int key = cv::waitKey(pause_ ? 0 : 1);
          switch(key)
          {
			  case 't': case 'T' : take_cloud(kinfu); break;
			  case 'i': case 'I' : interactive_mode_ = !interactive_mode_; break;
			  case 'm': take_mesh(kinfu, false); break;
			  case 'M': take_mesh(kinfu, true, "mesh/color_mesh " + capture_.current_timestamp() + ".ply"); break;
			  case 's': take_semantic_mesh(kinfu, false); break;
			  case 'S': take_semantic_mesh(kinfu, true, "mesh/semantic_mesh " + capture_.current_timestamp() + ".ply"); break;
			  case 27:
				  kinfu.saveEstimatedTrajectories();
				  exit_ = true;
				  break;
			  case 32: pause_ = !pause_; break;
          }

          //exit_ = exit_ || i > 100;
          viz.spinOnce(1, true);
      }
      return true;
  }

  SequenceSource& capture_;
  /**< */
  bool pause_; // = false
  /**< Stop the execution when set to true */
  bool exit_;
  /**< Allow for free point of view (otherwise, follows the camera) */
  bool interactive_mode_;
  /**< Pointer to the instance of kinfu */
  KinFu::Ptr kinfu_;
  /**< */
  cv::viz::Viz3d viz;

  /**< View of the scene (raycasting) */
  cv::Mat view_host_;
  /**< View of the scene on the GPU */
  cuda::Image view_device_;
  /**< Depth frame on the GPU */
  cuda::Depth depth_device_;
  /**< Color frame on the GPU */
  cuda::Image color_device_;
  /**< point buffer used when fetching the point cloud from the tsdf volume */
  cuda::Image semantic_device_;

  cuda::DeviceArray<Point> cloud_buffer_;
  /**< color buffer used when fetching the colors from the color volume */
  cuda::DeviceArray<RGB> color_buffer_;

  /**< Marching cubes instance (to generate a MESH) */
  cv::Ptr<cuda::MarchingCubes> marching_cubes_;
  /**< triangles buffer used in marching cubes */
  cuda::DeviceArray<Point> triangles_buffer_;
};

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

int main (int argc, char* argv[])
{

  if (argc < 2)
    cout << "[Error] Config file is necessary." << endl;

  ConfigParser config(argv[1]);

  cuda::setDevice (config.gpu_id);
  cuda::printShortCudaDeviceInfo (config.gpu_id);

  string dataset_dir = config.dataset_dir;
  float magic_factor = config.magic_factor;
  float volume_size = config.volume_size;
  int img_cols = config.img_cols;
  int img_rows = config.img_rows;
  float focal_length = config.focal_length;
  int start_frame = config.start_frame;

  SequenceSource capture(dataset_dir, magic_factor, start_frame);

  KinFuParams custom_params = KinFuParams::default_params();
  custom_params.integrate_color = true;
  custom_params.integrate_semantic = true;
  custom_params.tsdf_volume_dims = Vec3i::all(512);
  custom_params.color_volume_dims = Vec3i::all(512);
  custom_params.semantic_volume_dims = Vec3i::all(512);
  custom_params.volume_size = Vec3f::all(volume_size);
  custom_params.volume_pose = Affine3f().translate(Vec3f(-custom_params.volume_size[0]/2, -custom_params.volume_size[1]/2, 0.0f));
  custom_params.tsdf_trunc_dist = 0.002;
  custom_params.bilateral_kernel_size = 3;     //pixels
  custom_params.cols = img_cols;
  custom_params.rows = img_rows;
  custom_params.intr = Intr(focal_length, focal_length, custom_params.cols/2 - 0.5f, custom_params.rows/2 - 0.5f);
  custom_params.icp_dist_thres = 0.25;                //meters
  custom_params.icp_angle_thres = deg2rad(30); //radians

  KinFuApp app (capture, custom_params);

  // executing
  try { app.execute (); }
  catch (const std::bad_alloc& /*e*/) { std::cout << "Bad alloc" << std::endl; }
  catch (const std::exception& /*e*/) { std::cout << "Exception" << std::endl; }

  return 0;
}


//
//  time_advance.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/8/18.
//
//

#ifndef time_advance_hpp
#define time_advance_hpp

#include <stdio.h>
#include <Eigen/Eigen>
using namespace Eigen;

void time_advance(VectorXd &u_n, VectorXd &v_n, VectorXd &F_total, VectorXd &M_global_vec, double dt);
#endif /* time_advance_hpp */