/* 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);

}


// Copyright 2010-2013 The Trustees of Indiana University.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#ifndef BOOST_DISTRIBUTED_ERDOS_RENYI_GENERATOR_HPP
#define BOOST_DISTRIBUTED_ERDOS_RENYI_GENERATOR_HPP

#include "splittable_ecuyer1988.hpp"
#include <boost/iterator.hpp>
#include <boost/random/geometric_distribution.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {
  template <typename VertexDistribution, typename Generator = boost::random::splittable_ecuyer1988, typename VertexType = size_t>
  class distributed_erdos_renyi_iterator:
          public boost::iterator_facade<distributed_erdos_renyi_iterator<VertexDistribution, Generator, VertexType>,
                                        std::pair<VertexType, VertexType>,
                                        std::input_iterator_tag,
                                        std::pair<VertexType, VertexType> >
  {
    VertexDistribution owner;
    typedef size_t rank_type;
    rank_type my_rank;
    typename Generator::split_iterator_pair generators; // One for each source vertex
    Generator current_generator;
    VertexType current_source, current_target, nverts;
    bool is_undirected, allow_self_loops;
    boost::geometric_distribution<VertexType> distrib;

    void next_source_vertex() {
      // fprintf(stderr, "next_source_vertex()\n");
      ++current_source;
      current_target = (VertexType)(-1);
      ++generators.first;
      if (generators.first != generators.second) {
        current_generator = *generators.first;
      }
    }

    void step_to_owned_vertex() {
      // Find a source that I own
      while (current_source != nverts &&
             rank_type(owner(current_source)) != my_rank) {
        next_source_vertex();
      }
    }

    void raw_increment() { // Does not always produce a valid vertex
      if (current_source == nverts) return;
      boost::random::uniform_01_wrapper<Generator> wr(current_generator);
      VertexType delta = distrib(wr);
      // fprintf(stderr, "delta = %zu\n", delta);
      // First test is to deal with overflows, plus handle that the current
      // target might be -1 and so delta == nverts would be valid
      if (delta > nverts || current_target + delta >= nverts) {
        next_source_vertex();
        step_to_owned_vertex();
        // Return target as -1, so raw_increment() will be repeated
      } else {
        current_target += delta;
      }
      // fprintf(stderr, "New loc is (%zu, %zu)\n", current_source, current_target);
    }

    public:
    distributed_erdos_renyi_iterator(): owner(), my_rank(0), current_source(0), current_target((VertexType)(-1)), nverts(0) {}

    distributed_erdos_renyi_iterator(Generator& gen, VertexType nverts, double prob, bool is_undirected, bool allow_self_loops, VertexDistribution owner, rank_type my_rank): owner(owner), my_rank(my_rank), current_source(0), current_target((VertexType)(-1)), nverts(nverts), is_undirected(is_undirected), allow_self_loops(allow_self_loops), distrib(1. - prob) {
      generators = gen.split_off_n(nverts);
      raw_increment();
    }

    std::pair<VertexType, VertexType> dereference() const {
      return std::make_pair(current_source, current_target);
    }

    bool equal(const distributed_erdos_renyi_iterator& o) const {
      // Only really works on end iterators or those generated with the same parameters
      return (nverts - current_source) == (o.nverts - o.current_source) && current_target == o.current_target;
    }

    void increment() {
      do {
        raw_increment();
      } while (current_source != nverts &&
               (current_target == (VertexType)(-1) ||
                (!allow_self_loops && current_source == current_target) ||
                (is_undirected && current_target > current_source)));
    }
  };

}

#endif // BOOST_DISTRIBUTED_ERDOS_RENYI_GENERATOR_HPP


// Copyright (c) 2019, Torsten Sattler
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of the copyright holder nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// author: Torsten Sattler, torsten.sattler.de@googlemail.com

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <random>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

#include "hybrid_line_estimator.h"

namespace ransac_lib {

HybridLineEstimator::HybridLineEstimator(
    const Eigen::Matrix2Xd& points, const Eigen::Matrix4Xd& points_with_normals,
    const std::vector<double>& prior_probabilities) {
  points_ = points;
  num_points_ = points_.cols();
  points_with_normals_ = points_with_normals;
  num_points_with_normals_ = points_with_normals_.cols();
  prior_probabilities_ = prior_probabilities;
}

void HybridLineEstimator::LeastSquares(
    const std::vector<std::vector<int>>& sample, Eigen::Vector3d* line) const {
  const int kNumSamplesPoints = static_cast<int>(sample[0].size());
  const int kNumSamplesPointsWithNormals = static_cast<int>(sample[1].size());
  const int kNumSamples = kNumSamplesPoints + kNumSamplesPointsWithNormals;

  if (kNumSamples < 6) return;
  // We fit the line by estimating the eigenvectors of the covariance matrix
  // of the data.
  Eigen::Vector2d mean(0.0, 0.0);
  for (int i = 0; i < kNumSamplesPoints; ++i) {
    mean += points_.col(sample[0][i]);
  }
  for (int i = 0; i < kNumSamplesPointsWithNormals; ++i) {
    mean += points_with_normals_.col(sample[1][i]).head<2>();
  }
  mean /= static_cast<double>(kNumSamples);

  // Builds the covariance matrix C.
  Eigen::Matrix2d C = Eigen::Matrix2d::Zero();

  for (int i = 0; i < kNumSamplesPoints; ++i) {
    Eigen::Vector2d d = points_.col(sample[0][i]) - mean;
    C += d * d.transpose();
  }
  for (int i = 0; i < kNumSamplesPointsWithNormals; ++i) {
    Eigen::Vector2d d = points_with_normals_.col(sample[1][i]).head<2>() - mean;
    C += d * d.transpose();
  }
  C /= static_cast<double>(kNumSamples - 1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig_solver(C);
  if (eig_solver.info() != Eigen::Success) return;

  line->head<2>() = eig_solver.eigenvectors().col(1);

  // Re-estimates the translation along the line to account for subtraction
  // of mean.
  (*line)[2] = -line->head<2>().dot(mean);
}

// Evaluates the line on the i-th data point of the t-th data type.
double HybridLineEstimator::EvaluateModelOnPoint(const Eigen::Vector3d& line,
                                                 int t, int i) const {
  double residual = 0.0;
  if (t == 0) {
    residual = line.dot(points_.col(i).homogeneous());
  } else {
    residual = line.dot(points_with_normals_.col(i).head<2>().homogeneous());
  }
  return residual * residual;
}

int HybridLineEstimator::TwoPointSolver(
    const std::vector<int>& sample, std::vector<Eigen::Vector3d>* lines) const {
  lines->clear();
  if (sample.size() < 2u) return 0;

  lines->resize(1);
  Eigen::Vector3d p1(points_(0, sample[0]), points_(1, sample[0]), 1.0);
  Eigen::Vector3d p2(points_(0, sample[1]), points_(1, sample[1]), 1.0);
  (*lines)[0] = p1.cross(p2);
  // Normalizes the line such that the normal of the line has unit length.
  double normal_norm = (*lines)[0].head<2>().norm();
  if (normal_norm == 0.0) {
    lines->clear();
    return 0;
  }

  (*lines)[0] /= normal_norm;

  return 1;
}

int HybridLineEstimator::PointNormalSolver(
    const std::vector<int>& sample, std::vector<Eigen::Vector3d>* lines) const {
  lines->clear();
  if (sample.size() < 1u) return 0;

  lines->resize(1);
  Eigen::Vector2d normal = points_with_normals_.col(sample[0]).tail<2>();
  normal.normalize();
  double c = -normal.dot(points_with_normals_.col(sample[0]).head<2>());
  (*lines)[0] = Eigen::Vector3d(normal[0], normal[1], c);
  // Normalizes the line such that the normal of the line has unit length.
  double normal_norm = (*lines)[0].head<2>().norm();
  if (normal_norm == 0.0) {
    lines->clear();
    return 0;
  }

  (*lines)[0] /= normal_norm;

  return 1;
}

}  // namespace ransac_lib


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "laplace_3d_calderon_projector.hpp"

#include "../common/shared_ptr.hpp"

#include "laplace_3d_single_layer_boundary_operator.hpp"
#include "laplace_3d_double_layer_boundary_operator.hpp"
#include "laplace_3d_hypersingular_boundary_operator.hpp"
#include "identity_operator.hpp"
#include "blocked_operator_structure.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../assembly/context.hpp"

#include <boost/make_shared.hpp>

namespace Bempp {

template <typename BasisFunctionType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
laplace3dExteriorCalderonProjector(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    const std::string &label) {

  typedef BoundaryOperator<BasisFunctionType, ResultType> BdOp;

  shared_ptr<const Space<BasisFunctionType>> internalHplusSpace =
      hplusSpace->discontinuousSpace(hplusSpace);

  BdOp internalSlp = laplace3dSingleLayerBoundaryOperator(
      context, internalHplusSpace, internalHplusSpace, internalHplusSpace,
      label + "_slp", SYMMETRIC);

  BdOp dlp = laplace3dDoubleLayerBoundaryOperator(context, hplusSpace,
                                                  hplusSpace, hminusSpace,
                                                  label + "_dlp", NO_SYMMETRY);

  BdOp adjDlp = adjoint(dlp);

  BdOp hyp = laplace3dHypersingularBoundaryOperator(
      context, hplusSpace, hminusSpace, hplusSpace, label + "_hyp", SYMMETRIC,
      internalSlp);

  BdOp idSpaceTransformation1 = identityOperator(
      context, hminusSpace, internalHplusSpace, internalHplusSpace);
  BdOp idSpaceTransformation2 =
      identityOperator(context, internalHplusSpace, hplusSpace, hminusSpace);
  BdOp idDouble =
      identityOperator(context, hplusSpace, hplusSpace, hminusSpace);
  BdOp idAdjDouble =
      identityOperator(context, hminusSpace, hminusSpace, hplusSpace);

  // Now Assemble the entries of the Calderon Projector

  BlockedOperatorStructure<BasisFunctionType, ResultType> structure;

  structure.setBlock(0, 0, .5 * idDouble + dlp);
  structure.setBlock(0, 1, -1. * idSpaceTransformation2 * internalSlp *
                               idSpaceTransformation1);
  structure.setBlock(1, 0, -1. * hyp);
  structure.setBlock(1, 1, .5 * idAdjDouble - adjDlp);

  return BlockedBoundaryOperator<BasisFunctionType, ResultType>(structure);
}

template <typename BasisFunctionType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
laplace3dInteriorCalderonProjector(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    const std::string &label) {

  typedef BoundaryOperator<BasisFunctionType, ResultType> BdOp;

  shared_ptr<const Space<BasisFunctionType>> internalHplusSpace =
      hplusSpace->discontinuousSpace(hplusSpace);

  BdOp internalSlp = laplace3dSingleLayerBoundaryOperator(
      context, internalHplusSpace, internalHplusSpace, internalHplusSpace,
      label + "_slp", SYMMETRIC);

  BdOp dlp = laplace3dDoubleLayerBoundaryOperator(context, hplusSpace,
                                                  hplusSpace, hminusSpace,
                                                  label + "_dlp", NO_SYMMETRY);

  BdOp adjDlp = adjoint(dlp);

  BdOp hyp = laplace3dHypersingularBoundaryOperator(
      context, hplusSpace, hminusSpace, hplusSpace, label + "_hyp", SYMMETRIC,
      internalSlp);

  BdOp idSpaceTransformation1 = identityOperator(
      context, hminusSpace, internalHplusSpace, internalHplusSpace);
  BdOp idSpaceTransformation2 =
      identityOperator(context, internalHplusSpace, hplusSpace, hminusSpace);
  BdOp idDouble =
      identityOperator(context, hplusSpace, hplusSpace, hminusSpace);
  BdOp idAdjDouble =
      identityOperator(context, hminusSpace, hminusSpace, hplusSpace);

  // Now Assemble the entries of the Calderon Projector

  BlockedOperatorStructure<BasisFunctionType, ResultType> structure;

  structure.setBlock(0, 0, .5 * idDouble - dlp);
  structure.setBlock(0, 1, idSpaceTransformation2 * internalSlp *
                               idSpaceTransformation1);
  structure.setBlock(1, 0, hyp);
  structure.setBlock(1, 1, .5 * idAdjDouble + adjDlp);

  return BlockedBoundaryOperator<BasisFunctionType, ResultType>(structure);
}

template <typename BasisFunctionType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
laplace3dExteriorCalderonProjector(
    const ParameterList &parameterList,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    const std::string &label) {

  shared_ptr<const Context<BasisFunctionType, ResultType>> context(
      new Context<BasisFunctionType, ResultType>(parameterList));
  return laplace3dExteriorCalderonProjector<BasisFunctionType, ResultType>(
      context, hminusSpace, hplusSpace, label);
}

template <typename BasisFunctionType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
laplace3dInteriorCalderonProjector(
    const ParameterList &parameterList,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    const std::string &label) {

  shared_ptr<const Context<BasisFunctionType, ResultType>> context(
      new Context<BasisFunctionType, ResultType>(parameterList));
  return laplace3dInteriorCalderonProjector<BasisFunctionType, ResultType>(
      context, hminusSpace, hplusSpace, label);
}

#define INSTANTIATE_NONMEMBER_CONSTRUCTOR(BASIS, RESULT)                       \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  laplace3dExteriorCalderonProjector(                                          \
      const ParameterList &, const shared_ptr<const Space<BASIS>> &,           \
      const shared_ptr<const Space<BASIS>> &, const std::string &);            \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  laplace3dInteriorCalderonProjector(                                          \
      const ParameterList &, const shared_ptr<const Space<BASIS>> &,           \
      const shared_ptr<const Space<BASIS>> &, const std::string &);            \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  laplace3dExteriorCalderonProjector(                                          \
      const shared_ptr<const Context<BASIS, RESULT>> &,                        \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, const std::string &);            \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  laplace3dInteriorCalderonProjector(                                          \
      const shared_ptr<const Context<BASIS, RESULT>> &,                        \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, const std::string &)

FIBER_ITERATE_OVER_BASIS_AND_RESULT_TYPES(INSTANTIATE_NONMEMBER_CONSTRUCTOR);

} // Bempp


#include "../include/cte_bits/bunch.hpp"
#include "../include/cte_bits/distributions.hpp"
#include "../include/cte_bits/physics.hpp"
#include "../include/cte_bits/radiation.hpp"
#include "../include/cte_bits/random.hpp"
#include "../include/cte_bits/synch.hpp"
#include "../include/cte_bits/utils.hpp"
#include <algorithm>
#include <boost/math/tools/roots.hpp>
#include <fstream>
#include <ibs>
#include <iterator>
#include <map>
#include <random>
#include <sstream>
#include <string>
#include <vector>

namespace cte_long {
/*
 *****************************************************************************
 *****************************************************************************
 * FUNCTOR TO RETURN VOLTAGE * CHARGE - U0 AND DERIVATIVE AS TUPLE
 * THIS IS USED AS INPUT FOR BOOST NEWTON RAPHSON ROOT SEARCH.
 *****************************************************************************
 * Authors:
 *  - Tom Mertens
 *
 * History:
 *  - 10/08/2021 : updated version from original ste code
 *****************************************************************************
 * Arguments:
 * ----------
 *  - T const &target
 *      Target value for the voltage (RF compensate for U0 so U0 in eV)
 *  - std::vector<double> &voltages
 *      Voltages of RF systems IMPORTANT: first element is for main RF
 *  - std::vector<double> &harmonicNumbers
 *      Harmonic numbers  IMPORTANT: first element is for main RF
 *  - double charge
 *      Particle charge
 *  - T const &phi
 *      phase in rad
 ******************************************************************************
 ******************************************************************************
 */
