void mexFunction(	int nlhs, mxArray *plhs[], 
				 int nrhs, const mxArray*prhs[] ) 
{ 
	/* retrive arguments */
	if( nrhs<5 ) 
		mexErrMsgTxt("5 or 6 input arguments are required."); 
	if( nlhs<1 ) 
		mexErrMsgTxt("1 or 2 output arguments are required."); 

	// first argument : weight list
	n = mxGetM(prhs[0]); 
	p = mxGetN(prhs[0]);
	W = mxGetPr(prhs[0]);
	// second argument : start_points
	start_points = mxGetPr(prhs[1]);
	int tmp = mxGetM(prhs[1]); 
	nb_start_points = mxGetN(prhs[1]);
	if( nb_start_points==0 || tmp!=2 )
		mexErrMsgTxt("start_points must be of size 2 x nb_start_poins."); 
	// third argument : end_points
	end_points = mxGetPr(prhs[2]);
	tmp = mxGetM(prhs[2]); 
	nb_end_points = mxGetN(prhs[2]);
	if( nb_end_points!=0 && tmp!=2 )
		mexErrMsgTxt("end_points must be of size 2 x nb_end_poins."); 
	// fourth argument : center_point
	center_point = mxGetPr(prhs[3]);
	// fifth argument : nb_iter_max
	nb_iter_max = (int) *mxGetPr(prhs[4]);
	// sixth argument : heuristic
	if( nrhs==6 )
	{
		H = mxGetPr(prhs[5]);
		if( mxGetM(prhs[5])!=n || mxGetN(prhs[5])!=p )
			mexErrMsgTxt("H must be of size n x p."); 
	}
	else
		H = NULL;

	// first ouput : distance
	plhs[0] = mxCreateDoubleMatrix(n, p, mxREAL); 
	D = mxGetPr(plhs[0]);
	// second output : state
	if( nlhs>=2 )
	{
		plhs[1] = mxCreateDoubleMatrix(n, p, mxREAL); 
		S = mxGetPr(plhs[1]);
	}
	else
	{
		S = new double[n*p];
	}

	// launch the propagation
	perform_front_propagation_2d(callback_intert_node);

	if( nlhs<2 )
		GW_DELETEARRAY(S);
	return;
}
void mexFunction(	int nlhs, mxArray *plhs[], 
				 int nrhs, const mxArray*prhs[] ) 
{ 
	/* retrive arguments */
	if( nrhs<4 ) 
		mexErrMsgTxt("4 or 5 input arguments are required."); 
	if( nlhs<1 ) 
		mexErrMsgTxt("1 or 2 output arguments are required."); 


     /* dealing with sparse array */
    W   = mxGetPr(prhs[0]); // returns a pointer to the numerical values
 	irs     = (mwIndex *)mxGetIr(prhs[0]); // returns a pointer to the row indices
    jcs     = (mwIndex *)mxGetJc(prhs[0]); // returns a pointer to the column pointer array
    n = mxGetM(prhs[0]);
    

	// second argument : start_points
	start_points = mxGetPr(prhs[1]);
	nb_start_points = mxGetN(prhs[1]);
	// third argument : end_points
	end_points = mxGetPr(prhs[2]);
	nb_end_points = mxGetN(prhs[2]);
	// third argument : nb_iter_max
	nb_iter_max = (int) *mxGetPr(prhs[3]);
	// second argument : heuristic
	if( nrhs==5 )
	{
		H = mxGetPr(prhs[4]);
		if( H!=NULL && (mxGetM(prhs[4])!=n || mxGetN(prhs[4])!=1) )
			mexErrMsgTxt("H must be of size n x 1."); 
	}
	else
		H = NULL;
	// first ouput : distance
	plhs[0] = mxCreateDoubleMatrix(n, 1, mxREAL); 
	D = mxGetPr(plhs[0]);
	// second output : state
	if( nlhs>=2 )
	{
		plhs[1] = mxCreateDoubleMatrix(n, 1, mxREAL); 
		S = mxGetPr(plhs[1]);
	}
	else
	{
		S = new double[n];
	}

	// launch the propagation
	perform_dijkstra_propagation();

	if( nlhs<2 )
		GW_DELETEARRAY(S);
	return;
}
void mexFunction(	int nlhs, mxArray *plhs[], 
				 int nrhs, const mxArray*prhs[] ) 
{ 
	/* retrive arguments */
	if( nrhs<4 ) 
		mexErrMsgTxt("4 - 7 input arguments are required."); 
	if( nlhs<1 ) 
		mexErrMsgTxt("1, 2 or 3 output arguments are required."); 

	// first argument : weight list
	n = mxGetM(prhs[0]); 
	p = mxGetN(prhs[0]);
	W = mxGetPr(prhs[0]);
	// second argument : start_points
	start_points = mxGetPr(prhs[1]);
	int tmp = mxGetM(prhs[1]); 
	nb_start_points = mxGetN(prhs[1]);
	if( nb_start_points==0 || tmp!=2 )
		mexErrMsgTxt("start_points must be of size 2 x nb_start_poins."); 
	// third argument : end_points
	end_points = mxGetPr(prhs[2]);
	tmp = mxGetM(prhs[2]); 
	nb_end_points = mxGetN(prhs[2]);
	if( nb_end_points!=0 && tmp!=2 )
		mexErrMsgTxt("end_points must be of size 2 x nb_end_poins."); 
	//  argument 4: nb_iter_max
	nb_iter_max = (int) *mxGetPr(prhs[3]);
	//  argument 5: heuristic
	if( nrhs>=5 )
	{
		H = mxGetPr(prhs[4]);
		if( mxGetM(prhs[4])==0 && mxGetN(prhs[4])==0 )
			H=NULL;
		if( H!=NULL && (mxGetM(prhs[4])!=n || mxGetN(prhs[4])!=p) )
			mexErrMsgTxt("H must be of size n x p.");  
	}
	else
		H = NULL;
	// argument 6: constraint map
	if( nrhs>=6 )
	{
		L = mxGetPr(prhs[5]);
		if( mxGetM(prhs[5])==0 && mxGetN(prhs[5])==0 )
			H=NULL;
		if( L!=NULL && (mxGetM(prhs[5])!=n || mxGetN(prhs[5])!=p) )
			mexErrMsgTxt("L must be of size n x p."); 
	}
	else
		L = NULL;
	// argument 7: value list
	if( nrhs>=7 )
	{
		values = mxGetPr(prhs[6]);
		if( mxGetM(prhs[6])==0 && mxGetN(prhs[6])==0 )
			values=NULL;
		if( values!=NULL && (mxGetM(prhs[6])!=nb_start_points || mxGetN(prhs[6])!=1) )
			mexErrMsgTxt("values must be of size nb_start_points x 1."); 
	}
	else
		values = NULL;
		
		
	// first ouput : distance
	plhs[0] = mxCreateDoubleMatrix(n, p, mxREAL); 
	D = mxGetPr(plhs[0]);
	// second output : state
	if( nlhs>=2 )
	{
		plhs[1] = mxCreateDoubleMatrix(n, p, mxREAL); 
		S = mxGetPr(plhs[1]);
	}
	else
	{
		S = new double[n*p];
	}
	// third output : index
	if( nlhs>=3 )
	{
		plhs[2] = mxCreateDoubleMatrix(n, p, mxREAL); 
		Q = mxGetPr(plhs[2]);
	}
	else
	{
		Q = new double[n*p];
	} 
	
	// launch the propagation
	perform_front_propagation_2d(); 
	

	if( nlhs<2 )
		GW_DELETEARRAY(S);		
	if( nlhs<3 )
		GW_DELETEARRAY(Q);
	return;
}
void perform_front_propagation_2d(T_callback_intert_node callback_insert_node)
{
	// create the Fibonacci heap
	struct fibheap* open_heap = fh_makeheap();
	fh_setcmp(open_heap, compare_points);

	double h = 1.0/n;
	
	// initialize points
	for( int i=0; i<n; ++i )
	for( int j=0; j<p; ++j )
	{
		D_(i,j) = GW_INFINITE;
		S_(i,j) = kFar;
		Q_(i,j) = -1;
	}

	// record all the points
	heap_pool = new fibheap_el*[n*p]; 
	memset( heap_pool, NULL, n*p*sizeof(fibheap_el*) );

	// inialize open list
	point_list existing_points;
	for( int k=0; k<nb_start_points; ++k )
	{
		int i = (int) start_points_(0,k);
		int j = (int) start_points_(1,k);

		if( D_( i,j )==0 )
			ERROR_MSG("start_points should not contain duplicates.");

		point* pt = new point( i,j );
		existing_points.push_back( pt );			// for deleting at the end
		heap_pool_(i,j) = fh_insert( open_heap, pt );			// add to heap
		if( values==NULL ) 
			D_( i,j ) = 0;
		else
			D_( i,j ) = values[k];
		S_( i,j ) = kOpen;
		Q_(i,j) = k;
	}

	// perform the front propagation
	int num_iter = 0;
	bool stop_iteration = GW_False;
	while( !fh_isempty(open_heap) && num_iter<nb_iter_max && !stop_iteration )
	{
		num_iter++;

		// current point
		point& cur_point = * ((point*) fh_extractmin( open_heap ));
		int i = cur_point.i;
		int j = cur_point.j;
		heap_pool_(i,j) = NULL;
		S_(i,j) = kDead;
		stop_iteration = end_points_reached(i,j);
		
		/*
		char msg[200];
		sprintf(msg, "Cool %f", Q_(i,j) );
		WARN_MSG( msg ); 
		*/
		
		CHECK_HEAP;

		// recurse on each neighbor
		int nei_i[4] = {i+1,i,i-1,i};
		int nei_j[4] = {j,j+1,j,j-1};
		for( int k=0; k<4; ++k )
		{
			int ii = nei_i[k];
			int jj = nei_j[k];
			bool bInsert = true;
			if( callback_insert_node!=NULL )
				bInsert = callback_insert_node(i,j,ii,jj);
			// check that the contraint distance map is ok
			if( ii>=0 && jj>=0 && ii<n && jj<p && bInsert )
			{
				double P = h/W_(ii,jj);
				// compute its neighboring values
				double a1 = GW_INFINITE;
				int k1 = -1;
				if( ii<n-1 )
				{
					bool bParticipate = true;
					if( callback_insert_node!=NULL )
						bParticipate = callback_insert_node(ii,jj,ii+1,jj);
					if( bParticipate )
					{
						a1 = D_(ii+1,jj);
						k1 = Q_(ii+1,jj);
					}
				}
				if( ii>0 )
				{
					bool bParticipate = true;
					if( callback_insert_node!=NULL )
						bParticipate = callback_insert_node(ii,jj,ii-1,jj);
					if( bParticipate )
					{
						if( D_(ii-1,jj)<a1 )
							k1 = Q_(ii-1,jj);
						a1 = GW_MIN( a1, D_(ii-1,jj) );
					}
				}
				double a2 = GW_INFINITE;
				int k2 = -1;
				if( jj<p-1 )
				{

					bool bParticipate = true;
					if( callback_insert_node!=NULL )
						bParticipate = callback_insert_node(ii,jj,ii,jj+1);
					if( bParticipate )
					{
						a2 = D_(ii,jj+1);
						k2 = Q_(ii,jj+1);
					}
				}
				if( jj>0 )
				{
					bool bParticipate = true;
					if( callback_insert_node!=NULL )
						bParticipate = callback_insert_node(ii,jj,ii,jj-1);
					if( bParticipate )
					{
						if( D_(ii,jj-1)<a2 )
							k2 = Q_(ii,jj-1);
						a2 = GW_MIN( a2, D_(ii,jj-1) );
					}
				}
				if( a1>a2 )	// swap so that a1<a2
				{
					double tmp = a1; a1 = a2; a2 = tmp;
					int tmpi = k1; k1 = k2; k2 = tmpi;
				}
				// update its distance
				// now the equation is   (a-a1)^2+(a-a2)^2 = P, with a >= a2 >= a1.
				double A1 = 0;
				if( P*P > (a2-a1)*(a2-a1) )
				{
					double delta = 2*P*P-(a2-a1)*(a2-a1);
					A1 = (a1+a2+sqrt(delta))/2.0;
				}
				else
					A1 = a1 + P;
				if( ((int) S_(ii,jj)) == kDead )
				{
					// check if action has change. Should not happen for FM
					// if( A1<D_(ii,jj) )
					//	WARN_MSG("The update is not monotone");
#if 1
					if( A1<D_(ii,jj) )	// should not happen for FM
					{
						D_(ii,jj) = A1;
						// update the value of the closest starting point
						//if( GW_ABS(a1-A1)<GW_ABS(a2-A1) && k1>=0  )
							Q_(ii,jj) = k1;
						//else
						//	Q_(ii,jj) = k2;
						//Q_(ii,jj) = Q_(i,j);
					}
#endif
				}
				else if( ((int) S_(ii,jj)) == kOpen )
				{
					// check if action has change.
					if( A1<D_(ii,jj) )
					{
						D_(ii,jj) = A1;
						// update the value of the closest starting point
						//if( GW_ABS(a1-A1)<GW_ABS(a2-A1) && k1>=0  )
							Q_(ii,jj) = k1;
						//else
						//	Q_(ii,jj) = k2;
						//Q_(ii,jj) = Q_(i,j);
						// Modify the value in the heap
						fibheap_el* cur_el = heap_pool_(ii,jj);
						if( cur_el!=NULL )
							fh_replacedata( open_heap, cur_el, cur_el->fhe_data );	// use same data for update
						else
							ERROR_MSG("Error in heap pool allocation."); 
					}
				}
				else if( ((int) S_(ii,jj)) == kFar )
				{
					if( D_(ii,jj)!=GW_INFINITE )
						ERROR_MSG("Distance must be initialized to Inf");
					if( L==NULL || A1<=L_(ii,jj) )
					{
						S_(ii,jj) = kOpen;
						// distance must have change.
						D_(ii,jj) = A1;
						// update the value of the closest starting point
						//if( GW_ABS(a1-A1)<GW_ABS(a2-A1) && k1>=0 )
							Q_(ii,jj) = k1;
						//else
						//	Q_(ii,jj) = k2;
						//Q_(ii,jj) = Q_(i,j);
						// add to open list
						point* pt = new point(ii,jj);
						existing_points.push_back( pt );
						heap_pool_(ii,jj) = fh_insert( open_heap, pt );			// add to heap	
					}
				}
				else 
					ERROR_MSG("Unkwnown state."); 
					
			}	// end switch
		}		// end for
	}			// end while

//				char msg[200];
//				sprintf(msg, "Cool %f", Q_(100,100) );
//				 WARN_MSG( msg ); 

	// free heap
	fh_deleteheap(open_heap);
	// free point pool
	for( point_list::iterator it = existing_points.begin(); it!=existing_points.end(); ++it )
		GW_DELETE( *it );
	// free fibheap pool
	GW_DELETEARRAY(heap_pool);
}
void perform_dijkstra_propagation(T_callback_insert_node callback_insert_node = NULL)
{
	// create the Fibonacci heap
	struct fibheap* open_heap = fh_makeheap();
	fh_setcmp(open_heap, compare_points);

	// initialize points
	for( int i=0; i<n; ++i )
	{
		D[i] = GW_INFINITE;
		S[i] = kFar;
	}

	// record all the points
	heap_pool = new fibheap_el*[n]; 
	memset( heap_pool, NULL, n*sizeof(fibheap_el*) );

	// inialize open list
	for( int k=0; k<nb_start_points; ++k )
	{
		int i = (int) start_points[k];

		if( D[i]==0 )
			mexErrMsgTxt("start_points should not contain duplicates.");

		heap_pool[i] = fh_insert( open_heap, (void*) i );			// add to heap
		D[i] = 0;
		S[i] = kOpen;
	}

	// perform the front propagation
	int num_iter = 0;
	bool stop_iteration = GW_False;
	while( !fh_isempty(open_heap) && num_iter<nb_iter_max && !stop_iteration )
	{
		num_iter++;

		// current point
		int i = (int) fh_extractmin( open_heap );
		heap_pool[i] = NULL;
		S[i] = kDead;
		stop_iteration = end_points_reached(i);

		CHECK_HEAP;

        // index in   irs[jcs[k]]...irs[jcs[k+1]-1] are the node connected to node k
        // values are W[jcs[k]]...W[jcs[k]-1]
		// recurse on each neighbor of i
		for( int k=jcs[i]; k<jcs[i+1]; ++k )
		{
			int ii = irs[k];
            double P = W[k]; // graph weight

			bool bInsert = true;
			if( callback_insert_node!=NULL )
				bInsert = callback_insert_node(ii,ii);
			if( ii>=0 && ii<n && bInsert )
			{
				// compute its neighboring values
				double a1 = D[i] + P;

				if( ((int) S[ii]) == kDead )
				{
					// check if action has change. Should not happen for Dijkstra
					if( a1<D[ii] )
					    mexWarnMsgTxt("The update is not monotone");
#if 1
					if( a1<D[ii] )	// should not happen for FM
						D[ii] = a1;
#endif
				}
				else if( ((int) S[ii]) == kOpen )
				{
					// check if action has change.
					if( a1<D[ii] )
					{
						D[ii] = a1;
						// Modify the value in the heap
						fibheap_el* cur_el = heap_pool[ii];
						if( cur_el!=NULL )
							fh_replacedata( open_heap, cur_el, cur_el->fhe_data );	// use same data for update
						else
							mexErrMsgTxt("Error in heap pool allocation."); 
					}
				}
				else if( ((int) S[ii]) == kFar )
				{
					if( D[ii]!=GW_INFINITE )
						mexErrMsgTxt("Distance must be initialized to Inf");  
					S[ii] = kOpen;
					// distance must have change.
					D[ii] = a1;
					// add to open list
					heap_pool[ii] = fh_insert( open_heap, (void*) ii );			// add to heap	
				}
				else 
					mexErrMsgTxt("Unkwnown state."); 
			}	// end switch
		}		// end for
	}			// end while


	// free heap
	fh_deleteheap(open_heap);
	// free fibheap pool
	GW_DELETEARRAY(heap_pool);
}