//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
#include "bbitkvtkGeodesicMeshDeformation.h"
#include "bbitkvtkPackage.h"
#include <vtkCellArrayIterator.h>
namespace bbitkvtk
{

BBTK_ADD_BLACK_BOX_TO_PACKAGE(itkvtk,GeodesicMeshDeformation)
BBTK_BLACK_BOX_IMPLEMENTATION(GeodesicMeshDeformation,bbtk::AtomicBlackBox);
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void GeodesicMeshDeformation::Process()
{
//printf("PG GeodesicMeshDeformation::Process START \n");
// THE MAIN PROCESSING METHOD BODY
//   Here we simply set the input 'In' value to the output 'Out'
//   And print out the output value
// INPUT/OUTPUT ACCESSORS ARE OF THE FORM :
//    void bbSet{Input|Output}NAME(const TYPE&)
//    const TYPE& bbGet{Input|Output}NAME() const 
//    Where :
//    * NAME is the name of the input/output
//      (the one provided in the attribute 'name' of the tag 'input')
//    * TYPE is the C++ type of the input/output
//      (the one provided in the attribute 'type' of the tag 'input')
    std::vector<double> lstCenter = bbGetInputCenter();
    double s   = bbGetInputS();
    bool    ok  = false;
    using MeshType = itk::QuadEdgeMesh<double, 3>;
    
    //Set up QuadEdge and filter every time polydata changes
    if ((bbGetInputIn() != polydata) && (bbGetInputActive()==true) && (bbGetInputIn() != NULL))
    {
    	//Reset displacement
    	if(lstCenter.size() != NULL)
    	{
			backLstCenter[0] = lstCenter[0];
			backLstCenter[1] = lstCenter[1];
			backLstCenter[2] = lstCenter[2];
    	}
    	/**
		Create QuadEdge
		*/
		using PointType = MeshType::PointType;
		//polydataBack
		polydata = bbGetInputIn();
		
		//points = points of input polydata
		vtkPoints* points=bbGetInputIn()->GetPoints();
		
		quadEdgeMesh = MeshType::New();
		double currentPoint[3] = {};
		PointType p;
		
		int numberOfPoints = points->GetNumberOfPoints();;
		for(int pointId = 0; pointId < numberOfPoints; pointId++){
			points->GetPoint(pointId, currentPoint);
			p[0] = currentPoint[0];
			p[1] = currentPoint[1];
			p[2] = currentPoint[2];
			quadEdgeMesh->SetPoint(pointId, p);
			//point data must be 1 on each point so that fast marching can calculate the distance.
			quadEdgeMesh->SetPointData(pointId, 1.);
		}

		//add cells to QuadEdge mesh
		auto cellIterator = vtk::TakeSmartPointer(bbGetInputIn()->GetPolys()->NewIterator());
		vtkIdList* cell;
		for(cellIterator->GoToFirstCell(); !cellIterator->IsDoneWithTraversal(); cellIterator->GoToNextCell()){
			cell = cellIterator->GetCurrentCell();
			quadEdgeMesh->AddFaceTriangle(cell->GetId(0), cell->GetId(1), cell->GetId(2));
		}
		
		using FastMarchingType = itk::FastMarchingQuadEdgeMeshFilterResultsBase<MeshType, MeshType>;	
		fmmFilter = FastMarchingType::New();
		fmmFilter->SetInput(quadEdgeMesh);
		
		//Filter and starting trial point removed because the range of point ids is not known when polydatas are split.
		
		//using NodePairType = FastMarchingType::NodePairType;
		//using NodePairContainerType = FastMarchingType::NodePairContainerType;

		//auto trial = NodePairContainerType::New();
		//NodePairType nodePair(0, 0.);
		//trial->push_back(nodePair);

		//fmmFilter->SetTrialPoints(trial);
		
		EdgeIdBack = bbGetInputEdgeId();

		using CriterionType = itk::FastMarchingThresholdStoppingCriterion<MeshType, MeshType>;
		auto criterion = CriterionType::New();
		sCurrent = 1;
		criterion->SetThreshold(1);
		fmmFilter->SetStoppingCriterion(criterion);
		fmmFilter->SetCollectPoints(true);
		fmmFilter->SetReleaseDataBeforeUpdateFlag(false);
		//fmmFilter->Update();

		//printf("PG GeodesicMeshDeformation::Process  Filter Created \n");
    }
    
    if (bbGetInputTypeIn()==0) // direction
    {
        if (bbGetInputDirection().size()==3)
        {
            ok = !( (bbGetInputDirection()[0]==0) && (bbGetInputDirection()[1]==0) && (bbGetInputDirection()[2]==0) );
        }
    } // if TypeIn 0
    
    if (bbGetInputTypeIn()==1)  // center
    {
        ok = ( lstCenter.size()==3 );
    } // if TypeIn 1

    if ( (bbGetInputIn()!=NULL) && (ok==true) && (bbGetInputEdgeId()>=0) && (bbGetInputActive()==true) )
    {
        vtkPoints* points=bbGetInputIn()->GetPoints();
        long    i,size=points->GetNumberOfPoints();
        double  p[3];  // point
        double  pb[3]; // point base
        double  np[3]; // new point
        double  sx,sy,sz;
        sx = s*2;
        sy = sx;
        sz = sy;

        double displcement_x = 0;
        double displcement_y = 0;
        double displcement_z = 0;
        
        if (bbGetInputTypeIn()==0) // Direction
        {
            displcement_x = bbGetInputDirection()[0];
            displcement_y = bbGetInputDirection()[1];
            displcement_z = bbGetInputDirection()[2];
        } // if TypeIn 0 Direction
		
		printf(" EED GeodesicMeshDeformation::Process   %ld   %ld  -   %f %f %f \n", EdgeIdBack, bbGetInputEdgeId() , lstCenter[0],lstCenter[1],lstCenter[2] );
		
        if (bbGetInputTypeIn()==1) // Center
        {
            if (EdgeIdBack==bbGetInputEdgeId() )
            {
                displcement_x = (lstCenter[0]-backLstCenter[0])/1.0;
                displcement_y = (lstCenter[1]-backLstCenter[1])/1.0;
                displcement_z = (lstCenter[2]-backLstCenter[2])/1.0;
            } // if EdgeIdBack!=bbGetInputEdgeId()
            backLstCenter[0] = lstCenter[0];
            backLstCenter[1] = lstCenter[1];
            backLstCenter[2] = lstCenter[2];
            
        } // if TypeIn 1 Center
        points->GetPoint( bbGetInputEdgeId() , pb );
        if (EdgeIdBack!=bbGetInputEdgeId() )
        {
            EdgeIdBack = bbGetInputEdgeId();
            
			backLstCenter[0] = lstCenter[0];
			backLstCenter[1] = lstCenter[1];
			backLstCenter[2] = lstCenter[2];
			
			using NodePairType = FastMarchingType::NodePairType;
			using NodePairContainerType = FastMarchingType::NodePairContainerType;
			
			auto processedPoints = NodePairContainerType::New();
			fmmFilter->SetProcessedPoints(processedPoints);

			auto trial = NodePairContainerType::New();

			//Seed point for fast marching with distance 0
			NodePairType nodePair(bbGetInputEdgeId(), 0.);
			trial->push_back(nodePair);
			
			fmmFilter->SetTrialPoints(trial);

			//threshold distance for fast marching method
			if(sCurrent != s)
			{			
				using CriterionType = itk::FastMarchingThresholdStoppingCriterion<MeshType, MeshType>;
				auto criterion = CriterionType::New();
				sCurrent = s;
				criterion->SetThreshold(s*2);
				fmmFilter->SetStoppingCriterion(criterion);
			}
			fmmFilter->Update();
			
        } // if EdgeIdBack
        
        if ( !((displcement_x==0) &&(displcement_y==0) && (displcement_z==0)) )
        {	
			if(fmmFilter != NULL)
			{
				MeshType::PointDataContainer::Pointer pointData = fmmFilter->GetOutput()->GetPointData();
				double pPD[3];
				
				FastMarchingType::NodePairContainerType::Iterator BegProcessedIt = fmmFilter->GetProcessedPoints()->Begin();
				FastMarchingType::NodePairContainerType::Iterator EndProcessedIt = fmmFilter->GetProcessedPoints()->End();
				
				while (BegProcessedIt != EndProcessedIt)
				{	
					//point modification
					points->GetPoint(BegProcessedIt.Value().GetNode(), pPD);
					double weight = exp(-BegProcessedIt.Value().GetValue() * 1. / s);
					if(weight > 0.0001){
						np[0] = pPD[0]+ weight*displcement_x;
						np[1] = pPD[1]+ weight*displcement_y;
						np[2] = pPD[2]+ weight*displcement_z;
						points->SetPoint(BegProcessedIt.Value().GetNode(), np);
					}
					++BegProcessedIt;
				}
				points->Modified();
            	bbGetInputIn()->Modified();
			}// if ffmFilter != NULL
        } // if distance_x y z  != 0
    } // In != NULL    ok    active
//    printf("PG GeodesicMeshDeformation::Process END \n");
}
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void GeodesicMeshDeformation::bbUserSetDefaultValues()
{

//  SET HERE THE DEFAULT INPUT/OUTPUT VALUES OF YOUR BOX 
//    Here we initialize the input 'In' to 0
   	bbSetInputActive(true);
	bbSetInputTypeIn(0);
	bbSetInputIn(NULL);
	std::vector<double> direction;
	direction.push_back(1);
	direction.push_back(0);
	direction.push_back(0);
	bbSetInputDirection(direction);
	EdgeIdBack=-1;
	bbSetInputEdgeId(EdgeIdBack);
	bbSetInputS(10);

	backLstCenter.push_back(0);
	backLstCenter.push_back(0);
	backLstCenter.push_back(0);
  	polydata = NULL;
  	quadEdgeMesh = NULL;
  	sCurrent = 0;
  	fmmFilter = NULL;
  	firstTime = true;
}
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void GeodesicMeshDeformation::bbUserInitializeProcessing()
{

//  THE INITIALIZATION METHOD BODY :
//    Here does nothing 
//    but this is where you should allocate the internal/output pointers 
//    if any 

  
}
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void GeodesicMeshDeformation::bbUserFinalizeProcessing()
{

//  THE FINALIZATION METHOD BODY :
//    Here does nothing 
//    but this is where you should desallocate the internal/output pointers 
//    if any
  
}
}
// EO namespace bbitkvtk