#ifndef _PHYS_EULER_CXX
#define _PHYS_EULER_CXX

#include "Phys_Euler.h"
#include "itkImageRegionIterator.h"
namespace mial
{

	template<class DataType, class TGradientImage, int nDims,class MType, class VType>
	Phys_Euler<DataType, TGradientImage, nDims,MType,VType>
		::Phys_Euler(int numNodes ,int numSprings,int numPossibleDeformations,int defK)
		:Physics<DataType,nDims,MType,VType>()
	{
		nodes.set_size(numNodes,nDims);

		nodesV.set_size(numNodes,nDims);

		nodesF.set_size(numNodes,nDims);

		nodesA.set_size(numNodes,nDims);

		nodesM.set_size(numNodes);

		springsRest.set_size(numSprings);

		springsDamp.set_size(numSprings);

		springsNodes.set_size(numSprings,2);

		springLengths.set_size(numSprings);

		springsK.set_size(numSprings);

		timeStep = 0.005;

		defaultK = defK;
		defaultDamp = 1;
		defaultMass = 1;

		defaultDrag = 1;

		gradientPointer = GradientImageType::New();

		imageForces = true;
	}


	template<class DataType, class TGradientImage, int nDims,class MType, class VType>
	void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setRestLengths(int* a, DataType* values, int n)
	{
		if(this->geom->didTopologyChange())
			updateSpringsFromGeometric();
		for(int i=0; i<n; i++)
		{
			springsRest(a[i]) = values[i];
		}
	}



	template<class DataType, class TGradientImage, int nDims,class MType, class VType>
	void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setRestLengths(VectorType a, VectorType values)
	{
		if(this->geom->didTopologyChange())
			updateSpringsFromGeometric();
		for(int i=0; i<a.size(); i++)
		{
			springsRest(a(i)) = values(i);
		}
	}


	template<class DataType, class TGradientImage, int nDims,class MType, class VType>
	void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setSpringLengths(int* a, DataType* values, int n)
	{// array a is an array of index values, and array values are the values to set them too.

		if(this->geom->didTopologyChange())
			updateSpringsFromGeometric();
		for(int i=0; i<n; i++)
		{
			springLengths(a[i]) = values[i];
		}
	}


	template<class DataType, class TGradientImage, int nDims,class MType, class VType>
	void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setSpringLengths(VectorType a, VectorType values)
	{
		if(this->geom->didTopologyChange())
			updateSpringsFromGeometric();
		for(int i=0; i<a.size(); i++)
		{
			springLengths(a(i)) = values(i);
		}
	}


	template<class DataType, class TGradientImage, int nDims,class MType, class VType>
	void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setSpringsK(int* a, DataType* values, int n)
	{// array a is an array of index values, and array values are the values to set them too.

		if(this->geom->didTopologyChange())
			updateSpringsFromGeometric();
		for(int i=0; i<n; i++)
		{
			springsK(a[i]) = values[i];
		}
	}


	template<class DataType, class TGradientImage, int nDims,class MType, class VType>
	void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setSpringsK(VectorType a, VectorType values)
	{
		if(this->geom->didTopologyChange())
			updateSpringsFromGeometric();
		for(int i=0; i<a.size(); i++)
		{
			springsK(a(i)) = values(i);
		}
	}

	template<class DataType, class TGradientImage, int nDims,class MType, class VType>
	bool Phys_Euler<DataType, TGradientImage, 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
		nodes = this->geom->getMatrixNodePositions();
		if(this->geom->didTopologyChange())
			updateSpringsFromGeometric();
		if(this->geom->didNodesChange())
		{
			nodesF.set_size(this->geom->getNumNodes(),nDims);
			nodesF.fill(0);
			nodesA.set_size(this->geom->getNumNodes(),nDims);
			nodesA.fill(0);
			nodesV.set_size(this->geom->getNumNodes(),nDims);
			nodesV.fill(0);
			nodesFDef.set_size(this->geom->getNumNodes(),nDims);
			nodesFDef.fill(0);
			nodesM.set_size(this->geom->getNumNodes());
			nodesM.fill(defaultMass);
		}

