#ifndef _PHYS_LEVEL_SET_CXX
#define _PHYS_LEVEL_SET_CXX

#include "Phys_LevelSet.h"

namespace mial
{

	// TODO: remove #defines when no longer necessary
#define DEFAULT_PROPAGATION_SCALING 0.5;
#define DEFAULT_CURVATURE_SCALING 1.0;
#define DEFAULT_ADVECTION_SCALING 3.0;
#define DEFAULT_MAX_RMS_ERROR 0.02;
#define DEFAULT_NUM_ITERATIONS 10;

	// default constructor:
	template< class DataType, class InputImageType, int nDims, class MType, class VType >
	Phys_LevelSet<DataType, InputImageType, nDims, MType, VType>
		::Phys_LevelSet()		
		: Physics<DataType, nDims, MType, VType>() 
	{
		inputImg =				NULL;
		distanceImg =			NULL;
		edgePotentialImg =		NULL;
		propagationScaling =	DEFAULT_PROPAGATION_SCALING;
		curvatureScaling =		DEFAULT_CURVATURE_SCALING;
		advectionScaling =		DEFAULT_ADVECTION_SCALING;
		maximumRMSError =		DEFAULT_MAX_RMS_ERROR;
		numIterations =			DEFAULT_NUM_ITERATIONS;
	}

	//----------------------------------------------------
	// Generate an distanceImg from a Geometric object
	// (e.g. the 'geom' member of a Phys_Euler).
	// (Should only be called after setInput() has been called):
	// TODO: check if setInput() precondition can be relaxed (only need inputImg's size...)
	template< class DataType, class InputImageType, int nDims, class MType, class VType >
	void
		Phys_LevelSet<DataType,InputImageType,nDims,MType,VType>
		::initializeDistanceImg()
	{
		if(inputImg.IsNotNull())
		{
			std::cout << "Phys_LevelSet: Starting initializeDistanceImg()..." << std::endl;
			//// the filter type for generating a signed distance 
			//// function image from the geometry layer nodes:
			//typedef  itk::FastMarchingImageFilter< 
			//	InternalImageType, 
			//	InternalImageType >  FastMarchingFilterType;

			//// construct the fast marching filter:
			//FastMarchingFilterType::Pointer  fastMarchingFilter = FastMarchingFilterType::New();

			//// typedefs for fast marching filter seed points:
			//typedef FastMarchingFilterType::NodeContainer  NodeContainer;
			//typedef FastMarchingFilterType::NodeType       NodeType;

			//NodeContainer::Pointer seeds = NodeContainer::New(); // will hold the seeds used by the fastMarching filter
			//seeds->Initialize();

			//------------------------------
			// extract the seed points from the geometry layer:
			//unsigned int numNodes = geom->getNumNodes();
			//const double INITIAL_DISTANCE = 5.0; 

			//InternalImageType::IndexType* seedPositions = new InternalImageType::IndexType[ numNodes ];
			//NodeType* nodes = new NodeType[ numNodes ];

			//MType geomNodes = geom->getMatrixNodePositions();

			//// move values from geom to NodeContainer:
			//for( unsigned int i=0; i<numNodes; i++ )
			//{
			//	for( unsigned int j=0; j<nDims; j++ )
			//	{
			//		seedPositions[i][j] = geomNodes(i,j);
			//	}
			//	nodes[i].SetValue( INITIAL_DISTANCE );
			//	nodes[i].SetIndex( seedPositions[i] );
			//	seeds->InsertElement( i, nodes[i] );
			//}

			// DEBUG: TEMPORARILY USE LESS SEED POINTS------------------------

			//InternalImageType::IndexType seedPosition;
			//NodeType node;
			//for( unsigned int j=0; j<nDims; j++ )
			//{
			//	seedPosition[j] = 50.0;
			//}
			//node.SetValue( INITIAL_DISTANCE );
			//node.SetIndex( seedPosition );
			//seeds->InsertElement( 0, node );
			// END DEBUG -----------------------------------------------------

			// DEBUG: GET BINARY IMAGE FROM MESH ------------------------
			typedef Geom_MeshSpatialObject<DataType, nDims>						GeomMeshSpatialObjectType;
			typedef typename GeomMeshSpatialObjectType::MeshSpatialObjectType			MeshSpatialObjectType;

					
			typename BinaryImageType::SizeType sz = inputImg->GetLargestPossibleRegion().GetSize();
			typename BinaryImageType::Pointer binImg = static_cast< GeomMeshSpatialObjectType* > (this->geom.GetPointer())->generateBinaryImageFromTopology( sz);

			//Assert that the phys level set is up to date.
			this->geom->didTopologyChange();
			this->geom->didNodesChange();


			typedef itk::SignedDanielssonDistanceMapImageFilter<
				BinaryImageType,
				InternalImageType > DistanceMapFilterType;
			typename DistanceMapFilterType::Pointer distanceMapFilter = DistanceMapFilterType::New();

			distanceMapFilter->SetInput(binImg );
			distanceMapFilter->Update();
			distanceImg = distanceMapFilter->GetOutput();
			// END DEBUG -----------------------------------------------------

			//// set fast marching filter seed points:
			//fastMarchingFilter->SetTrialPoints(  seeds  );
			//// since we're just generating a distance map, use a constant speed function of 1:
			//fastMarchingFilter->SetSpeedConstant( 1.0 );

			//fastMarchingFilter->SetOutputSize( inputImg->GetBufferedRegion().GetSize() );

			//fastMarchingFilter->Update();	// run the filter
			//distanceImg = fastMarchingFilter->GetOutput();

			//delete [] nodes;
			//delete [] seedPositions;

			//// DEBUG -------------------------------------------------------------
			if(DEBUG)
			{
				std::cout << "Phys_LevelSet: distance image initialized." << std::endl; 
				typedef itk::ImageFileWriter< InternalImageType >		DistanceWriterType;
				typename DistanceWriterType::Pointer distanceImageWriter = DistanceWriterType::New();
				std::string distanceImageFileName = "C:\\distanceImage.mhd";
				distanceImageWriter->SetFileName( distanceImageFileName.c_str() );
				distanceImageWriter->SetInput( distanceImg );
				distanceImageWriter->Update();
				std::cout << "Phys_LevelSet: distance image written to '" 
					<< distanceImageFileName << "'." << std::endl;

				typedef itk::ImageFileWriter< BinaryImageType >		BinaryWriterType;
				typename BinaryWriterType::Pointer binaryImageWriter = BinaryWriterType::New();
				std::string binaryImageFileName = "C:\\binaryImageGeneratedFromMesh.mhd";
				binaryImageWriter->SetFileName( binaryImageFileName.c_str() );
				binaryImageWriter->SetInput( binImg );
				binaryImageWriter->Update();
				std::cout << "Phys_LevelSet: generated binary image written to '" 
					<< binaryImageFileName << "'." << std::endl;
			}
			// END DEBUG ----------------------------------------------------------
		} // end if(inputImg != NULL)
	} // end initializeDistanceImg(Geometric *geom)