template <class T> struct synchronousPhaseFunctor {
  synchronousPhaseFunctor(T const &target, std::vector<double> &voltages,
                          std::vector<double> &harmonicNumbers, double charge)
      : U0(target), volts(voltages), hs(harmonicNumbers), ch(charge) {}
  std::tuple<double, double> operator()(T const &phi) {

    // init
    T vrf = ch * volts[0] * sin(phi);
    T dvrf = ch * volts[0] * cos(phi);

    // add the rest taking harmonic numbers into account
    for (int i = 1; i < hs.size(); i++) {
      vrf += ch * volts[i] * sin((hs[i] / hs[0]) * phi);
      dvrf += ch * volts[i] * (hs[i] / hs[0]) * cos((hs[i] / hs[0]) * phi);
    }

    std::tuple<double, double> out = {vrf - U0, dvrf};
    return out;
  }

private:
  T U0;
  std::vector<double> volts;
  std::vector<double> hs;
  double ch;
};

/*
================================================================================
================================================================================
BOOST NEWTON RAPHSON ROOT SEARCH FOR SYNCHRONOUS PHASE.
================================================================================

================================================================================
Arguments:

//phi is in rad
================================================================================
================================================================================
*/
template <class T>
T synchronousPhaseFunctorRoot(T x, std::vector<double> &voltages,
                              std::vector<double> &harmnumbers, double charge,
                              T guess, T min, T max) {
  // return cube root of x using 1st derivative and Newton_Raphson.
  using namespace boost::math::tools;

  const int digits =
      std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy
                                      // for type T.
  int get_digits = static_cast<int>(
      digits * 0.6); // Accuracy doubles with each step, so stop when we have
                     // just over half the digits correct.
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = newton_raphson_iterate(
      synchronousPhaseFunctor<T>(x, voltages, harmnumbers, charge), guess, min,
      max, get_digits, it);
  return result;
};

