#ifndef _Geom_MeshSpatialObject_cxx
#define _Geom_MeshSpatialObject_cxx
#include "Geom_MeshSpatialObject.h"

namespace mial
{

	template <class dType, int nDims,class MType, class VType >
	Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::Geom_MeshSpatialObject(dType p)
	{
		theMeshSpatialObject = MeshSpatialObjectType::New();
		theMesh = theMeshSpatialObject->GetMesh();
		writer = WriterType::New();
		m_IsInsidePrecision = p;
	}

	template <class dType, int nDims,class MType, class VType >
	MType 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::getMatrixNodePositions()
	{
		this->mNodes.set_size(this->numNodes,nDims);
		this->updateMatrixNodePositions();
		return this->mNodes;
	}

	/*template <class dType, int nDims,class MType, class VType >
	vType Geom_MeshSpatialObject<dType, nDims,MType,VType>::getVectorNodePositions()
	{
	// TODO finish this function ;)
	this.updateVectorNodePositions();
	//double a[nDims];
	/*for(int i=0; i<nDims; i++)
	{
	container[i] = nodes(index,i);
	}
	return vNodes;
	//return a;
	}*/

	template <class dType, int nDims,class MType, class VType >
	bool 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::setMatrixNodePositions(MatrixType pos, int * inds)
	{
		for(int i=0; i< sizeof(inds)/sizeof(int); i++)
			this->mNodes.set_row(i,pos.get_row(i));
		this->updateMeshNodePositions();
		return true;
	}

	template <class dType, int nDims,class MType, class VType >
	bool 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::addNodes(MatrixType pos, VectorType classes)
	{
		
		MatrixType tmp;
		tmp.set_size(this->numNodes+pos.rows(),nDims);
		VectorType tmpC;
		tmpC.set_size(this->numNodes+pos.rows());

		//Copy the information
		for(int i=0;i<this->numNodes;i++)
		{
			tmp.set_row(i,this->mNodes.get_row(i));
			tmpC(i) = this->nodeClass(i);
		}
		//Add the new information
		for(int i=this->numNodes, c=0;i<this->numNodes+pos.rows();i++,c++)
		{
			tmp.set_row(i,pos.get_row(c));
			tmpC(i) = classes(c);
		}

		this->numNodes +=pos.rows();

		this->mNodes.set_size(this->numNodes,nDims);
		this->mNodes = tmp;//Copy back the old information plus the new information
		this->nodeClass = tmpC;

		//Update the mesh node positions to include the new nodes
		this->updateMeshNodePositions();
		return true;
	}
	template <class dType, int nDims,class MType, class VType >
	bool 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::setMatrixNodePositions(MatrixType pos)
	{
		this->mNodes = pos;
		this->updateMeshNodePositions();
		return true;
	}

	template <class dType, int nDims,class MType, class VType >
	void 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::updateMatrixNodePositions()
	{
		typename MeshType::PointsContainer::Iterator pointIterator;
		pointIterator = theMesh->GetPoints()->Begin();
		typename MeshType::PointsContainer::Iterator end = theMesh->GetPoints()->End();

		int count =0;
		this->numNodes =0;
		//TODO just get the # of points from the mes
		while( pointIterator != end )
		{	++pointIterator; // advance to next point
		this->numNodes++;
		}


		pointIterator = theMesh->GetPoints()->Begin();
		this->mNodes.set_size(this->numNodes,nDims);
		while( pointIterator != end )
		{	
			typename MeshType::PointType p = pointIterator.Value(); // access the point
			for(int i=0;i<nDims;i++)
			{
				this->mNodes(count,i)=p[i];
			}
			++pointIterator; // advance to next point
			count++;
		}
	}

	template <class dType, int nDims,class MType, class VType >
	void 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::updateMeshNodePositions()
	{

		PointType tmp;
		for(int count=0;count < this->numNodes; count++)
		{
			for(int i=0; i<nDims; i++)
			{
				tmp[i] = this->mNodes(count,i);
			}
			theMesh->SetPoint(count,tmp); //Add the point to the mesh
		}
		theMeshSpatialObject->SetMesh(theMesh);
	}