	//----------------------------------------------------
	// Generate edge potential image based on the current inputImg
	template< class DataType, class InputImageType, int nDims, class MType, class VType >
	void
		Phys_LevelSet<DataType,InputImageType,nDims,MType,VType>
		::initializeEdgePotentialImg()
	{
		if(inputImg.IsNotNull())
		{
			// TODO: Add CurvatureAnisotropicDiffusionImageFilter here?

			typedef itk::GradientMagnitudeRecursiveGaussianImageFilter<
				InputImageType, 
				InternalImageType >  GradientFilterType;

			typedef itk::SigmoidImageFilter<                               
				InternalImageType, 
				InternalImageType >  SigmoidFilterType;

			// TODO: eliminate hardcoding of gradientMagnitudeFilter sigma
			typename GradientFilterType::Pointer  gradientMagnitudeFilter = GradientFilterType::New();
			const double sigma = 1.0;
			gradientMagnitudeFilter->SetSigma( sigma );

			// TODO: eliminate hardcoding of sigmoidFilter parameters
			typename SigmoidFilterType::Pointer sigmoidFilter = SigmoidFilterType::New();
			sigmoidFilter->SetOutputMinimum( 0.0 );
			sigmoidFilter->SetOutputMaximum( 1.0 );
			const double alpha =  -0.3;
			const double beta  =  2;
			sigmoidFilter->SetAlpha( alpha );
			sigmoidFilter->SetBeta( beta );

			// connect the filters:
			gradientMagnitudeFilter->SetInput( inputImg );
			sigmoidFilter->SetInput( gradientMagnitudeFilter->GetOutput() );

			// run the filters:
			sigmoidFilter->Update();

			edgePotentialImg = sigmoidFilter->GetOutput();

		} // end if(inputImg != NULL)
	}  // end initializeEdgePotentialImg()

	//----------------------------------------------------
	// DEBUG FUNCTION
	template< class DataType, class InputImageType, int nDims, class MType, class VType >
	void 
		Phys_LevelSet<DataType,InputImageType,nDims,MType,VType>
		::printInternalValues()
	{
		std::cout << "inputImg initialized: " << inputImg.IsNotNull() << std::endl;
		std::cout << "distanceImg initialized: " << distanceImg.IsNotNull() << std::endl;
		std::cout << "edgePotentialImg initialized: " << edgePotentialImg.IsNotNull() << std::endl;
		std::cout << "propagationScaling: " << propagationScaling << std::endl;
		std::cout << "curvatureScaling: " << curvatureScaling << std::endl;
		std::cout << "advectionScaling: " << advectionScaling << std::endl;
		std::cout << "maximumRMSError: " << maximumRMSError << std::endl;
		std::cout << "numIterations: " << numIterations << std::endl;
		return;
	}