/********************************************************************************
 ********************************************************************************
 * CALCULATE TOTAL RF VOLTAGE FOR GIVEN PHASE (IN RAD)
 *
 ********************************************************************************
 */
double VoltageRf(double phi, std::vector<double> &volts,
                 std::vector<double> &hs) {
  double vrf = volts[0] * sin(phi);

  for (int i = 1; i < hs.size(); i++) {
    vrf += volts[i] * sin((hs[i] / hs[0]) * phi);
  }

  return vrf;
};

double VoltageRfPrime(double phi, double charge, std::vector<double> &volts,
                      std::vector<double> &hs) {
  // init
  double vrf = volts[0] * cos(phi);

  // add other rfs
  for (int i = 1; i < volts.size(); i++) {
    vrf += volts[i] * (hs[i] / hs[0]) * cos((hs[i] / hs[0]) * phi);
  }

  // V -> eV
  vrf *= charge;
  return vrf;
}

double SynchrotronTune(double omega0, double U0, double charge,
                       std::vector<double> &volts, std::vector<double> &hs,
                       double phis, double eta, double pc) {
  return sqrt(hs[0] * eta *
              fabs(charge * VoltageRfPrime(phis, charge, volts, hs)) /
              (2 * pi * pc * 1e9));
}

} // namespace cte_long