		//std::string defName;
		//args >> defName; //read the first string from the stream
		// 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.nodes = &nodes;
					org.nodesV = &nodesV;
					org.nodesF = &nodesF;
					org.nodesFDef = &nodesFDef;
					org.springsRest = &springsRest;
					org.springsNodes = &springsNodes;
					org.springLengths = &springLengths;

					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 TGradientImage, int nDims,class MType, class VType>
	void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::updateSpringsFromGeometric()
	{
		unsigned int length = springsNodes.rows();
		if(this->geom->getNumConnections() != length) //if there are new springs
		{
			unsigned int newLength = this->geom->getNumConnections();

			springsRest.set_size(newLength);
			springsDamp.set_size(newLength);
			springLengths.set_size(newLength);
			springsK.set_size(newLength);
			springsNodes.set_size(newLength,2);

			this->springsNodes = this->geom->getMatrixConnections();
			int from;
			int to;
			for(int i=0; i<newLength; i++)
			{
				from = springsNodes(i,0);
				to = springsNodes(i,1);
				if(DEBUG)
					std::cout << "from: " << from << " to: " << to << " i " << i << std::endl;
				springsRest(i) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
				springsDamp(i) = defaultDamp;
				springLengths(i) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
				springsK(i) =defaultK;
			}

			// TODO write method for dynamically changing topology that preserves current settings
			// This will need to search the set of current connections for existing springs and find those
			// still in use
			/*
			//Store current values in a temp variable

			vnl_Vector tmp(length*4);

			tmp.update(springsRest,0);
			tmp.update(springsDamp, length);
			tmp.update(springLengths, length*2);
			tmp.update(springsK, length*3);

			vnl_matrix<int> tmp2(length,2);
			tmp2.update(springsNodes,0,0);

			//Increase the size
			springsRest.set_size(newLength);
			springsDamp.set_size(newLength);
			springLengths.set_size(newLength);
			springsK.set_size(newLength);
			springsNodes.set_size(newLength,2);

			//Copy back the old values
			for(int i =0; i< length; i++)
			{
			springsRest(i) = tmp(i);

			//springsDamp.update(tmp.get_n_rows(nodes.rows(),nodes.rows()) ,0);
			springsDamp(i) = tmp(i+length);

			//springLengths.update(tmp.get_n_rows(nodes.rows()*2,nodes.rows()) ,0);
			springLengths(i) = tmp(i+2*length);

			//springsK.update(tmp.get_n_rows(nodes.rows()*3,nodes.rows()) ,0);
			springsK(i) = tmp(i+3*length);

			}
			springsNodes.update(tmp2,0);

			//Add the new springs
			int from =0;
			int to =0;
			for(int i=length; i<newLength; i++)
			{
			from = 0;
			springsRest(i) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
			springsDamp(i) = defaultDamp;

			springLengths(length) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();

			springsK(length) =defaultK;

			springsNodes(length,0) = from;
			springsNodes(length,1) = to;*/
		}
	}

