/*------------------------------------------------------------------------------*/
void GW_VoronoiMesh::CreateVoronoiVertex()
{
	/* Create Vornoi vertex and make the inverse map GeodesicVertex->VoronoiVertex */
	this->SetNbrVertex( (GW_U32) BaseVertexList_.size() );
	GW_U32 nNum = 0;
	for( IT_GeodesicVertexList it = BaseVertexList_.begin(); it!=BaseVertexList_.end(); ++it )
	{
		GW_GeodesicVertex* pVert = *it;
		GW_ASSERT( pVert!=NULL );
		/* create a new voronoi vertex */
		GW_VoronoiVertex* pVornoiVert = new GW_VoronoiVertex;
		pVornoiVert->SetBaseVertex( *pVert );
		this->SetVertex( nNum, pVornoiVert );
		nNum++;
		/* build the inverse map */
		GW_ASSERT( VoronoiVertexMap_.find( pVert->GetID() )==VoronoiVertexMap_.end() );
		VoronoiVertexMap_[ pVert->GetID() ] = pVornoiVert;
	}
}
GW_Bool InsersionCallback( GW_GeodesicVertex& Vert, GW_Float rNewDist )
{
	// check if the distance of the new point is less than the given distance
	GW_U32 i = Vert.GetID();
	bool doinsersion = nbr_iter<=niter_max;
	if( L!=NULL )
		doinsersion = doinsersion && (rNewDist<L[i]);
	nbr_iter++;
	return doinsersion;
}
GW_Bool StopMarchingCallback( GW_GeodesicVertex& Vert )
{
	// check if the end point has been reached
	GW_U32 i = Vert.GetID();
//	display_message("ind %d",i );
//	display_message("dist %f",Vert.GetDistance() );
	if( Vert.GetDistance()>dmax )
		return true;
	for( int k=0; k<nend; ++k )
		if( end_points[k]==i )
			return true;
	return false;
}
/*------------------------------------------------------------------------------*/
GW_Bool GW_VoronoiMesh::FastMarchingCallbackFunction_VertexInsersionRD2( GW_GeodesicVertex& CurVert, GW_Float rNewDist )
{
	GW_ASSERT( pCurWeights_!=NULL );
	GW_U32 nId = CurVert.GetID();
	if( pCurWeights_->find(nId)!=pCurWeights_->end() )	// this is one of our natural neighbor, yeah.
	{
		if( (*pCurWeights_)[nId]<0 )
		{
			/* first time we encounter this vertex, sounds good */
			if( rNewDist>0 )
				(*pCurWeights_)[nId] = 1.0/(rNewDist);
			else
				(*pCurWeights_)[nId] = GW_INFINITE;
			nNbrBaseVertex_RD_++;
		}
		else
			GW_ASSERT( GW_False );
	}
	return GW_True;
}
GW_Float WeightCallback(GW_GeodesicVertex& Vert)
{
	GW_U32 i = Vert.GetID();
	return Ww[i];
}
GW_Float HeuristicCallback( GW_GeodesicVertex& Vert )
{
	// return the heuristic distance
	GW_U32 i = Vert.GetID();
	return H[i];
}
/*------------------------------------------------------------------------------*/
void GW_VoronoiMesh::FlattenBasePoints( GW_GeodesicMesh& Mesh, T_Vector2DMap& FlatteningMap )
{
	char str[100];
	/* create a voronoi vertex for each basis vertex */
	this->CreateVoronoiVertex();

	GW_U32 n = this->GetNbrVertex();
	GW_MatrixNxP dist_matrix(n,n,-1);


	/* compute the distance matrix */
	for( GW_U32 i=0; i<n; ++i )
	{
		GW_VoronoiVertex* pVoronoiVerti = (GW_VoronoiVertex*) this->GetVertex(i);					GW_ASSERT( pVoronoiVerti!=NULL );
		GW_GeodesicVertex* pGeodesicVerti = (GW_GeodesicVertex*) pVoronoiVerti->GetBaseVertex();	GW_ASSERT( pGeodesicVerti!=NULL );
		
		sprintf( str, "Computing distance information for vertex %d.", i );
		GW_OutputComment( str );

		/* compute the distance map */
		Mesh.ResetGeodesicMesh();
		Mesh.PerformFastMarching( pGeodesicVerti );

		for( GW_U32 j=0; j<n; ++j )
		{
			GW_VoronoiVertex* pVoronoiVertj = (GW_VoronoiVertex*) this->GetVertex(j);					GW_ASSERT( pVoronoiVerti!=NULL );
			GW_GeodesicVertex* pGeodesicVertj = (GW_GeodesicVertex*) pVoronoiVertj->GetBaseVertex();	GW_ASSERT( pGeodesicVerti!=NULL );
			GW_Float rDist = pGeodesicVertj->GetDistance();
			if( dist_matrix[i][j]<0 )
				dist_matrix[i][j] = rDist*rDist;
			else
				dist_matrix[i][j] = (dist_matrix[i][j]+rDist*rDist)*0.5;

			if( dist_matrix[j][i]<0 )
				dist_matrix[j][i] = rDist*rDist;
			else
				dist_matrix[j][i] = (dist_matrix[j][i]+rDist*rDist)*0.5;
		}
	}

	/* center the matrix */
	sprintf( str, "Performing the multidimensional scaling.", i );
	GW_OutputComment( str );
	GW_MatrixNxP J(n,n,-1.0/n);
	for( GW_U32 i=0; i<n; ++i )
		J[i][i] += 1;

	/* center the matrix */
	dist_matrix = J*dist_matrix*J*(-0.5);

	/* perform eigen-decomposition */
	GW_MatrixNxP v(n,n);
	GW_VectorND RealEig(n);
	dist_matrix.Eigenvalue( v, NULL, &RealEig, NULL );

	GW_Float rScaleX = 0;
	GW_Float rScaleY = 0;
	GW_I32 nEigenvectorX = -1;
	GW_I32 nEigenvectorY = -1;
	/* find the two maximum eigenvalues */
	for( GW_U32 i=0; i<n; ++i )
	{
		if( RealEig[i]>=rScaleX )
		{
			rScaleY = rScaleX;
			nEigenvectorY = nEigenvectorX;
			rScaleX = RealEig[i];
			nEigenvectorX = i;
		}
		else if( RealEig[i]>=rScaleY )
		{
			rScaleY = RealEig[i]; 
			nEigenvectorY = i;
		}
	}
	rScaleX = sqrt(rScaleX);
	rScaleY = sqrt(rScaleY);
	GW_ASSERT( nEigenvectorX>=0 );
	GW_ASSERT( nEigenvectorY>=0 );

	/* compute the 2D position */
	FlatteningMap.clear();
	if( nEigenvectorX>=0 && nEigenvectorY>=0 )
	for( GW_U32 i=0; i<n; ++i )
	{
		GW_VoronoiVertex* pVoronoiVerti = (GW_VoronoiVertex*) this->GetVertex(i);					GW_ASSERT( pVoronoiVerti!=NULL );
		GW_GeodesicVertex* pGeodesicVerti = (GW_GeodesicVertex*) pVoronoiVerti->GetBaseVertex();	GW_ASSERT( pGeodesicVerti!=NULL );

		GW_Vector2D pos;
		pos[0] = rScaleX*v[i][nEigenvectorX];
		pos[1] = rScaleY*v[i][nEigenvectorY];
		FlatteningMap[ pGeodesicVerti->GetID() ] = pos;
	}
}