Bunch::Bunch(std::map<std::string, double> &twissheader,
             std::map<std::string, std::vector<double>> &twiss,
             std::map<std::string, double> &bparam, std::vector<double> &h,
             std::vector<double> &v) {
  twheader = twissheader;
  tw = twiss;
  bunchParam = bparam;

  setBasic();
  setLongitudinalParameters(h, v);
  setRadiationParameters();
  setDistribution(h, v);
};

void Bunch::setBasic() {
  // basic
  double gamma = twheader["GAMMA"];
  double gammatr = twheader["GAMMATR"];
  double p0 = twheader["PC"];
  double len = twheader["LENGTH"];
  double mass = twheader["MASS"];
  double charge = twheader["CHARGE"];
  // set Atomic Number in twiss header
  twheader["aatom"] = bunchParam["atomNumber"];
  twheader["sigs"] = bunchParam["sigs"];
  twheader["betar"] = BetaRelativisticFromGamma(gamma);
  twheader["trev"] = len / (twheader["betar"] * clight);
  twheader["frev"] = 1.0 / twheader["trev"];
  twheader["omega"] = 2.0 * pi * twheader["frev"];
  twheader["eta"] = eta(gamma, gammatr);
  twheader["timeratio"] = bunchParam["timeRatio"];
}

void Bunch::setLongitudinalParameters(std::vector<double> &h,
                                      std::vector<double> &v) {
  double gamma = twheader["GAMMA"];
  double gammatr = twheader["GAMMATR"];
  double angularf = twheader["trev"] * h[0] * twheader["omega"];

  // search synch phase parameters
  double search1 = angularf / (8.0 * *std::max_element(h.begin(), h.end()));
  double search2 = search1 + angularf / *std::min_element(h.begin(), h.end());
  double searchWidth = angularf / (2.0 * *std::max_element(h.begin(), h.end()));

  // bucket number determines phis
  search1 += bunchParam["bucket"] * 2.0 * pi;
  search2 += bunchParam["bucket"] * 2.0 * pi;

  // energy loss per turn
  double U0 = cte_radiation::RadiationLossesPerTurn(twheader);

  // synchronuous phases
  double phis = cte_long::synchronousPhaseFunctorRoot(
      U0, v, h, bunchParam["charge"], search1, search1 - searchWidth,
      search1 + searchWidth);
  double phis1 = cte_long::synchronousPhaseFunctorRoot(
      U0, v, h, bunchParam["charge"], search2, search2 - searchWidth,
      search2 + searchWidth);

  double phisNext = cte_long::synchronousPhaseFunctorRoot(
      U0, v, h, bunchParam["charge"], search1 + pi,
      search1 + pi - searchWidth / 2.0, search1 + pi + searchWidth / 2.0);
  double phis1Next = cte_long::synchronousPhaseFunctorRoot(
      U0, v, h, bunchParam["charge"], search2 + pi,
      search2 + pi - searchWidth / 2.0, search2 + pi + searchWidth / 2.0);

  // save
  longitudinalParameters["sigs"] = bunchParam["sigs"];
  longitudinalParameters["phis"] = phis;
  longitudinalParameters["phis1"] = phis1;
  longitudinalParameters["phisNext"] = phisNext;
  longitudinalParameters["phisNext1"] = phis1Next;

  longitudinalParameters["qs"] =
      CTESYNCH::SynchrotronTune(twheader, longitudinalParameters, v, h);
  longitudinalParameters["tauhat"] =
      abs((phisNext - phis) / (h[0] * twheader["omega"]));

  // add to twheader for easy passing around of the values (pre-existing code)
  // twheader["phis"] = phis;
  // twheader["phisNext"] = phisNext;
  // twheader["qs"] = longitudinalParameters["qs"];
  twheader["tauhat"] = longitudinalParameters["tauhat"];
  twheader["betxavg"] = twheader["LENGTH"] / (twheader["Q1"] * 2.0 * pi);
  twheader["betyavg"] = twheader["LENGTH"] / (twheader["Q2"] * 2.0 * pi);
  longitudinalParameters["sige"] =
      sigefromsigs(twheader["omega"], bunchParam["sigs"],
                   longitudinalParameters["qs"], gamma, gammatr);
  longitudinalParameters["delta"] =
      dee_to_dpp(twheader["sige"], twheader["betar"]);
}

void Bunch::setRadiationParameters() {
  // add energy loss per turn
  double U0 = cte_radiation::RadiationLossesPerTurn(twheader);
  // add the equilibrium values and radiation damping times
  radiationParameters =
      cte_radiation::radiationEquilib(twheader, longitudinalParameters);
  radiationParameters["U0"] = U0;
  cte_radiation::CalcRadDecayExcitation(twheader, radiationParameters);
};

void Bunch::setDistribution(std::vector<double> &h, std::vector<double> &v) {
  distribution = cte_distributions::GenerateDistributionMatched(
      bunchParam["nMacro"], twheader["betxavg"], bunchParam["ex"],
      twheader["betyavg"], bunchParam["ey"], h, v, twheader,
      longitudinalParameters, bunchParam["seed"]);
}