	template <class dType, int nDims,class MType, class VType >
	void 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::updateMatrixConnections()
	{
		CellIterator cellIterator = theMesh->GetCells()->Begin();
		CellIterator cellEnd = theMesh->GetCells()->End();

		cellIterator = theMesh->GetCells()->Begin();
		cellEnd = theMesh->GetCells()->End();

		typename TriangleType::PointIdIterator pit;

		//TODO dynamically figure out the number of connections
		//There are three connections per cell
		this->mConnections.set_size(3*theMesh->GetNumberOfCells(),2);
		this->numConnections =0;
		while( cellIterator != cellEnd )
		{
			CellType * cell = cellIterator.Value();
			switch( cell->GetType() )
			{
			case CellType::TRIANGLE_CELL:
				{
					TriangleType * tri = dynamic_cast<TriangleType *>( cell );
					pit = tri->PointIdsBegin();
					//Create a spring for each edge of the triangle

					mConnections(this->numConnections,0) = *(pit);
					mConnections(this->numConnections,1) = *(pit+1);
					this->numConnections++;

					mConnections(this->numConnections,0) = *(pit+1);
					mConnections(this->numConnections,1) = *(pit+2);
					this->numConnections++;

					mConnections(this->numConnections,0) = *(pit+2);
					mConnections(this->numConnections,1) = *(pit);
					this->numConnections++;

					break;
				}
			}
			++cellIterator;
		}
	}

	template <class dType, int nDims,class MType, class VType >
	void 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::writeNodesToFile( std::string fileName )
	{
		std::ofstream out(fileName.c_str());

		typename MeshType::PointsContainer::Iterator pointIterator;
		pointIterator = theMesh->GetPoints()->Begin();
		typename MeshType::PointsContainer::Iterator end = theMesh->GetPoints()->End();
		out << theMesh->GetNumberOfPoints() << std::endl;
		while( pointIterator != end )
		{
			typename MeshType::PointType p = pointIterator.Value(); // access the point
			out << p << std::endl; // print the point
			++pointIterator; // advance to next point
		}
	}

	template <class dType, int nDims,class MType, class VType >
	void 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::writeObjectToFile(std::string fileName)
	{
		writer->SetInput( theMeshSpatialObject );
		writer->SetFileName( fileName.c_str() );
		writer->Update();
	}

	template <class dType, int nDims,class MType, class VType >
	void 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::readNodesFromFile( std::string fileName )
	{
		std::ifstream in(fileName.c_str());
		int count = 0;
		int size =0;
		in >> size;
		//MeshType::CoordRepType nodes[size][nDims];
		PointType tmp;
		while(count < size)
		{
			for(int i=0; i<nDims; i++)
			{
				in >> tmp[i];
			}
			theMesh->SetPoint(count,tmp); //Add the point to the mesh
			count++;
		}
		this->numNodes = theMesh->GetNumberOfPoints();
		theMeshSpatialObject->SetMesh(theMesh);
		updateMatrixNodePositions();

		//Update the state variable
		this->nodesChange = true;
	}