	template<class DataType, class TGradientImage, int nDims,class MType, class VType>
	bool Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::simulate()
	{
		//**************************//
		//Simulate the spring forces
		//**************************//

		//std::cout << "Begin deform" << std::endl;
		//TODO make this more efficient--only update if change, only update new springs etc
		nodes = this->geom->getMatrixNodePositions();
		if(this->geom->didTopologyChange())
			updateSpringsFromGeometric();
		if(this->geom->didNodesChange())
		{
			nodesF.set_size(this->geom->getNumNodes(),nDims);
			nodesF.fill(0);
			nodesA.set_size(this->geom->getNumNodes(),nDims);
			nodesA.fill(0);
			nodesV.set_size(this->geom->getNumNodes(),nDims);
			nodesV.fill(0);
			nodesFDef.set_size(this->geom->getNumNodes(),nDims);
			nodesFDef.fill(0);
			nodesM.set_size(this->geom->getNumNodes());
			nodesM.fill(defaultMass);
		}

		const unsigned int length = springsNodes.rows();

		VectorType nodeDist(nDims);
		DataType distNorm;
		VectorType velDiff(nDims);
		DataType D;
		VectorType F(nDims);
		VectorType pointForce(nDims);
		VectorType tmpVel(nDims);
		VectorType tmpDist(nDims);
		VectorType tmpPos(nDims);
		//Can't seem to get working with length
		/*	vnl_matrix_fixed<DataType, 4 ,nDims> nodeDist;
		vnl_vector_fixed<DataType,length> distNorm;
		vnl_matrix_fixed<DataType, 4,nDims> velDiff;
		vnl_vector_fixed<DataType, 4> D;
		vnl_matrix_fixed<DataType, 4, nDims> F;*/
		int a,b;
		DataType rowSum = 0;
		int count = 0;
		int runTime = 25;
		int numNodes = nodesF.rows();
		typename GradientImageType::RegionType gradientImageRegion = gradientPointer->GetLargestPossibleRegion();
		itk::ImageRegionIterator<GradientImageType> gradIT(gradientPointer,gradientImageRegion);
		typename GradientImageType::IndexType gradIndex;

		typedef itk::VectorLinearInterpolateImageFunction< GradientImageType, DataType > InterpolatorType;
		typename InterpolatorType::Pointer interp = InterpolatorType::New();
		typename InterpolatorType::OutputType imgForce;
		interp->SetInputImage(gradientPointer);


		DataType zero = 0.0;

		//	springsRest(0) = 15;
		bool incTime = true;
		DataType dispLength =0;
		DataType localTimeStep = timeStep;

		DataType endTime = this->time+1.0;
		//while(count<runTime)
		while(this->time< endTime)
		{
			nodesF.fill(0);
			if(count==0)
			{
				nodesF = nodesF + nodesFDef; //Add on any deformation forces, then zero them
				nodesFDef.fill(0);
			}
			//Loop over all springs
			for(int i=0; i<length ;i++)
			{
				//TODO: Finalize code, vectorize or loop

				//A = nodesXYZ(springsNodes(:,2),:)  -  nodesXYZ(springsNodes(:,1),:);
				//B = sum(nodeDist.^2,2).^0.5; //%norm(Xa-Xb) for all springs (numSprings*1)
				//C = nodesV(springsNodes(:,2),:)  -  nodesV(springsNodes(:,1),:);
				nodeDist = nodes.get_row(springsNodes(i,1)) - nodes.get_row(springsNodes(i,0));
				distNorm = nodeDist.two_norm();
				velDiff	= nodesV.get_row(springsNodes(i,1)) - nodesV.get_row(springsNodes(i,0));

				//D = sum(C.*nodeDist,2) ./ B ;// %[(Va-Vb)'(Xa-Xb)]/norm(Xa-Xb)  for all springs (numSprings*1) 
				for(int j=0; j<nDims;j++)
				{
					rowSum+= velDiff(j)*nodeDist(j);
				}
				D = rowSum/distNorm;
									
				rowSum =0;

				//F = repmat((springsK.*(B-springsRest)+springsDamp.*D)./B , [1 3]).*nodeDist; //%spring forces for all springs (numSprings*2)
				F = (( (springsK(i)* (distNorm-springsRest(i)) + springsDamp(i)*D)/ distNorm ) )* nodeDist;
				if(distNorm == zero)
					F.fill(0);

				/*	for k=1:numSprings,  // %assign spring forces to nodes
				a=springsNodes(k,1);
				b=springsNodes(k,2);
				nodesF(a,:)=nodesF(a,:) + F(k,:);
				nodesF(b,:)=nodesF(b,:) - F(k,:);
				end*/

				a = springsNodes(i,0);
				b = springsNodes(i,1);
				nodesF.set_row(a,nodesF.get_row(a)+ F);
				nodesF.set_row(b,nodesF.get_row(b)- F);
				//if(i%4000)
					//std::cout << "done " << i << std::endl;
			}
			//Loop over all nodes
			for(int i=0; i<numNodes; i++)
			{
			
				//**************************//
				//Calculate Image forces
				//**************************//
				//nodesF=nodesF+ repmat(nodesExt.* nodesBnd , [1,2]) .* [...
				//     diag(Gx(round(nodesXY(:,2)),round(nodesXY(:,1)))) , ...
				//   diag(Gy(round(nodesXY(:,2)),round(nodesXY(:,1)))) ];

				if(DEBUG)
				{	std::cout << "node: " << i << " x: " << nodes(i,0) << " y: " << nodes(i,1) << " z: " << nodes(i,2) << std::endl;
				std::cout << "Force: " << i << " x: " << nodesF(i,0) << " y: " << nodesF(i,1) << " z: " << nodesF(i,2) << std::endl;
				}

				if(imageForces)
				{
					for(int a =0; a<nDims; a++)
					{gradIndex[a] = nodes(i,a);
					}

					//pointForce(0) = interp->EvaluateAtIndex(gradIndex);
					imgForce = interp->EvaluateAtIndex(gradIndex);

					for(int a =0; a<nDims; a++)
					{pointForce(a) = 5*imgForce[a];
					}
					//pointForce(1) = interp->EvaluateAtIndex(gradIndex);

					nodesF.set_row(i, nodesF.get_row(i) +  pointForce);
				}

				if(DEBUG)
					std::cout << "Force: " << i << " x: " << nodesF(i,0) << " y: " << nodesF(i,1) << " z: " << nodesF(i,2) << std::endl;

				//std::cout << "Force: " << i << " x: " << 100*pointForce(0) << " y: " << 100*pointForce(1) << " z: " << std::endl;

				//**************************//
				//Drag Force
				//**************************//
				nodesF.set_row(i, nodesF.get_row(i)-nodesV.get_row(i)*defaultDrag); 

				//**************************//
				//Apply forces to model
				//**************************//
				//nodesA=nodesF ./ (repmat(nodesM,[1,3]));%accelaration
				//nodesV=DT * nodesA + nodesV;%new velocity 
				//nodesXYZ=(DT * nodesV).*repmat(1-nodesFxd,[1,3]) + nodesXYZ;%new position 

				nodesA.set_row(i, nodesF.get_row(i)/nodesM(i) );
				if(DEBUG)
					std::cout << "Accel: " << i << " x: " << nodesA(i,0) << " y: " << nodesA(i,1) << " z: " << nodesA(i,2) << std::endl;

				//TODO: This method of time adjustment may actually slow down time for some nodes and not others. Fix that.
				//TODO: Possible solution: Build a tmp matrix of new velocities, stop and update if time changes.
				//int tc = 0;
				while(incTime)
				{
					tmpVel = (localTimeStep*nodesA.get_row(i) + nodesV.get_row(i));
					tmpDist = (localTimeStep * tmpVel );

					if(DEBUG)
					{	std::cout << "Dist: " << i << " x: " << tmpDist(0) << " y: " << tmpDist(1) << " z: " << tmpDist(2) << std::endl;
					std::cout << "Vel: " << i << " x: " << nodesV(i,0) << " y: " << nodesV(i,1) << " z: " << nodesV(i,2) << std::endl;
					}

					//Check if velocity passes thresh
					dispLength = abs(tmpDist.two_norm());
					if(  dispLength< 1 )
					{
						incTime = false;
						//Make sure it doesn't push the node outside of the boundary
						if(BOUND_CHECKING)
						{
							tmpPos = tmpDist+nodes.get_row(i);
							//TODO: Is there a way without converting to an itk index?
							for(int a =0; a<nDims; a++)
								gradIndex[a] = tmpPos(a);
							if(gradientImageRegion.IsInside(gradIndex))
							{
								nodesV.set_row(i, tmpVel );		
								nodes.set_row(i,tmpPos);
							}
						}
						else
						{
							nodesV.set_row(i, tmpVel );			
							nodes.set_row(i,  tmpDist + nodes.get_row(i) );
						}
					}
					else //shrink the time step and recalc.
					{
						//std::cout << "DT : " << localTimeStep << std::endl;
						localTimeStep = localTimeStep/2;
						//tc++;
					}
				}//end while(incTime)
				incTime = true;

				if(DEBUG)
					std::cout << "done node: " << i << std::endl;
			}//end loop over nodes
			count++;
			//Update the time
			this->time = this->time+localTimeStep;
			//std::cout << "Clock: " << this->time << std::endl;
		}//end while
		this->geom->setMatrixNodePositions(nodes);
		//std::cout << "End deform" << std::endl;
		std::cout << "Clock: " << this->time << std::endl;
		return 1;
	}

} // end namespace mial

#endif