void Bunch::getEmittance() {
  std::vector<double> out, avg;
  std::vector<double> res;
  for (int i = 0; i < distribution[0].size(); i++) {
    out.push_back(0.0);
    avg.push_back(0.0);
  }

  // calculate averages
  std::for_each(distribution.begin(), distribution.end(),
                [&](std::vector<double> v) {
                  for (int i = 0; i < distribution[0].size(); i++) {
                    avg[i] += v[i] / distribution.size();
                  }
                });

  // subtract avg, square and sum
  std::for_each(
      distribution.begin(), distribution.end(), [&](std::vector<double> v) {
        for (int i = 0; i < distribution[0].size(); i++) {
          out[i] += ((v[i] - avg[i]) * (v[i] - avg[i])) / distribution.size();
        }
      });

  out[0] /= twheader["betxavg"];
  out[2] /= twheader["betyavg"];
  out[4] = sqrt(out[4]) * clight;
  out[5] = sqrt(out[5]);

  res.push_back(out[0]);
  res.push_back(out[2]);
  res.push_back(out[4]);
  res.push_back(out[5]);

  emittances.push_back(res);
}

void Bunch::getIBSGrowthRates(int model) {

  double pnumber = bunchParam["nReal"];
  std::vector<double> emit = emittances.back();
  double ex = emit[0];
  double ey = emit[1];
  double sigs = emit[2];
  double sige = emit[3];
  double aatom = twheader["aatom"];
  double r0 = ParticleRadius(twheader["CHARGE"], aatom);
  double *ibs;
  /*
    std::printf("%-30s %12.8e\n", "aatom", aatom);
    std::printf("%-30s %12.8e\n", "ex", ex);
    std::printf("%-30s %12.8e\n", "ey", ey);
    std::printf("%-30s %12.8e\n", "ro", r0);
    std::printf("%-30s %12.8e\n", "pnumber", pnumber);
  */
  // ibs growth rates update
  switch (model) {
  case 1:
    ibs = PiwinskiSmooth(pnumber, ex, ey, sigs, sige, twheader, r0);
    break;
  case 2:
    ibs = PiwinskiLattice(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 3:
    ibs =
        PiwinskiLatticeModified(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 4:
    ibs = Nagaitsev(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 5:
    ibs =
        Nagaitsevtailcut(pnumber, ex, ey, sigs, sige, twheader, tw, r0, aatom);
    break;
  case 6:
    ibs = ibsmadx(pnumber, ex, ey, sigs, sige, twheader, tw, r0, false);
    break;
  case 7:
    ibs = ibsmadxtailcut(pnumber, ex, ey, sigs, sige, twheader, tw, r0, aatom);
    break;
  case 8:
    ibs = BjorkenMtingwa2(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 9:
    ibs = BjorkenMtingwa(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 10:
    ibs = BjorkenMtingwatailcut(pnumber, ex, ey, sigs, sige, twheader, tw, r0,
                                aatom);
    break;
  case 11:
    ibs = ConteMartini(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 12:
    ibs = ConteMartinitailcut(pnumber, ex, ey, sigs, sige, twheader, tw, r0,
                              aatom);
    break;
  case 13:
    ibs = MadxIBS(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  }

  std::vector<double> ibsgr;
  for (int i = 0; i < 3; i++) {
    // std::printf("%-30s %12.8e\n", "ibs", ibs[i]);
    ibsgr.push_back(ibs[i] * 2.0 * bunchParam["fracibstot"] *
                    bunchParam["nMacro"] * bunchParam["timeRatio"] /
                    bunchParam["nReal"]);
  }
  ibsGrowthRates.push_back(ibsgr);
}

void Bunch::getIBSCoefficients() {
  double coeffs, coeffx, coeffy;
  double alphaAverage;
  double coeffMulT;

  std::vector<double> ibsgr = ibsGrowthRates.back();
  double alfax = ibsgr[1];
  double alfay = ibsgr[2];
  double alfap = ibsgr[0];
  /*
    std::printf("%-30s %12.8e\n", "alfax", alfax);
    std::printf("%-30s %12.8e\n", "alfay", alfay);
    std::printf("%-30s %12.8e\n", "alfap", alfap);
  */
  double dtsamp2 = 2.0 * longitudinalParameters["tauhat"] / bunchParam["nbins"];
  double rmsdelta = CalcRMS(distribution)[5];
  std::vector<double> emit = emittances.back();
  double sigs = emit[3];

  double rmsx = sqrt(emit[0] * twheader["betxavg"]);
  double rmsy = sqrt(emit[1] * twheader["betyavg"]);

  // debugging
  // cout << "alfap " << alfap << endl << endl;
  if (alfap > 0.0)
    coeffs = sqrt(6.0 * alfap * twheader["trev"]) * rmsdelta;
  else
    coeffs = 0.00;

  // coupling
  if (bunchParam["ibsCoupling"] == 0.0) {
    if (alfay > 0.0)
      coeffy = sqrt(6.0 * alfay * twheader["trev"]) * rmsy;
    else
      coeffy = 0.0;

    if (alfax > 0.0)
      coeffx = sqrt(6.0 * alfax * twheader["trev"]) * rmsx;
    else
      coeffx = 0.0;
  } else {
    // alphaAverage
    alphaAverage = 0.5 * (alfax + alfay);
    if (alphaAverage > 0.0) {
      coeffx = sqrt(6.0 * alphaAverage * twheader["trev"]) * rmsx;
      coeffy = sqrt(6.0 * alphaAverage * twheader["trev"]) * rmsy;
    } else {
      coeffx = 0.0;
      coeffy = 0.0;
    }
    // end if alphaAverage
  }
  // end if ibs coupling
  coeffMulT = sigs * 2.0 * sqrt(pi) / (bunchParam["nMacro"] * dtsamp2 * clight);

  std::vector<double> ibscoeffs;
  ibscoeffs.push_back(coeffx);
  ibscoeffs.push_back(coeffy);
  ibscoeffs.push_back(coeffs);
  ibscoeffs.push_back(coeffMulT);

  ibsCoeff.push_back(ibscoeffs);
}

void Bunch::updateIBS(std::vector<double> &h) {
  histogramTime = ParticlesTimesToHistogram(
      distribution, bunchParam["nbins"], longitudinalParameters["tauhat"] / 2.,
      longitudinalParameters["phisNext"] / (h[0] * twheader["omega"]));
  /*
    for (std::vector<int>::const_iterator i = histogramTime.begin();
         i != histogramTime.end(); ++i) {
      std::printf("%3i\n", *i);
    }
  */
  getIBSGrowthRates(bunchParam["model"]);
  getIBSCoefficients();
  /*
  std::for_each(ibsCoeff.begin(), ibsCoeff.end(),
                [](std::vector<double> &particle) {
                  std::printf("%12.8e %12.8e %12.8e \n", particle[0],
                              particle[1], particle[2]);
                });
*/
  sqrthistogram = HistogramToSQRTofCumul(histogramTime, ibsCoeff.back()[3]);
  // for (std::vector<double>::const_iterator i = sqrthistogram.begin();
  //   i != sqrthistogram.end(); ++i)
  //  std::printf("%4.2f ", *i);
  // std::printf("\n");
}
/*
************************************************************************************************************************
************************************************************************************************************************
*/
void Bunch::printBunchParameters() {
  std::printf("%-30s\n", "Bunch Parameters");
  std::printf("%-30s\n", "================");
  for (auto const &pair : bunchParam) {
    std::printf("%-30s %16.8e\n", pair.first.c_str(), pair.second);
  }
  std::printf("\n");
}

void Bunch::printTwissHeader() {
  std::printf("%-30s\n", "Twiss Header");
  std::printf("%-30s\n", "============");
  for (auto const &pair : twheader) {
    std::printf("%-30s %16.8e\n", pair.first.c_str(), pair.second);
  }
  std::printf("\n");
}

void Bunch::printLongParam() {
  std::printf("%-30s\n", "Longitudinal Parameters");
  std::printf("%-30s\n", "=======================");
  for (auto const &pair : longitudinalParameters) {
    std::printf("%-30s %16.8e\n", pair.first.c_str(), pair.second);
  }
  std::printf("\n");
}
void Bunch::printRadiationParameters() {
  std::printf("%-30s\n", "Radiation Parameters");
  std::printf("%-30s\n", "===================");
  for (auto const &pair : Bunch::radiationParameters) {
    std::printf("%-30s %16.8e\n", pair.first.c_str(), pair.second);
  }
  std::printf("\n");
}

void Bunch::printDistribution() {
  std::for_each(distribution.begin(), distribution.end(),
                [](std::vector<double> &particle) {
                  std::printf("%12.8e %12.8e %12.8e %12.8e %12.8e %12.8e\n",
                              particle[0], particle[1], particle[2],
                              particle[3], particle[4], particle[5]);
                });

  std::printf("\n");
};

void Bunch::printEmittance() {
  std::for_each(emittances.begin(), emittances.end(),
                [](std::vector<double> &particle) {
                  std::printf("%12.8e %12.8e %12.8e %12.8e\n", particle[0],
                              particle[1], particle[2], particle[3]);
                });

  std::printf("\n");
};

void Bunch::printIBSGrowthRates() {
  std::for_each(ibsGrowthRates.begin(), ibsGrowthRates.end(),
                [](std::vector<double> &particle) {
                  std::printf("%12.8e %12.8e %12.8e\n", particle[0],
                              particle[1], particle[2]);
                });

  std::printf("\n");
};

void Bunch::printDebunchLosses() {
  for (int i = 0; i < debunchLosses.size(); i++) {
    std::printf("%12i\n", debunchLosses[i]);
  };
}
#define MAX_DATE 12

std::string Bunch::get_date() {

  /* function returning current date as string for creating timestamped output
   * files */

  time_t now;

  // struct tm * timeinfo;
  char the_date[MAX_DATE];
  // char buffer [80];

  // time_t rawtime;
  // timeinfo = localtime (&rawtime);
  // strftime (buffer,80,"Now it's %I:%M%p.",timeinfo);

  // std::stringstream ss;
  // ss << fn << "_" << "%d_%m_%Y" << "_" << buffer << ".dat";
  // std::string s = ss.str();

  the_date[0] = '\0';

  now = time(NULL);

  if (now != -1) {
    strftime(the_date, MAX_DATE, "%d_%m_%Y", gmtime(&now));
  }

  return std::string(the_date);
}

template <typename T>
std::ostream &operator<<(std::ostream &os, const std::vector<T> &v) {
  for (int i = 0; i < v.size(); i++) {
    os << std::scientific << std::setw(15) << v[i];
  }
  // os << std::endl;
  // std::copy(v.begin(), v.end(), ostream_iterator<T>(os, "\t"));
  return os;
}

template <typename T>
std::ostream &operator<<(std::ostream &os,
                         const std::vector<std::vector<T>> &v) {
  using namespace std;

  // NOTE: for some reason std::copy doesn't work here, so I use manual loop
  // copy(v.begin(), v.end(), ostream_iterator<std::vector<T>>(os, "\n"));

  for (size_t i = 0; i < v.size(); ++i)
    os << std::setw(6) << i << v[i] << "\n";
  return os;
}

void Bunch::writeDistribution(int turn) {

  std::stringstream ss;
  ss << "Distribution_bucket_" << bunchParam["bucket"] << "_"
     << get_date()
     // << "_turn_" << std::setw(10) << std::setfill('0') << turn
     << ".dat";

  std::string s;
  s = ss.str();

  if (turn == 0) {
    if (remove(s.c_str()) != 0)
      perror("Error deleting file");
    else
      puts("File successfully deleted");
  }
  std::ofstream ofile;
  ofile.open(s.c_str(), std::ios_base::app);
  ofile << distribution;
  // std::copy(distribution.begin(), distribution.end(),
  //        std::ostream_iterator<std::vector<double>>(ofile));

  // std::cout << "writing vector size = " << distribution.size() << std::endl;
  ofile.close();
}

void Bunch::writeEmittances() {

  std::stringstream ss;
  ss << "CTE_Emittances_" << bunchParam["bucket"] << "_" << get_date()
     << ".dat";

  std::string s;
  s = ss.str();

  if (remove(s.c_str()) != 0)
    perror("Error deleting file");
  else
    puts("File successfully deleted");

  std::ofstream ofile(s.c_str());
  ofile << emittances;

  ofile.close();
}


#include "utils/stl_to_string.hpp"
#include "trajopt/kinematic_constraints.hpp"
#include "trajopt/utils.hpp"
#include "trajopt/rave_utils.hpp"
#include "utils/logging.hpp"
#include "sco/expr_ops.hpp"
#include "sco/modeling_utils.hpp"
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <iostream>
#include <Eigen/Geometry>
#include <openrave/openrave.h>
#include "utils/eigen_conversions.hpp"
#include "utils/eigen_slicing.hpp"
using namespace std;
using namespace sco;
using namespace Eigen;
using namespace util;

namespace {

static MatrixXd diffAxis0(const MatrixXd& in) {
  return in.middleRows(1, in.rows()-1) - in.middleRows(0, in.rows()-1);
}
Vector3d rotVec(const Matrix3d& m) {
  Quaterniond q; q = m;
  return Vector3d(q.x(), q.y(), q.z());
}
Vector3d rotVec(const OpenRAVE::Vector& q) {
  return Vector3d(q[1], q[2], q[3]);
}

VectorXd concat(const VectorXd& a, const VectorXd& b) {
  VectorXd out(a.size()+b.size());
  out.topRows(a.size()) = a;
  out.middleRows(a.size(), b.size()) = b;
  return out;
}

template <typename T>
vector<T> concat(const vector<T>& a, const vector<T>& b) {
  vector<T> out;
  vector<int> x;
  out.insert(out.end(), a.begin(), a.end());
  out.insert(out.end(), b.begin(), b.end());
  return out;
}

}

namespace trajopt {

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

void makeTrajVariablesAndBounds(int n_steps, RobotAndDOFPtr manip, OptProb& prob_out, VarArray& vars_out) {
  int n_dof = manip->GetDOF();
  DblVec lower, upper;
  manip->GetDOFLimits(lower, upper);
  LOG_INFO("Dof limits: %s, %s", CSTR(lower), CSTR(upper));
  vector<double> vlower, vupper;
  vector<string> names;
  for (int i=0; i < n_steps; ++i) {
    vlower.insert(vlower.end(), lower.data(), lower.data()+lower.size());
    vupper.insert(vupper.end(), upper.data(), upper.data()+upper.size());
    for (unsigned j=0; j < n_dof; ++j) {
      names.push_back( (boost::format("j_%i_%i")%i%j).str() );
    }
  }

  prob_out.createVariables(names, vlower, vupper);
  vars_out = VarArray(n_steps, n_dof, prob_out.getVars().data());

}

JointPosCost::JointPosCost(const VarVector& vars, const VectorXd& vals, const VectorXd& coeffs) :
    Cost("JointVel"), vars_(vars), vals_(vals), coeffs_(coeffs) {
    for (int i=0; i < vars.size(); ++i) {
      if (coeffs[i] > 0) {
        AffExpr diff = exprSub(AffExpr(vars[i]), AffExpr(vals[i]));
        exprInc(expr_, exprMult(exprSquare(diff), coeffs[i]));
      }
    }
}
double JointPosCost::value(const vector<double>& xvec) {
  VectorXd dofs = getVec(xvec, vars_);
  return ((dofs - vals_).array().square() * coeffs_.array()).sum();
}
ConvexObjectivePtr JointPosCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointVelCost::JointVelCost(const VarArray& vars, const VectorXd& coeffs) :
    Cost("JointVel"), vars_(vars), coeffs_(coeffs) {
  for (int i=0; i < vars.rows()-1; ++i) {
    for (int j=0; j < vars.cols(); ++j) {
      AffExpr vel;
      exprInc(vel, exprMult(vars(i,j), -1));
      exprInc(vel, exprMult(vars(i+1,j), 1));
      exprInc(expr_, exprMult(exprSquare(vel),coeffs_[j]));
    }
  }
}
double JointVelCost::value(const vector<double>& xvec) {
  MatrixXd traj = getTraj(xvec, vars_);
  return (diffAxis0(traj).array().square().matrix() * coeffs_.asDiagonal()).sum();
}
ConvexObjectivePtr JointVelCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointAccCost::JointAccCost(const VarArray& vars, const VectorXd& coeffs) :
    Cost("JointAcc"), vars_(vars), coeffs_(coeffs) {
  for (int i=0; i < vars.rows()-2; ++i) {
    for (int j=0; j < vars.cols(); ++j) {
      AffExpr acc;
      exprInc(acc, exprMult(vars(i,j), -1));
      exprInc(acc, exprMult(vars(i+1,j), 2));
      exprInc(acc, exprMult(vars(i+2,j), -1));
      exprInc(expr_, exprMult(exprSquare(acc), coeffs_[j]));
    }
  }
}
double JointAccCost::value(const vector<double>& xvec) {
  MatrixXd traj = getTraj(xvec, vars_);
  return (diffAxis0(diffAxis0(traj)).array().square().matrix() * coeffs_.asDiagonal()).sum();
}
ConvexObjectivePtr JointAccCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

struct CartPoseErrCalculator : public VectorOfVector {
  OR::Transform pose_inv_;
  RobotAndDOFPtr manip_;
  OR::KinBody::LinkPtr link_;
  VectorXd coeffs_;
  CartPoseErrCalculator(const OR::Transform& pose, RobotAndDOFPtr manip, OR::KinBody::LinkPtr link, const VectorXd& coeffs) :
  pose_inv_(pose.inverse()),
  manip_(manip),
  link_(link),
  coeffs_(coeffs)
  {}
//  CartPoseCostCalculator(const CartPoseCostCalculator& other) : pose_(other.pose_), manip_(other.manip_), rs_(other.rs_) {}
  VectorXd operator()(const VectorXd& dof_vals) const {
    manip_->SetDOFValues(toDblVec(dof_vals));
    OR::Transform newpose = link_->GetTransform();

    OR::Transform pose_err = pose_inv_ * newpose;
    VectorXd err = coeffs_.cwiseProduct(concat(rotVec(pose_err.rot), toVector3d(pose_err.trans)));
    return err;
  }
};

CartPoseCost::CartPoseCost(const VarVector& vars, const OR::Transform& pose, const Vector3d& rot_coeffs,
    const Vector3d& pos_coeffs, RobotAndDOFPtr manip, KinBody::LinkPtr link) :
    CostFromErrFunc(VectorOfVectorPtr(new CartPoseErrCalculator(pose, manip, link, VectorXd::Ones(6))),
        vars, concat(rot_coeffs, pos_coeffs), ABS, "CartPose")
{}
void CartPoseCost::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  CartPoseErrCalculator* calc = static_cast<CartPoseErrCalculator*>(f_.get());
  DblVec dof_vals = getDblVec(x, vars_);
  calc->manip_->SetDOFValues(dof_vals);
  OR::Transform target = calc->pose_inv_.inverse(), cur = calc->link_->GetTransform();
  PlotAxes(env, cur, .1,  handles);
  PlotAxes(env, target, .1,  handles);
  handles.push_back(env.drawarrow(cur.trans, target.trans, .01, OR::Vector(1,0,1,1)));
}

CartPoseConstraint::CartPoseConstraint(const VarVector& vars, const OR::Transform& pose,
    RobotAndDOFPtr manip, KinBody::LinkPtr link, const VectorXd& coeffs) :
    ConstraintFromFunc(VectorOfVectorPtr(new CartPoseErrCalculator(pose, manip, link, coeffs)),
        vars, EQ, "CartPose")
{
}

void CartPoseConstraint::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  // IDENTITCAL TO CartPoseCost::Plot
  CartPoseErrCalculator* calc = static_cast<CartPoseErrCalculator*>(f_.get());
  DblVec dof_vals = getDblVec(x, vars_);
  calc->manip_->SetDOFValues(dof_vals);
  OR::Transform target = calc->pose_inv_.inverse(), cur = calc->link_->GetTransform();
  PlotAxes(env, cur, .1,  handles);
  PlotAxes(env, target, .1,  handles);
  handles.push_back(env.drawarrow(cur.trans, target.trans, .01, OR::Vector(1,0,1,1)));
}

struct CartPositionErrCalculator {
  Vector3d pt_world_;
  RobotAndDOFPtr manip_;
  OR::KinBody::LinkPtr link_;
  CartPositionErrCalculator(const Vector3d& pt_world, RobotAndDOFPtr manip, OR::KinBody::LinkPtr link) :
  pt_world_(pt_world),
  manip_(manip),
  link_(link)
  {}
  VectorXd operator()(const VectorXd& dof_vals) {
    manip_->SetDOFValues(toDblVec(dof_vals));
    OR::Transform newpose = link_->GetTransform();
    return pt_world_ - toVector3d(newpose.trans);
  }
};

struct CartVelJacCalculator : MatrixOfVector {
  RobotAndDOFPtr manip_;
  KinBody::LinkPtr link_;
  double limit_;
  CartVelJacCalculator(RobotAndDOFPtr manip, KinBody::LinkPtr link, double limit) :
    manip_(manip), link_(link), limit_(limit) {}

  MatrixXd operator()(const VectorXd& dof_vals) const {
    int n_dof = manip_->GetDOF();
    MatrixXd out(6, 2*n_dof);
    manip_->SetDOFValues(toDblVec(dof_vals.topRows(n_dof)));
    OR::Transform pose0 = link_->GetTransform();
    MatrixXd jac0 = manip_->PositionJacobian(link_->GetIndex(), pose0.trans);
    manip_->SetDOFValues(toDblVec(dof_vals.bottomRows(n_dof)));
    OR::Transform pose1 = link_->GetTransform();
    MatrixXd jac1 = manip_->PositionJacobian(link_->GetIndex(), pose1.trans);
    out.block(0,0,3,n_dof) = -jac0;
    out.block(0,n_dof,3,n_dof) = jac1;
    out.block(3,0,3,n_dof) = jac0;
    out.block(3,n_dof,3,n_dof) = -jac1;
    return out;
  }
};

struct CartVelCalculator : VectorOfVector {
  RobotAndDOFPtr manip_;
  KinBody::LinkPtr link_;
  double limit_;
  CartVelCalculator(RobotAndDOFPtr manip, KinBody::LinkPtr link, double limit) :
    manip_(manip), link_(link), limit_(limit) {}

  VectorXd operator()(const VectorXd& dof_vals) const {
    int n_dof = manip_->GetDOF();
    manip_->SetDOFValues(toDblVec(dof_vals.topRows(n_dof)));
    OR::Transform pose0 = link_->GetTransform();
    manip_->SetDOFValues(toDblVec(dof_vals.bottomRows(n_dof)));
    OR::Transform pose1 = link_->GetTransform();
    VectorXd out(6);
    out.topRows(3) = toVector3d(pose1.trans - pose0.trans - OR::Vector(limit_,limit_,limit_));
    out.bottomRows(3) = toVector3d( - pose1.trans + pose0.trans - OR::Vector(limit_, limit_, limit_));
    return out;
  }
};

CartVelConstraint::CartVelConstraint(const VarVector& step0vars, const VarVector& step1vars, RobotAndDOFPtr manip, KinBody::LinkPtr link, double distlimit) :
        ConstraintFromFunc(VectorOfVectorPtr(new CartVelCalculator(manip, link, distlimit)),
                           MatrixOfVectorPtr(new CartVelJacCalculator(manip, link, distlimit)),
                          concat(step0vars, step1vars), INEQ, "CartVel")
{}

struct UpErrorCalculator {
  Vector3d dir_local_;
  Vector3d goal_dir_world_;
  RobotAndDOFPtr manip_;
  OR::KinBody::LinkPtr link_;
  MatrixXd perp_basis_; // 2x3 matrix perpendicular to goal_dir_world
  UpErrorCalculator(const Vector3d& dir_local, const Vector3d& goal_dir_world, RobotAndDOFPtr manip, KinBody::LinkPtr link) :
    dir_local_(dir_local),
    goal_dir_world_(goal_dir_world),
    manip_(manip),
    link_(link)
  {
    Vector3d perp0 = goal_dir_world_.cross(Vector3d::Random()).normalized();
    Vector3d perp1 = goal_dir_world_.cross(perp0);
    perp_basis_.resize(2,3);
    perp_basis_.row(0) = perp0.transpose();
    perp_basis_.row(1) = perp1.transpose();
  }
  VectorXd operator()(const VectorXd& dof_vals) {
    manip_->SetDOFValues(toDblVec(dof_vals));
    OR::Transform newpose = link_->GetTransform();
    return perp_basis_*(toRot(newpose.rot) * dir_local_ - goal_dir_world_);
  }
};
}