	template <class dType, int nDims,class MType, class VType >
	bool 
		Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::readTopologyFromFile( std::string fileName )
	{
		//TODO error checking on correct input type

		typename ReaderType::Pointer reader = ReaderType::New();
		reader->SetFileName(fileName.c_str());
		reader->Update();


		/*typedef itk::SpatialObjectToImageFilter< GroupType,BinaryImageType >		SpatialObjectToImageFilterType;
		SpatialObjectToImageFilterType::Pointer objectToImageFilter = SpatialObjectToImageFilterType::New();
		objectToImageFilter->SetInput( reader->GetGroup());

		objectToImageFilter->Update();
		typedef typename itk::ImageFileWriter<BinaryImageType> writerType;
		writerType::Pointer writer = writerType::New();
		writer->SetFileName("C:\\testSpatial.mhd");
		writer->SetInput( objectToImageFilter->GetOutput() );
		writer->Update();*/



		/*	typedef itk::SceneSpatialObject<3> SceneSpatialObjectType;
		SceneSpatialObjectType::Pointer scene = SceneSpatialObjectType::New();
		typedef itk::EllipseSpatialObject<3> EllipseType;
		EllipseType::Pointer ellipse1 = EllipseType::New();
		ellipse1->SetRadius(1);
		ellipse1->SetId(1);
		EllipseType::Pointer ellipse2 = EllipseType::New();
		ellipse2->SetId(2);
		ellipse2->SetRadius(2);
		scene->AddSpatialObject(ellipse1);
		scene->AddSpatialObject(ellipse2);

		EllipseType* ellipse3;// = EllipseType::New();
		ellipse3 = dynamic_cast<EllipseType *> (scene->GetObjectById(1)); */






		//GroupType::ChildrenListPointer children = (reader->GetGroup())->GetChildren();
		//GroupType::ChildrenListType::iterator it = children->begin();

		//Get the first child's mesh
		MeshSpatialObjectType* t = dynamic_cast<MeshSpatialObjectType *> ( (reader->GetScene())->GetObjectById(0));
		theMeshSpatialObject->SetMesh( t->GetMesh());
		theMesh = theMeshSpatialObject->GetMesh();
		//theMeshSpatialObject->SetMesh( ->GetMesh() );

		//Update the points
		updateMatrixNodePositions();

		//Update the matrix connections
		updateMatrixConnections();

		//Set the state variable so other modules are aware of the change.
		this->topologyChange = true;

		/*
		int count = 0;
		int size =0;
		int start =0;
		int middle =0;
		int end =0;
		in >> size;
		//MeshType::CoordRepType nodes[size][nDims];
		PointType tmp;
		CellAutoPointer Lines;
		mConnections.set_size(size,2);//Store all of the connections in vector format
		while(count < size)
		{
		in >> start >> end;
		Lines.TakeOwnership( new LineType);
		Lines->SetPointId(0,start);
		Lines->SetPointId(1,end);
		//theMesh->SetCell(theMesh->GetNumberOfCells(),Lines);

		mConnections(count,0) = start;
		mConnections(count,1) = end;
		count++;
		}
		numConnections = count;
		theMeshSpatialObject->SetMesh(theMesh);
		//std::cout << "Mesh bounds : " << theMeshSpatialObject->GetBoundingBox()->GetBounds() << std::endl;
		topologyChange = true;*/
		return false;
	}

	template < class dType, int nDims, class MType, class VType >
	typename itk::Image< unsigned char, nDims>::Pointer
		Geom_MeshSpatialObject< dType, nDims, MType, VType >
		::generateBinaryImageFromTopology( typename Geom_MeshSpatialObject<dType,nDims,MType,VType>::BinaryImageType::SizeType size)
	{
//		std::cout << "Starting binary image generation from mesh..." << std::endl;
		typename BinaryImageType::Pointer image = BinaryImageType::New();

		typedef itk::TriangleMeshToBinaryImageFilter< MeshType, BinaryImageType >  meshToImageFilterType;
		typename meshToImageFilterType::Pointer meshToImageFilter = meshToImageFilterType::New();
		
		// TODO fix this workaround and pass BinaryImageType::SizeType variable instead of dType*
		/*BinaryImageType::SizeType size;
		for( unsigned int i=0; i<nDims; i++)
		{
			size[i] = imageSize[i];
		}*/

		meshToImageFilter->SetSize( size );

		meshToImageFilter->SetInput( theMeshSpatialObject->GetMesh() );
		image = meshToImageFilter->GetOutput();
		meshToImageFilter->Update();

//		std::cout << "Finished binary image generation from mesh." << std::endl;
//		BinaryImageType::IndexType start;
//		BinaryImageType::SizeType size;
//		dType origin[ nDims ]; 
//
//		//Compute the size of the spatial object
//		theMeshSpatialObject->ComputeBoundingBox();
//		//std::cout << "Mesh bounds : " << theMeshSpatialObject->GetBoundingBox()->GetBounds() << std::endl;
//		for(unsigned int i=0;i<nDims;i++)
//		{
//			size[i] = (long unsigned int)(theMeshSpatialObject->GetBoundingBox()->GetMaximum()[i]- theMeshSpatialObject->GetBoundingBox()->GetMinimum()[i]);
//			start[i] =0;
//			origin[i] =theMeshSpatialObject->GetBoundingBox()->GetMinimum()[i];
//		}
//		BinaryImageType::RegionType region;
//		region.SetSize( size );
//		region.SetIndex( start);
//
//		image->SetRegions( region );
//		image->Allocate();
//
//		//TODO get spacing from input image
//		/*dType spacing[ nDims ];
//		image->SetSpacing( spacing );*/
//
//		//TODO get correct origin (offset) to map to world coridinates
//		image->SetOrigin( origin );
//
//		//Define a point for the image and the iterators
//		BinaryImageType::PointType pointXYZ;
//		typedef itk::ImageRegionIterator< BinaryImageType> IteratorType;
//
//		IteratorType imgIt( image, region );
//		VectorType vPoint(nDims);
//		for ( imgIt.GoToBegin(); !imgIt.IsAtEnd();++imgIt)
//		{
//			//Convert from index to 3-space
//			image->TransformIndexToPhysicalPoint(imgIt.GetIndex(),pointXYZ);
//			for(int i=0;i<nDims;i++)
//			{
//				vPoint(i) = pointXYZ[i];
//			}
//			imgIt.Set( 255*isInside(vPoint) );
//		}
///*
//		MeshSpatialObjectType::PointType myPhysicalPoint; 
//		myPhysicalPoint.Fill(50);
//		std::cout << "Is my physical point inside? : " << theMeshSpatialObject->IsInside(myPhysicalPoint) << std::endl;
//
//		VectorType p(nDims);
//		p(0) = 50;
//		p(1) = 60;
//		p(2) = 30;
//		std::cout << "Is my physical point REALLY inside? : " << this->isInside(p) << std::endl;
//
//
//		p(0) = 5;
//		p(1) = 5;
//		p(2) = 5;
//		std::cout << "Is my other physical point REALLY inside? : " << this->isInside(p) << std::endl;
//
//
//		typedef typename itk::ImageFileWriter<BinaryImageType> writerType;
//		writerType::Pointer writer = writerType::New();
//		writer->SetFileName("C:\\testSpatial.mhd");
//		writer->SetInput( image );
//		writer->Update();
//
//		//end testing*/

		return image;
	}

