/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit (ITK)
  Module:    $RCSfile: itkManifoldIntegrationAlgorithm.cxx,v $
  Language:  C++
  Date:      $Date: 2006/10/13 19:21:27 $
  Version:   $Revision: 1.3 $

=========================================================================*/
#ifndef _itkManifoldIntegrationAlgorithm_cxx_
#define _itkManifoldIntegrationAlgorithm_cxx_

#include "itkSurfaceMeshCurvature.h"
#include "vtkFeatureEdges.h"
#include "vtkPointLocator.h"
#include "vtkCellLocator.h"
#include "vtkTriangleFilter.h"
#include "vtkCleanPolyData.h"
#include "vtkPolyDataConnectivityFilter.h"

namespace itk {


template<class TGraphSearchNode >
ManifoldIntegrationAlgorithm<TGraphSearchNode>::ManifoldIntegrationAlgorithm()
{
  m_SurfaceMesh=NULL;
  m_QS = DijkstrasAlgorithmQueue<TGraphSearchNode>::New();
  m_MaxCost=vnl_huge_val(m_MaxCost);
  m_PureDist=false;
  //  m_LabelCost=0;
  m_ParamWhileSearching=false;
  this->m_DistanceCostWeight=1;
  this->m_LabelCostWeight=0;
}


template<class TGraphSearchNode >
float ManifoldIntegrationAlgorithm<TGraphSearchNode>::dstarUestimate(
	typename TGraphSearchNode::Pointer G)
{
  typedef itk::SurfaceMeshCurvature<TGraphSearchNode,TGraphSearchNode> surfktype;
  typename surfktype::Pointer surfk=surfktype::New();
  surfk->SetSurfacePatch(G);
  surfk->FindNeighborhood();
  float dsu=(float) surfk->dstarUestimate();
  return dsu;
}


template<class TGraphSearchNode >
void ManifoldIntegrationAlgorithm<TGraphSearchNode>::InitializeGraph3()
{
  if (!m_SurfaceMesh) return;

  // Construct simple triangles
  vtkTriangleFilter* fltTriangle = vtkTriangleFilter::New();
  fltTriangle->SetInput(m_SurfaceMesh);

  cout << "   converting mesh to triangles " << endl;
  fltTriangle->Update();

  /* Clean the data
  vtkCleanPolyData* fltCleaner = vtkCleanPolyData::New();
  fltCleaner->SetInput(fltTriangle->GetOutput());
  fltCleaner->SetTolerance(0);
  fltCleaner->ConvertPolysToLinesOn();

  cout << "   cleaning up triangle mesh " << endl;
  fltCleaner->Update();

  // Go through and delete the cells that are of the wrong type
  //m_SurfaceMesh
  vtkPolyData* clean= fltCleaner->GetOutput();
  for(vtkIdType i = clean->GetNumberOfCells();i > 0;i--)
    {
    if(clean->GetCellType(i-1) != VTK_TRIANGLE)
      clean->DeleteCell(i-1);
    }
  clean->BuildCells();
  m_SurfaceMesh=clean;*/
  m_SurfaceMesh=fltTriangle->GetOutput();

  typedef float labelType;
  typedef std::vector<labelType>                  LabelSetType;
  LabelSetType myLabelSet;


  vtkPoints* vtkpoints = m_SurfaceMesh->GetPoints();
  vtkPointData *pd=m_SurfaceMesh->GetPointData();
  int numPoints = vtkpoints->GetNumberOfPoints();
  vtkDataArray* scs=pd->GetScalars();
  m_GraphX.resize(numPoints);
  for(int i =0; i < numPoints; i++)
  {
    NodeLocationType loc;
    double* pt = vtkpoints->GetPoint(i);
    typename GraphSearchNode<PixelType,CoordRep,GraphDimension>::Pointer G=
      GraphSearchNode<PixelType,CoordRep,GraphDimension>::New();
    G->SetUnVisited();
    G->SetTotalCost(m_MaxCost);
    G->SetValue(scs->GetTuple1(i),3); /** here we put the label value */

    labelType label = scs->GetTuple1(i);
    if( find( myLabelSet.begin(), myLabelSet.end(), label )
	== myLabelSet.end() )
      myLabelSet.push_back( label );

    //    std::cout << " label " << 	 scs->GetTuple1(i) << std::endl;
    	 // std::cout << " set3 " << 	 scs->GetTuple3(i) << std::endl;
    	 // std::cout << " set4 " << 	 scs->GetTuple4(i) << std::endl;
    for (int j=0; j<GraphDimension; j++) loc[j]=pt[j];
	 G->SetLocation(loc);
  	 G->SetPredecessor(NULL);
	 G->m_NumberOfNeighbors=0;
	 G->SetIdentity(i);
	 m_GraphX[i]=G;
  }
  std::cout << " you have " << myLabelSet.size() << " labels " << std::endl;
  for ( unsigned int i=0; i < myLabelSet.size(); i++) 
    std::cout <<" label " << myLabelSet[i] << std::endl;
  std::cout << " allocation of graph done ";

// now loop through the cells to get triangles and also edges

  vtkCellArray* vtkcells = m_SurfaceMesh->GetPolys();

  vtkIdType npts;
  vtkIdType* pts;

  /* count possible neighbors ... */
  for(vtkcells->InitTraversal(); vtkcells->GetNextCell(npts, pts); )
  {
    m_GraphX[pts[0]]->m_NumberOfNeighbors+=2;
    m_GraphX[pts[1]]->m_NumberOfNeighbors+=2;
    m_GraphX[pts[2]]->m_NumberOfNeighbors+=2;
  }
 

  for(int i =0; i < numPoints; i++)
  {
    m_GraphX[i]->m_Neighbors.resize(m_GraphX[i]->m_NumberOfNeighbors);
//	std::cout <<" Num Neigh " << i << " is " << m_GraphX[i]->m_NumberOfNeighbors << std::endl;
	m_GraphX[i]->m_NumberOfNeighbors=0;
  }

  for(vtkcells->InitTraversal(); vtkcells->GetNextCell(npts, pts); )
  {
  m_GraphX[pts[0]]->m_Neighbors[ m_GraphX[pts[0]]->m_NumberOfNeighbors ]=m_GraphX[pts[1]];
  m_GraphX[pts[0]]->m_NumberOfNeighbors++;
  m_GraphX[pts[0]]->m_Neighbors[ m_GraphX[pts[0]]->m_NumberOfNeighbors ]=m_GraphX[pts[2]];
  m_GraphX[pts[0]]->m_NumberOfNeighbors++;

  m_GraphX[pts[1]]->m_Neighbors[ m_GraphX[pts[1]]->m_NumberOfNeighbors ]=m_GraphX[pts[0]];
  m_GraphX[pts[1]]->m_NumberOfNeighbors++;
  m_GraphX[pts[1]]->m_Neighbors[ m_GraphX[pts[1]]->m_NumberOfNeighbors ]=m_GraphX[pts[2]];
  m_GraphX[pts[1]]->m_NumberOfNeighbors++;

  m_GraphX[pts[2]]->m_Neighbors[ m_GraphX[pts[2]]->m_NumberOfNeighbors ]=m_GraphX[pts[0]];
  m_GraphX[pts[2]]->m_NumberOfNeighbors++;
  m_GraphX[pts[2]]->m_Neighbors[ m_GraphX[pts[2]]->m_NumberOfNeighbors ]=m_GraphX[pts[1]];
  m_GraphX[pts[2]]->m_NumberOfNeighbors++;
  }

  // now go through each node and make its list of neighbors unique 
  // had to do this b/c it's easier than fixing the junk above ... too tired!
  for(int i =0; i < numPoints; i++)
    {
      std::vector<unsigned int> neighlist;
      for ( unsigned int n=0; n<m_GraphX[i]->m_NumberOfNeighbors; n++) {
	neighlist.push_back(m_GraphX[i]->m_Neighbors[n]->GetIdentity()); 
      }
      std::sort( neighlist.begin(), neighlist.end() );
      std::vector<unsigned int>::iterator new_end_pos;
      new_end_pos = std::unique( neighlist.begin(),neighlist.end() );
      neighlist.erase( new_end_pos, neighlist.end() );
      //      std::cout << " new leng " << neighlist.size() << " old " << len1 << std::endl;
    }
 
}


template<class TGraphSearchNode >
void ManifoldIntegrationAlgorithm<TGraphSearchNode>::InitializeGraph2()
{
  if (!m_SurfaceMesh) return;
/*
  // Construct simple triangles
  vtkTriangleFilter* fltTriangle = vtkTriangleFilter::New();
  fltTriangle->SetInput(m_SurfaceMesh);

  cout << "   converting mesh to triangles " << endl;
  fltTriangle->Update();

  cout << "  this mesh has " << fltTriangle->GetOutput()->GetNumberOfPoints() << " points" << endl;
  cout << "  this mesh has " << fltTriangle->GetOutput()->GetNumberOfCells() << " cells" << endl;

  // Clean the data
  vtkCleanPolyData* fltCleaner = vtkCleanPolyData::New();
  fltCleaner->SetInput(fltTriangle->GetOutput());
  fltCleaner->SetTolerance(0);
  fltCleaner->ConvertPolysToLinesOn();

  cout << "   cleaning up triangle mesh " << endl;
  fltCleaner->Update();

  // Go through and delete the cells that are of the wrong type
  //m_SurfaceMesh
  vtkPolyData* clean= fltCleaner->GetOutput();
  for(vtkIdType i = clean->GetNumberOfCells();i > 0;i--)
    {
    if(clean->GetCellType(i-1) != VTK_TRIANGLE)
      clean->DeleteCell(i-1);
    }
  clean->BuildCells();
*/

  vtkFeatureEdges* fltEdge = vtkFeatureEdges::New();
  fltEdge->BoundaryEdgesOff();
  fltEdge->FeatureEdgesOff();
  fltEdge->NonManifoldEdgesOff();
  fltEdge->ManifoldEdgesOn();
  fltEdge->ColoringOff();
  fltEdge->SetInput(m_SurfaceMesh);


  cout << "   extracting edges from the mesh" << endl;
  fltEdge->Update();

  // Got the new poly data
  vtkPolyData* m_EdgePolys = fltEdge->GetOutput();
  m_EdgePolys->BuildCells();
  m_EdgePolys->BuildLinks();

  unsigned int nEdges = m_EdgePolys->GetNumberOfLines();
  cout << "      number of edges (lines) : " << nEdges << endl;
  cout << "      number of cells : " << m_EdgePolys->GetNumberOfCells() << endl;
  cout << "      number if points : " << m_EdgePolys->GetNumberOfPoints() << endl;

  vtkPoints* vtkpoints = m_EdgePolys->GetPoints();
  int numPoints = vtkpoints->GetNumberOfPoints();
  m_GraphX.resize(numPoints);
  for(int i =0; i < numPoints; i++)
  {
     NodeLocationType loc;
     double* pt = vtkpoints->GetPoint(i);
  	 typename GraphSearchNode<PixelType,CoordRep,GraphDimension>::Pointer G=
    		GraphSearchNode<PixelType,CoordRep,GraphDimension>::New();
  	 G->SetUnVisited();
     G->SetTotalCost(m_MaxCost);
     for (int j=0; j<GraphDimension; j++) loc[j]=pt[j];
	 G->SetLocation(loc);
  	 G->SetPredecessor(NULL);
	 G->m_NumberOfNeighbors=0;
	 m_GraphX[i]=G;
  }

  std::cout << " allocation of graph done ";

  vtkIdType nPoints = 0; vtkIdType *xPoints = NULL;
  for(unsigned int i=0;i<nEdges;i++)
    {
    // Get the next edge
    m_EdgePolys->GetCellPoints(i, nPoints, xPoints);

    // Place the edge into the Edge structure
    assert(nPoints == 2);
    // Place the edge into the Edge structure
//	std::cout << " nPoints " << nPoints << std::endl;
//	std::cout << " pt " << xPoints[0] << " connects " << xPoints[1] << std::endl;
    assert(nPoints == 2);
	m_GraphX[xPoints[0]]->m_NumberOfNeighbors++;
	}

	std::cout << " counting nhood done ";
	// second, resize the vector for each G
	for (int i=0; i<numPoints; i++)
	{
      m_GraphX[i]->m_Neighbors.resize(m_GraphX[i]->m_NumberOfNeighbors);
	  m_GraphX[i]->m_NumberOfNeighbors=0;
	}

    for(unsigned int i=0;i<nEdges;i++)
    {
    // Get the next edge
    m_EdgePolys->GetCellPoints(i, nPoints, xPoints);
    // Place the edge into the Edge structure
    assert(nPoints == 2);
	m_GraphX[xPoints[0]]->m_Neighbors[ m_GraphX[xPoints[0]]->m_NumberOfNeighbors ]=m_GraphX[xPoints[1]];
	m_GraphX[xPoints[0]]->m_NumberOfNeighbors++;
	}

//	vtkPolyDataConnectivityFilter* con = vtkPolyDataConnectivityFilter::New();
//    con->SetExtractionModeToLargestRegion();
//    con->SetInput(m_EdgePolys);
//	m_SurfaceMesh=con->GetOutput();
    m_SurfaceMesh=m_EdgePolys;

}


template<class TGraphSearchNode >
void ManifoldIntegrationAlgorithm<TGraphSearchNode>::InitializeGraph()
{
if (!m_SurfaceMesh) return;
	std::cout << " Generate graph from surface mesh " << std::endl;
// get size of the surface mesh

    vtkExtractEdges* edgeex=vtkExtractEdges::New();
    edgeex->SetInput(m_SurfaceMesh);
    edgeex->Update();
    vtkPolyData* edg1=edgeex->GetOutput();
    vtkIdType nedg=edg1->GetNumberOfCells();
    vtkIdType vers = m_SurfaceMesh->GetNumberOfPoints();
    int nfac = m_SurfaceMesh->GetNumberOfPolys();
    float g = 0.5 * (2.0 - vers + nedg - nfac);
    std::cout << " Genus " << g << std::endl;
    edg1->BuildCells();


	// now cruise through all edges and add to each node's neighbor list
// first, count the num of edges for each node
//    m_SurfaceMesh=edg1;

    vtkPoints* vtkpoints = edg1->GetPoints();
    int numPoints = vtkpoints->GetNumberOfPoints();
    m_GraphX.resize(numPoints);
    for(int i =0; i < numPoints; i++)
      {
	NodeLocationType loc;
	double* pt = vtkpoints->GetPoint(i);
	typename GraphSearchNode<PixelType,CoordRep,GraphDimension>::Pointer G=
	  GraphSearchNode<PixelType,CoordRep,GraphDimension>::New();
	G->SetUnVisited();
	G->SetTotalCost(m_MaxCost);
	for (int j=0; j<GraphDimension; j++) loc[j]=pt[j];
	G->SetLocation(loc);
  	 G->SetPredecessor(NULL);
	 G->m_NumberOfNeighbors=0;
	 m_GraphX[i]=G;
      }
  std::cout << " allocation of graph done ";
  
  std::cout << " begin edg iter ";
  vtkIdType nPoints = 0;
  vtkIdType *xPoints = NULL;
  for(unsigned int i=0;i<nedg;i++)
    {
      // Get the next edge
      edg1->GetCellPoints(i, nPoints, xPoints);
      // Place the edge into the Edge structure
//	std::cout << " nPoints " << nPoints << std::endl;
//	std::cout << " pt " << xPoints[0] << " connects " << xPoints[1] << std::endl;
      assert(nPoints == 2);
      m_GraphX[xPoints[0]]->m_NumberOfNeighbors++;
    }
  
  std::cout << " counting nhood done ";
	// second, resize the vector for each G
  for (int i=0; i<numPoints; i++)
    {
      m_GraphX[i]->m_Neighbors.resize(m_GraphX[i]->m_NumberOfNeighbors);
      m_GraphX[i]->m_NumberOfNeighbors=0;
    }

  for(unsigned int i=0;i<nedg;i++)
    {
      // Get the next edge
    edg1->GetCellPoints(i, nPoints, xPoints);
    // Place the edge into the Edge structure
    assert(nPoints == 2);
    m_GraphX[xPoints[0]]->m_Neighbors[ m_GraphX[xPoints[0]]->m_NumberOfNeighbors ]=m_GraphX[xPoints[1]];
    m_GraphX[xPoints[0]]->m_NumberOfNeighbors++;
    }
  
  m_SurfaceMesh=edg1;
  
  return;
  
}
  
  
template<class TGraphSearchNode >
void ManifoldIntegrationAlgorithm<TGraphSearchNode>
::ConvertGraphBackToMesh()
{// this is a sanity check

}



template<class TGraphSearchNode >
void ManifoldIntegrationAlgorithm<TGraphSearchNode>::InitializeQueue()
{
  int n = m_QS->m_SourceNodes.size();
//  GraphIteratorType GraphIterator( m_Graph, m_GraphRegion );
//  GraphIterator.GoToBegin();
//  m_GraphIndex = GraphIterator.GetIndex();
  NodeLocationType loc;
  // make sure the graph contains the right pointers
  for (int i=0; i<n; i++)
  {
    typename GraphSearchNode<PixelType,CoordRep,GraphDimension>::Pointer G = m_QS->m_SourceNodes[i];
	  G->SetPredecessor(G);
  	m_QS->m_Q.push(G);
  	loc=G->GetLocation();
//  	for (int d=0;d<GraphDimension;d++) m_GraphIndex[d]=loc[d];
//	    m_Graph->SetPixel(m_GraphIndex,G);
  }
  for (unsigned int i=0; i<m_QS->m_SinkNodes.size(); i++)
  {
    typename GraphSearchNode<PixelType,CoordRep,GraphDimension>::Pointer G = m_QS->m_SinkNodes[i];
	  G->SetPredecessor(NULL);
	  loc=G->GetLocation();
//	  for (int d=0;d<GraphDimension;d++) m_GraphIndex[d]=(long)loc[d];
//	    m_Graph->SetPixel(m_GraphIndex,G);
  }
  m_SearchFinished=false;
}

/**
*  parameterize the boundary --- an estimate 
*/

template<class TGraphSearchNode >
bool ManifoldIntegrationAlgorithm<TGraphSearchNode>
::ParameterizeBoundary( ManifoldIntegrationAlgorithm<TGraphSearchNode>::SearchNodePointer rootNode )
{
  std::vector<SearchNodePointer>  neighborlist; 
  bool I_Am_A_Neighbor=false;
  SearchNodePointer neighbor=NULL;
  SearchNodePointer curNode=rootNode;
  //  unsigned int rootnn=rootNode>m_NumberOfNeighbors; 
  unsigned int ct=0;
  bool canparam=false;
  unsigned int qsz= this->m_QS->m_Q.size();
  while ( !  I_Am_A_Neighbor && ct <= qsz*3  ) {
    unsigned int limit = curNode->m_NumberOfNeighbors;
   for (unsigned int i = 0; i < limit; i++)
     {
	neighbor=curNode->m_Neighbors[i];
	bool inb=false;
	for ( unsigned int q=0; q<neighborlist.size(); q++) 
	  {
	    if ( neighbor == neighborlist[q] ) inb=true;
	  }
	if ( neighbor == rootNode && !inb && ct > 2  ) {
	    I_Am_A_Neighbor=true; 
	    canparam=true;
	}
	if (  neighbor->IsInQueue() && !inb ) { // add to border list
	  neighborlist.push_back(neighbor);
	  curNode=neighbor;
	  i=limit;
	} // add to border
     } // neighborhood     
   ct++;
 }// while
 
 if ( neighborlist.size() >= this->m_BoundaryList.size() && canparam ) {
   neighborlist.push_back(rootNode);
   this->m_BoundaryList.clear();
   this->m_BoundaryList.assign(neighborlist.begin(),neighborlist.end());
}
 
 // if ( ct > 0 && canparam)  std::cout <<" qfrac " << this->m_BoundaryList.size()  << " canp  "<< canparam << " qsz " << qsz << " cost " << m_CurrentCost << std::endl;
 return canparam;
  
}  
 



/**
*  Compute the local cost using Manhattan distance.
*/
template<class TGraphSearchNode >
typename ManifoldIntegrationAlgorithm<TGraphSearchNode>::
PixelType ManifoldIntegrationAlgorithm<TGraphSearchNode>::MyLocalCost()
{
  NodeLocationType  dif=m_CurrentNode->GetLocation()-m_NeighborNode->GetLocation();
  float mag = 0.0;
  for (int jj=0; jj<GraphDimension;jj++) mag+=dif[jj]*dif[jj];
  mag=sqrt(mag);
  if (m_PureDist)
    {
      return mag;
    }
  else
    {
      float dL=fabs(m_CurrentNode->GetValue(3)-m_NeighborNode->GetValue(3));
      if ( dL > 0.5 ) dL=this->m_MaxCost*this->m_LabelCostWeight; else dL=0;
      return (mag*this->m_DistanceCostWeight+dL);
    }
      
}

template<class TGraphSearchNode >
bool ManifoldIntegrationAlgorithm<TGraphSearchNode>::TerminationCondition()
{
  if (!m_QS->m_SinkNodes.empty())
  {
    if (m_NeighborNode == m_QS->m_SinkNodes[0] && !m_SearchFinished  )
    {
      //      std::cout << " FOUND SINK ";
      m_SearchFinished=true;
	  m_NeighborNode->SetTotalCost( m_CurrentCost + MyLocalCost());
	  m_NeighborNode->SetPredecessor(m_CurrentNode);
  	}
  }
  if (m_CurrentCost>=m_MaxCost) m_SearchFinished=true;
  return m_SearchFinished;
}


template<class TGraphSearchNode >
void ManifoldIntegrationAlgorithm<TGraphSearchNode>::SearchEdgeSet()
{
  int i=0;//,j=0;
  for (i = 0; i < m_CurrentNode->m_NumberOfNeighbors; i++)
    {
      m_NeighborNode=m_CurrentNode->m_Neighbors[i];
      //    	std::cout << " i " << i << " position " << m_NeighborNode->GetLocation() << endl;
      //    	std::cout << " i " << i << " position " << m_NeighborNode->GetLocation() << " label " << m_CurrentNode->GetValue() << endl;
      TerminationCondition();
      if (!m_SearchFinished && m_CurrentNode != m_NeighborNode &&
	  !m_NeighborNode->GetDelivered())
	{
	  m_NewCost = m_CurrentCost + MyLocalCost();
	  CheckNodeStatus();
	}
  }
}

template<class TGraphSearchNode >
void ManifoldIntegrationAlgorithm<TGraphSearchNode>::GetSearchBoundary()
{
  unsigned int gsz=this->GetGraphSize();
  for ( unsigned int j=0; j<gsz ; j++) { 
    float cost=0;
    this->m_CurrentNode=this->m_GraphX[j];
    if ( this->m_CurrentNode ) {
      cost=m_CurrentNode->GetTotalCost();
      if (  cost <= this->m_MaxCost &&  (cost >= this->m_MaxCost-4) ) {
	this->m_BoundaryList.push_back(this->m_CurrentNode);
      }
    }
  }

}


template<class TGraphSearchNode >
void ManifoldIntegrationAlgorithm<TGraphSearchNode>::CheckNodeStatus()
// checks a graph neighbor's status
{

	NodeLocationType  dif=m_CurrentNode->GetLocation()-m_NeighborNode->GetLocation();
//	std::cout << " visited? " << m_NeighborNode->GetVisited() <<
//		" old cost " << m_NeighborNode->GetTotalCost() << " new cost " <<m_NewCost << std::endl;
  if (!m_NeighborNode->GetVisited() && ! m_NeighborNode->GetUnVisitable() )
  {
    // set the cost and put into the queue
    m_NeighborNode->SetTotalCost(m_NewCost);
    float delt=fabs(m_CurrentNode->GetValue()-m_NeighborNode->GetValue());//*dif.magnitude();
    m_NeighborNode->SetValue(m_CurrentNode->GetValue()+delt);
    m_NeighborNode->SetPredecessor(m_CurrentNode);
    m_NeighborNode->SetVisited();


    float mag = 0.0;
    for (int jj=0; jj<GraphDimension;jj++) mag+=dif[jj]*dif[jj];
    mag = sqrt(mag);
    m_NeighborNode->SetValue(m_CurrentNode->GetValue(2)+mag,2); // the actual manifold distance travelled
    //    if  ( 
      m_QS->m_Q.push(m_NeighborNode);
      // }
      // else {
      //  m_NeighborNode->SetUnVisitable();
      // }
    
    //	std::cout << " Pushing new node on " << m_NewCost << std::endl;
  }
  else if (m_NewCost < m_NeighborNode->GetTotalCost()&& ! m_NeighborNode->GetUnVisitable()  )
    {
      //	  std::cout << " Updating " << std::endl;
      float delt=fabs(m_CurrentNode->GetValue()-m_NeighborNode->GetValue());//*dif.magnitude();
      m_NeighborNode->SetValue(m_CurrentNode->GetValue()+delt);
      m_NeighborNode->SetTotalCost(m_NewCost);
      m_NeighborNode->SetPredecessor(m_CurrentNode);

      
      float mag = 0.0;
      for (int jj=0; jj<GraphDimension;jj++) mag+=dif[jj]*dif[jj];
      mag = sqrt(mag);
      m_NeighborNode->SetValue(m_CurrentNode->GetValue(2)+mag,2); // the actual manifold distance travelled
      m_QS->m_Q.push(m_NeighborNode);
    }
  
}


template<class TGraphSearchNode >
void ManifoldIntegrationAlgorithm<TGraphSearchNode>::FindPath()
{
  if (m_QS->m_SourceNodes.empty())
    {
      std::cout << "ERROR !! DID NOT SET SOURCE!!\n";
      return;
    }

  //  std::cout << "MI start find path " << " Q size " << m_QS->m_Q.size() << " \n";
  
  while ( !m_SearchFinished && !m_QS->m_Q.empty()  )
    {
      m_CurrentNode=m_QS->m_Q.top();
      m_CurrentCost=m_CurrentNode->GetTotalCost();
         if ( this->m_ParamWhileSearching  ) 
             this->ParameterizeBoundary( this->m_CurrentNode );  
      m_QS->m_Q.pop();
      if (!m_CurrentNode->GetDelivered()) {
	m_QS->IncrementTimer();
	///std::cout << " searching " << m_CurrentNode->GetLocation()   << " \n";
	this->SearchEdgeSet();
	  //if ( (m_CurrentNode->GetTimer() % 1.e5 ) == 0)
	// std::cout << " searched  " << m_CurrentNode->GetTimer()   << " \n";
      }
      m_CurrentNode->SetDelivered();
      
    }  // end of while
  
  m_NumberSearched = (unsigned long) m_QS->GetTimer();
  //  std::cout << "Done with find path " << " Q size " << m_QS->m_Q.size() <<
  //" num searched " << m_NumberSearched << " \n";
  
  //  std::cout << " Max Distance " << m_CurrentCost << std::endl;
  if ( ! this->m_ParamWhileSearching )
    this->GetSearchBoundary(); 
  
  return;

}




}
#endif