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

//#include "itkImage.h"
#include <iostream>
#include "itkOrganism.h"
#include "Phys_Euler.h"
#include "Ctrl_ScheduleDriven.h"
#include "Beh_SearchForObject.h"
#include "Beh_TranslateAll.h"
#include "Def_Translation.h"
#include "Beh_UniformScale.h"
#include "Def_UniformScale.h"
#include "Sense_Gradient.h"
#include "Geom_MeshSpatialObject.h"

using namespace mial;
int main(int argc, char ** argv)
{
	if( argc < 5 )
	{
		std::cerr << "Usage: " << std::endl;
		std::cerr << argv[0] << "  inputImageFile   outputImageFile  inputEulerScheduleFile inputMesh" << std::endl;
		return EXIT_FAILURE;
	}
	std::string inputImageFileName = argv[1];
	std::string outputImageFileName = argv[2];
	std::string topologyInputFileName = argv[4];
	std::string scheduleFileName = argv[3];
	
	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());
	writer->SetFileName(outputImageFileName.c_str());



	// Instantiate the organism:
	typedef itk::ItkOrganism <ImageType, ImageType, GradientImageType, float, 3> organismType;
	organismType::Pointer testOrg = organismType::New();
	std::cout << "Organism created..." << std::endl;

	//Instatiate a sensor
	typedef Sense_Gradient<float,ImageType,GradientImageType,3> gradientSensorType;
	gradientSensorType::sensorIn::Pointer input = gradientSensorType::sensorIn::New();
	gradientSensorType::Pointer gradientSensor = gradientSensorType::New();
	input->sigma = 1.0;
	reader->Update();
	input->imageIn = reader->GetOutput();
	gradientSensor->run(input);
	gradientSensorType::sensorOut::Pointer output = (gradientSensorType::sensorOut *) (gradientSensor->getOutput()).GetPointer();


	//Instantiate geomtery and physics layers
	typedef Phys_Euler<float,GradientImageType,3> PhysLayerType;
	typedef Geom_MeshSpatialObject<float,3> GeometricType;

	PhysLayerType::Pointer physLayer = PhysLayerType::New();
	GeometricType::Pointer geomLayer = GeometricType::New();
	
	physLayer->setExternalForces((void *) &(output->imageOut));
	physLayer->setGeometry(geomLayer);

	testOrg->setPhysicsLayer(physLayer);
	testOrg->setGeometricLayer(geomLayer);
	std::cout << "Physics layer added..." << std::endl;

	//Load the topology
	geomLayer->readTopologyFromFile(topologyInputFileName);
	std::cout << "Topology read from '" << topologyInputFileName << "'..." << std::endl;
	
	//Add cogntive, behavioral, and deformations
	Ctrl_ScheduleDriven<float, 3>::Pointer cgL = Ctrl_ScheduleDriven<float, 3>::New();
	cgL->setSchedule(scheduleFileName);
	
	Beh_TranslateAll<float, 3>::Pointer beh1 = Beh_TranslateAll<float,3>::New();
	Beh_UniformScale<float, 3>::Pointer beh2 = Beh_UniformScale<float,3>::New();
	Def_Translation<float, 3>::Pointer def1 = Def_Translation<float,3>::New();								
	Def_UniformScale<float, 3>::Pointer def2 = Def_UniformScale<float,3>::New();								
	testOrg->setCognitiveLayer(cgL);
	testOrg->addBehaviour(beh1);
	testOrg->addBehaviour(beh2);
	testOrg->addDeformation(def1);
	testOrg->addDeformation(def2);

	Beh_SearchForObject<float,ImageType,3>::Pointer beh3 = Beh_SearchForObject<float,ImageType,3>::New();
	beh3->image = reader->GetOutput();
	beh3->geomLayer = geomLayer;
	testOrg->addBehaviour(beh3);

	testOrg->setRunTime(120);
	//Pass the output of the reader to the filter, then to the writer.
	testOrg->SetInput(reader->GetOutput());

	writer->SetInput(testOrg->GetOutput());
	try
	{
		std::cout << "Running organism..." << std::endl;
		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;
}