	template < class dType, int nDims, class MType, class VType >
	void 
		Geom_MeshSpatialObject< dType, nDims, MType, VType >
		::generateTopologyFromBinaryImage( BinaryImageTypePointer binaryInputImage )
	{
		typedef itk::BinaryMask3DMeshSource< BinaryImageType, MeshType >		MeshSourceType;
		typename MeshSourceType::Pointer imgToMeshFilter = MeshSourceType::New();

		imgToMeshFilter->SetInput( binaryInputImage );

		// Get the max binary image value so we can define what is "inside" the mesh:
		typedef itk::MinimumMaximumImageFilter< BinaryImageType >				MinMaxFilterType;
		typename MinMaxFilterType::Pointer minMaxFilter = MinMaxFilterType::New();
		minMaxFilter->SetInput( binaryInputImage );
		minMaxFilter->Update();

		imgToMeshFilter->SetObjectValue( minMaxFilter->GetMaximum() ); 

		imgToMeshFilter->Update();

		std::cout << "BinaryMask3DMeshSource update completed." << std::endl; // DEBUG
		theMesh = imgToMeshFilter->GetOutput();

		// TODO: find out why CellData is bad and use a better solution
		// for some reason, unknown CellData is being written to file; temporary workaround:
		typename MeshType::CellDataContainer::Pointer emptyCellDataContainer = MeshType::CellDataContainer::New();
		theMesh->SetCellData( emptyCellDataContainer );
		theMeshSpatialObject->SetMesh( theMesh );
	}
	template < class dType, int nDims, class MType, class VType >
	bool
		Geom_MeshSpatialObject< dType, nDims, MType, VType >
		::isInside( VectorType p)
	{//Check if a given point is inside the object

		//For all triangles, check if a ray extending from the object in both directions intersects.
		//If the ray intersects an odd number of triangles on both sides in it is inside.
		CellIterator cellIterator = theMesh->GetCells()->Begin();
		CellIterator cellEnd = theMesh->GetCells()->End();

		cellIterator = theMesh->GetCells()->Begin();
		cellEnd = theMesh->GetCells()->End();

		TriangleType * tri;
		int pCount = 0;
		int nCount = 0;
		VectorType ray(nDims);

		//Since any arbritary ray can be checked always use x-axis
		ray.fill(0);
		ray(0)=1;
		ray(1)=0;
		ray(2)=0;

		double minDist =0;
		CoordRepType closestPoint[nDims];
		CoordRepType position[nDims]; //A point

		for(int i=0; i<nDims;i++)
			position[i] = p(i);

		while( cellIterator != cellEnd )
		{
			CellType * cell = cellIterator.Value();
			if( cell->GetType() == CellType::TRIANGLE_CELL )
			{
				tri = dynamic_cast<TriangleType *>( cell );
				tri->EvaluatePosition(position,theMesh->GetPoints(),closestPoint,NULL,&minDist,NULL);
				if(abs(minDist) <= m_IsInsidePrecision)
				{//If the point is on the surface return true
					return true;
				}else
				{//otherwise check if it is within the object.
					if(intersectWithLine(ray,p,tri))
						pCount++;
					if(intersectWithLine(-ray,p,tri))
						nCount++;
				}
			}
			++cellIterator;
		}
		return ( (pCount%2 !=0)&&(nCount%2!=0));//Return true iff both are odd
	}