	//----------------------------------------------------
	template< class DataType, class InputImageType, int nDims, class MType, class VType >
	bool 
		Phys_LevelSet<DataType,InputImageType,nDims,MType,VType>
		::runDeformation(const std::string defName,typename DeformationType::deformationIn* const arg ,std::stringstream * const stream)
	{

		//TODO make this more efficient--only update if change, only update new springs etc
		if(this->geom->didTopologyChange() || this->geom->didNodesChange())
		{
			//The distance image must be reinitialized
			initializeDistanceImg();
		}

		// TODO: replace O(n) search with lg(n) search on sorted list.
		for(int i=0; i< this->numDeformations; i++)
		{
			if( defName.compare(this->deformationsList[i]->getName()) == 0 )
			{
				try
				{
					typename DeformationType::DefArgSet org;
					org.distanceImg = distanceImg;
					this->deformationsList[i]->run(arg,&org,stream);
					break;
				}catch(typename DeformationType::Error * de)
				{
					std::cerr << "Could not complete deformation" << std::endl;
					Error e;
					e.msg = "Could not complete deformation";
					e.deformationNumber = i;
					e.deformationError = de;
					throw & e;
				}
			}
		}
		return false;
	}


	//----------------------------------------------------
	template< class DataType, class InputImageType, int nDims, class MType, class VType >
	bool 
		Phys_LevelSet<DataType,InputImageType,nDims,MType,VType>
		::simulate()
	{
		//std::cout << "Starting Phys_LevelSet simulation..." << std::endl; // DEBUG
		// NOTE: use smartPointer.IsNull(), not == NULL!
		if( inputImg.IsNull() ) { return -1; }
		else
		{
			if( distanceImg.IsNull() )
			{
				initializeDistanceImg();
				// TODO: add code here to generate a generic distance img (e.g. with a single seed in the middle)
			}
			if( edgePotentialImg.IsNull() ) 
			{
				initializeEdgePotentialImg(); // generate an edge image based on the current inputImg
			}

			// note that the third template parameter for this filter is 
			// defaulted to 'float' if not provided, which causes problems 
			// later with assigning its output to an image:
			typedef itk::GeodesicActiveContourLevelSetImageFilter<
				InternalImageType, 
				InternalImageType, 
				DataType >  GeodesicActiveContourFilterType;

			typename GeodesicActiveContourFilterType::Pointer 
				levelSetFilter = GeodesicActiveContourFilterType::New();

			// assign the levelSetFilter's inputs (class member images):
			levelSetFilter->SetInput(distanceImg );
			levelSetFilter->SetFeatureImage(		edgePotentialImg );

			levelSetFilter->SetPropagationScaling(	propagationScaling );
			levelSetFilter->SetCurvatureScaling(	curvatureScaling );
			levelSetFilter->SetAdvectionScaling(	advectionScaling );
			levelSetFilter->SetMaximumRMSError(		maximumRMSError );
			levelSetFilter->SetNumberOfIterations(	numIterations );

			// thresholding filter:
			typedef itk::BinaryThresholdImageFilter< 
				InternalImageType, 
				BinaryImageType >  ThresholdingFilterType;
			typename ThresholdingFilterType::Pointer thresholdFilter = ThresholdingFilterType::New();

			// TODO: eliminate hardcoding of thresholdFilter parameters?
			thresholdFilter->SetLowerThreshold( -1000.0 );
			thresholdFilter->SetUpperThreshold(     0.0 );
			thresholdFilter->SetOutsideValue(  0  );
			thresholdFilter->SetInsideValue(  255 );

			// connect the output of the levelSetFilter to the thresholdFilter's input:
			thresholdFilter->SetInput( levelSetFilter->GetOutput() );

			// run level set filter and threshold result:
			thresholdFilter->Update();

			outputImg = thresholdFilter->GetOutput();

			std::cout << "Phys_LevelSet simulation completed." << std::endl;
			//Feed the output distance image back into the level set filter
			distanceImg = levelSetFilter->GetOutput();

			std::cout << "Updating geometry" << std::endl;
			this->geom->generateTopologyFromBinaryImage(outputImg);

			std::cout << "Clock: " << this->time << std::endl;
			this->time ++;

			return false; // DEBUG: TEMPORARY!
		}
	} // end simulate()

	//----------------------------------------------------
	template< class DataType, class InputImageType, int nDims, class MType, class VType >
	void 
		Phys_LevelSet<DataType,InputImageType,nDims,MType,VType>
		::setExternalForces( void * notUsed)
	{
		// this is required because setExternalForces is pure virtual in the ABC
	}

	//----------------------------------------------------
	//template<class DataType,  int nDims>
	//	bool 
	//	Phys_LevelSet<DataType, vType, nDims>
	//	::runDeformation(std::istream& args)
	//{

	//}

} // end namespace mial

#endif // _PHYS_LEVEL_SET_CXX included