#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkMeanImageFilter.h"
#include "itkBinaryThresholdImageFilter.h"
#include "itkCovariantVector.h"

//#include "itkImage.h"
#include <iostream>
#include "Org_EulerSchedule.h"
#include "Org_LevelSetSchedule.h"
#include "Geometric.h"

using namespace mial;
int main(int argc, char * argv[] )
{
	if( argc > 6 )
	{
		std::cerr << "Usage: " << std::endl;
		std::cerr << argv[0] << "  inputImageFile   outputImageFile  inputEulerScheduleFile inputLeveSetScheduleFile inputMesh" << std::endl;
		return EXIT_FAILURE;
	}
	std::string inputImageFileName = argv[1];
	std::string outputImageFileName = argv[2];
	std::string eulerScheduleFileName = argv[3];
	std::string levelSetScheduleFileName = argv[4];
	std::string topologyInputFileName = argv[5];

	typedef unsigned char PixelType;
	const unsigned int Dimension = 3;
	typedef itk::Image< PixelType, Dimension > ImageType; 


	typedef itk::CovariantVector< float, 3 >  GradientPixelType;
	typedef itk::Image< GradientPixelType, 3 > GradientImageType;

	//Instantiate the reader/writer:
	typedef itk::ImageFileReader< ImageType >  ReaderType;
	typedef itk::ImageFileWriter< ImageType >  WriterType;
	ReaderType::Pointer reader = ReaderType::New();
	WriterType::Pointer writer = WriterType::New();

	reader->SetFileName(inputImageFileName.c_str());
	//TODO Make sensors pipelinable to avoid forcing update of reader
	reader->Update();
	writer->SetFileName(outputImageFileName.c_str());


	// Instantiate the organism:
	typedef itk::Org_EulerSchedule<ImageType, Geometric<float,3>::BinaryImageType, GradientImageType, float, 3> eulerOrganismType;
	eulerOrganismType::Pointer eulerOrg = eulerOrganismType::New();
	std::cout << "Euler organism created..." << std::endl;

	//Add Springs:
	eulerOrg->setTopology(topologyInputFileName);
	std::cout << "Topology read from '" << topologyInputFileName << "'..." << std::endl;
	//eulerOrg->writeNodesToFile(nodeOutputFileName);

	//TEST BEHAVIOR
	eulerOrg->setSchedule(eulerScheduleFileName);
	
	//Pass the output of the reader to the filter, then to the writer.
	eulerOrg->SetInput(reader->GetOutput());
	

	
	eulerOrg->setUp();
	eulerOrg->setRunTime(25);
	
	//Use a level set organism to smooth the euler's final shape
	typedef itk::Org_LevelSetSchedule<ImageType, ImageType, GradientImageType, float, 3> levelSetOrganismType;
	levelSetOrganismType::Pointer levelSetOrg = levelSetOrganismType::New();
	levelSetOrg->SetInput(reader->GetOutput());
	levelSetOrg->setSchedule(levelSetScheduleFileName);
	levelSetOrg->setUp();
	//Use the eulerOrg's output to create the shape for the levelSetOrg (creates the mesh and the distance image).
	//Will cause an update on the eulerOrganism since the mesh needs to be generated at this time.
	std::cout << "Running Euler organism..." << std::endl;
	levelSetOrg->setTopologyFromBinaryImage(eulerOrg->GetOutput());

	levelSetOrg->setRunTime(35);
	std::cout << "Running levelSet organism..." << std::endl;
	writer->SetInput(levelSetOrg->GetOutput());
	try
	{
		writer->Update(); //Will cause an update on the reader, because it depends on it
		std::cout << "Output written to '" << outputImageFileName << "'." << std::endl;
	}
	catch(itk::ExceptionObject & err)
	{
		std::cout << "ExceptionObject caught!" << std::endl;
		std::cout << err << std::endl;
		return -1;
	}
	return 0;
}