#ifndef _Beh_SearchForObject_txx
#define _Beh_SearchForObject_txx

#include "Beh_SearchForObject.h"

namespace mial {

	template<class Type, class TInputImage, int nDims>
	Beh_SearchForObject<Type,TInputImage,nDims>::Beh_SearchForObject()
		:Behavior<Type,nDims>("Beh_SearchForObject")
	{
		//Default constructor
		srand ( time(NULL) );
		avgIntensitySensor = AvgIntensitySensorType::New();
		translateBehavior = TranslateBehaviorType::New();
		scaleBehavior = ScaleBehaviorType::New();

		startTime =0;
		endTime =0;
		found = false;
		trans =true;
	}
	template<class Type, class TInputImage, int nDims>
	bool Beh_SearchForObject<Type,TInputImage,nDims>::run(typename Behavior<Type,nDims>::behaviorIn * i,std::stringstream *s)
	{

		typename behaviorIn::Pointer in;
		//TODO: Make sure the null works.
		if(i != NULL && s != NULL)
		{
			Error e;
			e.msg = "Only one of struct or stream input may be provided.";
			e.name = "Beh_SearchForObject";
			throw e;
		}
		else if(i != NULL)
			in = reinterpret_cast<behaviorIn *>(i); //Typecast the input to its desired form
		else if( s!= NULL)
		{
			in = behaviorIn::New();
			in->fillFromStream(*s);//in = &behaviorIn(*(reinterpret_cast<std::istream *>(s)));
		}
		else
		{
			Error e;
			e.msg = "Either struct or stream input must be provided.";
			e.name = "Beh_SearchForObject";
			throw e;
		}

		input = in;
		startTime = this->physLayer->getTime();
		endTime = startTime+in->duration;
		
		//TODO, make part of base class (set for all subbehaviors)
		this->translateBehavior->setPhysLayer(this->physLayer);
		scaleBehavior->setPhysLayer(this->physLayer);

		if(!(this->isFinished()))
		{
			//Get current location
			VectorType dir = in->translateLoc - geomLayer->getCentroid();
			dir.normalize();

			//Translate to new location
			//TODO: fix amplitude
			this->translate(dir);
		}
		else
		{
			if(scaleBehavior->isFinished())
				this->scale();
		}


		return false;
	}
	template<class Type, class TInputImage, int nDims>
	bool Beh_SearchForObject<Type,TInputImage,nDims>::update()
	{ 
		VectorType dir = VectorType(nDims);
		if(!this->isFinished())
		{//Keep searching
			if(!found)
			{
				if(trans && translateBehavior->isFinished())
				{
					for(int i=0;i<nDims;i++)
						dir.put(i,rand()-0.5);
					dir.normalize();
					this->translate(dir);
				}
			}
			else
			{
				if(trans)
				{
					std::cout << "Intensity level located." << std::endl;
					trans = false;
				}
				if(scaleBehavior->isFinished())
					this->scale();

			}
		}
		return false;
	}
	template<class Type, class TInputImage, int nDims>
	bool Beh_SearchForObject<Type,TInputImage,nDims>::isFinished()
	{
		//Check the current intensity
		typename AvgIntensitySensorType::sensorIn::Pointer avgInput = AvgIntensitySensorType::sensorIn::New();

		avgInput->imageIn = this->image;
		avgInput->geom = this->geomLayer;
		avgIntensitySensor->run(avgInput);
		typename AvgIntensitySensorType::sensorOut::Pointer output = (typename AvgIntensitySensorType::sensorOut *) (avgIntensitySensor->getOutput()).GetPointer();

		if( (output->avgIntensity >= input->intensityRequirement))
			found = true;
		else
			found = false;

		return found  && (this->physLayer->getTime()>endTime);
	}
	template<class Type, class TInputImage, int nDims>
	void Beh_SearchForObject<Type,TInputImage,nDims>::scale()
	{

		scaleBehavior->cleanUp();
		//Run the translate behavior
		std::stringstream scaleArgs;
		//TODO assign duration better
		scaleArgs << 10.0 << " ";
		scaleArgs << 1.025 << " ";
		scaleBehavior->run(NULL,&scaleArgs);

	}

	template<class Type, class TInputImage, int nDims>
	void Beh_SearchForObject<Type,TInputImage,nDims>::translate(VectorType &loc)
	{

		translateBehavior->cleanUp();
		//Run the translate behavior
		std::stringstream transArgs;
		//TODO assign duration better
		transArgs << 25.0 << " ";
		for(int i=0;i<nDims;i++)
		{
			transArgs << loc(i) << " ";
		}
		translateBehavior->run(NULL,&transArgs);

	}
	template<class Type, class TInputImage, int nDims>
	void Beh_SearchForObject<Type,TInputImage,nDims>::cleanUp()
	{
		startTime =0;
		endTime =0;
		found = false;
		trans = true;
	}
}//end namespace mial

#endif