	template < class dType, int nDims, class MType, class VType >
	bool
		Geom_MeshSpatialObject< dType, nDims, MType, VType >
		::intersectWithLine( VectorType L, VectorType P, TriangleType* tri)
	{//Check if a line L(s) = P + s*L intersects a triangle tri

		if(this->nodesChange) //Make sure the positions are up to date
			updateMatrixNodePositions();

		typename TriangleType::PointIdIterator pit;
		pit = tri->PointIdsBegin();

		//Compute the normal to triangle
		VectorType v1(nDims);
		VectorType v2(nDims);
		VectorType n(nDims);
		VectorType w(nDims);
		VectorType intersectionPoint(nDims);

		v1 = this->mNodes.get_row(*(pit+1))-this->mNodes.get_row(*pit);
		v2 = this->mNodes.get_row(*(pit+2))-this->mNodes.get_row(*pit);
		n = vnl_cross_3d(v1,v2);
		n.normalize(); //Ensure that n is unit length
		L.normalize(); //Ensure that L is unit length

		/*std::cout << "v1 : " << v1(0) << " " << v1(1) << " " << v1(2) <<std::endl;
		std::cout << "v2 : " << v2(0) << " " << v2(1) << " " << v2(2) <<std::endl;
		std::cout << "n : " << n(0) << " " << n(1) << " " << n(2) <<std::endl;
		std::cout << "L : " << L(0) << " " << L(1) << " " << L(2) <<std::endl;*/
		dType s;
		CoordRepType position[nDims]; //A point
		double minDist;
		CoordRepType closestPoint[nDims];
		//Are L and the normal orthogonal, if so return false
		double t;
		t = dot_product(n,L);
		if( t != 0)
		{
			w = P - this->mNodes.get_row(*(pit));//Vector from a point on the line to a point on the plane
			s = -dot_product(n,w)/dot_product(n,L);
			if(s>=0)
			{
				intersectionPoint = P+ s*L;
				//std::cout << "Intersection at : " << intersectionPoint(0) << " " << intersectionPoint(1) << " " << intersectionPoint(2) <<std::endl;
				for(int i=0;i<nDims;i++)
					position[i] = intersectionPoint(i);

				//Does the point lie within the triangle?
				//Check how close the intersection point is to the triangle, accept if its within a tolerance.
				//The distance will be returned in minDist;
				tri->EvaluatePosition(position,theMesh->GetPoints(),closestPoint,NULL,&minDist,NULL);
				//std::cout << "Min dist: " << minDist << std::endl;
				if(abs(minDist) <= m_IsInsidePrecision)
					return true;
				else
				{
					return false;
				}
			}
			else
				return false;
		}
		else
		{
			return false;
		}
	}

	template < class dType, int nDims, class MType, class VType >
	bool
		Geom_MeshSpatialObject< dType, nDims, MType, VType >
		::addConnection(int start,int end)
	{
		MatrixType tmp;
		tmp.set_size(this->numConnections+1,2);

		//Copy the information
		for(int i=0;i<this->numConnections;i++)
		{
			tmp(i,0) = this->mConnections(i,0);
			tmp(i,1) = this->mConnections(i,1);
		}
		tmp(this->numConnections,0) = start;
		tmp(this->numConnections,1) = end;

		this->numConnections++;

		this->mConnections.set_size(this->numConnections,2);
		this->mConnections = tmp;//Copy back the old information plus the new information

		CellAutoPointer Lines;
		//Add a new line
		
		//Can't use lines, as SOV will not render them
		/*Lines.TakeOwnership( new LineType);
		Lines->SetPointId(0,start);
		Lines->SetPointId(1,end);
		this->theMesh->SetCell(theMesh->GetNumberOfCells(),Lines);*/

		//Need triangles instead.


		this->topologyChange = true;

		return false;
	}

}//end mial
#endif