#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <time.h>
#include <unistd.h>
#include <string.h>


template <typename FLOAT>
class RandomWalk
{
  public:
	// ================= INITIALIZATION STUFF ================= 

	RandomWalk()
		: rnd_ForceNewSeeds_U(-1), rnd_ForceNewSeeds_G(-1), rnd_ForceNewSeeds_P(-1)
	{
		//initiate rnd number generators
		rnd_State_U=rnd_State_G=rnd_State_P=NULL;
		rnd_State_U=gsl_rng_alloc(gsl_rng_default);
		rnd_State_G=gsl_rng_alloc(gsl_rng_default);
		rnd_State_P=gsl_rng_alloc(gsl_rng_default);

		if ((!rnd_State_U) || (!rnd_State_G) || (!rnd_State_P))
			throw std::string("RandomWalk: Couldn't inititate random number generators.");

		unsigned long seed=-1 * (int)time(NULL) * (int)getpid();
		gsl_rng_set(rnd_State_U,seed+0);
		gsl_rng_set(rnd_State_G,seed+1);
		gsl_rng_set(rnd_State_P,seed+2);
		rnd_UseCounter_U=rnd_UseCounter_G=rnd_UseCounter_P=0;

		//initiate rnd walk parameters
		wlk_mode=isotropic;
		wlk_displacementBias_x=wlk_displacementBias_y=wlk_displacementBias_z=0.;
		wlk_persistenceInTicks=1.;      //disables persistence basically
		wlk_persistenceVariance=0.13;   //6 sigma apprx. PI/4
		wlk_heading=0.;
		wlk_displacement_x=wlk_displacement_y=wlk_displacement_z=0.;
	}

	~RandomWalk()
	{
		//clear rnd number generators
		if (rnd_State_U) { gsl_rng_free(rnd_State_U); rnd_State_U=NULL; }
		if (rnd_State_G) { gsl_rng_free(rnd_State_G); rnd_State_G=NULL; }
		if (rnd_State_P) { gsl_rng_free(rnd_State_P); rnd_State_P=NULL; }
	}

	// ================= RND WALKING STUFF ================= 

	/// Supported regimes/modes of random walking
	typedef enum
	{
		isotropic,	//no persistence
		isotropicWithBias,
		persistent,	//aka Velocity Jump Process
		persistentWithBias
	} WalkMode;


	///Get walking mode
	WalkMode GetWalkMode(void) const
	{ return(wlk_mode); }

	/*
	 * Set walking mode.
	 * Note: Some modes may require additional parameters to be set before first use.
	 */
	WalkMode SetWalkMode(const WalkMode newMode)
	{ wlk_mode=newMode; }


	///Get currently used bias 2D vector
	void GetBiasVector(FLOAT& x,FLOAT& y) const
	{ x=wlk_displacementBias_x; y=wlk_displacementBias_y; }

	///Get currently used bias 3D vector
	void GetBiasVector(FLOAT& x,FLOAT& y,FLOAT& z) const
	{ x=wlk_displacementBias_x; y=wlk_displacementBias_y; z=wlk_displacementBias_z; }

	/*
	 * Set bias vector to use.
	 * Applies only if _WithBias mode is used.
	 */
	void SetBiasVector(const FLOAT x,const FLOAT y,const FLOAT z=0.)
	{ wlk_displacementBias_x=x; wlk_displacementBias_y=y; wlk_displacementBias_z=z; }


	///Get currently used persistence time (given time resolution)
	FLOAT GetPersistenceTime(const FLOAT dt) const
	{ return(wlk_persistenceInTicks*dt); }

	///Get currently used persistence length (given avg. speed and time resolution)
	FLOAT GetPersistenceLength(const FLOAT avgSpeed,const FLOAT dt) const
	{ return(avgSpeed*wlk_persistenceInTicks*dt); }

	///Get currently used persistence as a number of simulation ticks
	FLOAT GetPersistenceInTicks(void) const
	{ return(wlk_persistenceInTicks); }

	/*
	 * Set persistence time (and time resolution).
	 * Applies only if persistent_ mode is used.
	 *
	 * Adjust the simulated time step (denoted as \e dt here) such that
	 * \e time / \e dt is a lot greater than 1. In other words,
	 * if persistence time is smaller than a simulated time step, it
	 * makes no sense to talk about persistance anymore as a "turning
	 * event" will happen everytime RandomWalk::DoOneStep() is called.
	 */
	void SetPersistenceTime(const FLOAT time,const FLOAT dt=1.)
	{ wlk_persistenceInTicks=time/dt; }

	/*
	 * Set persistence length (and avg. speed and time resolution).
	 * Applies only if persistent_ mode is used.
	 *
	 * Adjust the simulated time step (denoted as \e dt here) such that
	 * \e length / (\e avgSpeed * \e dt) is a lot greater than 1. In other words,
	 * if persistence time (\e length / \e avgSpeed) is smaller than a simulated
	 * time step, it makes no sense to talk about persistance anymore as a "turning
	 * event" will happen everytime RandomWalk::DoOneStep() is called.
	 */
	void SetPersistenceLength(const FLOAT length,const FLOAT avgSpeed,
	                          const FLOAT dt=1.)
	{ wlk_persistenceInTicks=length/(avgSpeed*dt); }

	///Get variance of the heading angle (in radians) during persistence phase
	FLOAT GetPersistenceHeadingVariance(void)
	{ return(wlk_persistenceVariance); }

	/*
	 * Set variance of the heading angle (in radians) during persistence phase.
	 * The variance should be small so that the trajectory resemble
	 * a confident heading towards (arbitrary) certain direction.
	 */
	void SetPersistenceHeadingVariance(const FLOAT variance)
	{ wlk_persistenceVariance=variance; }


	///Do one step of the random walk
	void DoOneStep(void)
	{
		/*
		//if persistence mode, chances are that we would skip the "turning event"
		if ( (wlk_mode == RandomWalk::persistent || wlk_mode == RandomWalk::persistentWithBias)
		  && (rnd_GetValue_U(0.,1.) > (1./wlk_persistenceInTicks)) )
		{
			//time has not come yet...: no turning event now,
			//just update the heading a bit
			newHeading=rnd_GetValue_G(wlk_heading,wlk_persistenceVariance);
			std::cout << " update: heading=" << newHeading << "\n";
		}
		*/

		float deltaHeading=0.;
		if (wlk_mode == RandomWalk::persistent || wlk_mode == RandomWalk::persistentWithBias)
		{
			//dependent turning
			deltaHeading=rnd_GetValue_G(0.,wlk_persistenceVariance);
		}
		else
		{
			//independent turning
			deltaHeading=rnd_GetValue_U(0,6.28318); //note: boundary may be limited/narrowed
		}

		wlk_heading+=deltaHeading;
		std::cout << "delta heading=" << deltaHeading << "\n";

		//update the (normalized) position displacement "vector"
		wlk_displacement_x=cos(wlk_heading);
		wlk_displacement_y=sin(wlk_heading);
		//wlk_displacement_z=TODO 3D

		if (wlk_mode == RandomWalk::isotropicWithBias || wlk_mode == RandomWalk::persistentWithBias)
		{
			wlk_displacement_x+=wlk_displacementBias_x;
			wlk_displacement_y+=wlk_displacementBias_y;
			wlk_displacement_z+=wlk_displacementBias_z;
		}
	}

	///Do one step of the random walk and return fresh new generated displacement
	void DoOneStep(FLOAT& x,FLOAT& y,FLOAT& z)
	{
		DoOneStep();
		x=wlk_displacement_x; y=wlk_displacement_y; z=wlk_displacement_z;
	}

	///Get the last 2D generated displacement; does no simulation (see RandomWalk::DoOneStep())
	void GetLastWalkedDisplacement(FLOAT& dx,FLOAT& dy) const
	{ dx=wlk_displacement_x; dy=wlk_displacement_y; }

	///Get the last 3D generated displacement; does no simulation (see RandomWalk::DoOneStep())
	void GetLastWalkedDisplacement(FLOAT& dx,FLOAT& dy,FLOAT& dz) const
	{ dx=wlk_displacement_x; dy=wlk_displacement_y; dz=wlk_displacement_z; }

	///Sets the current heading; should be used only at initialization
	void ResetHeading(FLOAT newHeading)
	{
		while (newHeading < 0.) newHeading+=6.28318;
		while (newHeading > 6.28318) newHeading-=6.28318;
		wlk_heading=newHeading;
	}


  protected:
	/// current walking regime/mode
	WalkMode wlk_mode;

	///Current bias vector (if _WithBias mode is used)
	FLOAT wlk_displacementBias_x,wlk_displacementBias_y,wlk_displacementBias_z;

	///Current persistence time, in terms of simulation ticks
	FLOAT wlk_persistenceInTicks;

	///Current persistence variance (in radians) during persistence phase
	FLOAT wlk_persistenceVariance;


	///Current heading (azimuth)(in radians) of this random walk //TODO 3D !!!
	FLOAT wlk_heading;

	///last position coordinate update
	FLOAT wlk_displacement_x,wlk_displacement_y,wlk_displacement_z;


	// ================= RND GENERATORS STUFF ================= 

	///helper random number generators: Flat/uniform distribution
	FLOAT rnd_GetValue_U(const FLOAT A, const FLOAT B)
	{
		if (rnd_UseCounter_U == rnd_ForceNewSeeds_U)
		{
			unsigned long seed=-1 * (int)time(NULL) * (int)getpid();
			gsl_rng_set(rnd_State_U,seed+0);
			rnd_UseCounter_U=0;
		}

		++rnd_UseCounter_U;
		return ( gsl_ran_flat(rnd_State_U, A,B) );
	}

	///helper random number generators: Gaussian distribution
	FLOAT rnd_GetValue_G(const FLOAT mean, const FLOAT sigma)
	{
		if (rnd_UseCounter_G == rnd_ForceNewSeeds_G)
		{
			unsigned long seed=-1 * (int)time(NULL) * (int)getpid();
			gsl_rng_set(rnd_State_G,seed+1);
			rnd_UseCounter_G=0;
		}

		++rnd_UseCounter_G;
		return ( gsl_ran_gaussian(rnd_State_G, sigma) + mean );
	}

	///helper random number generators: Poisson distribution
	FLOAT rnd_GetValue_P(const FLOAT mean)
	{
		if (rnd_UseCounter_P == rnd_ForceNewSeeds_P)
		{
			unsigned long seed=-1 * (int)time(NULL) * (int)getpid();
			gsl_rng_set(rnd_State_P,seed+2);
			rnd_UseCounter_P=0;
		}

		++rnd_UseCounter_P;
		return ( gsl_ran_poisson(rnd_State_P, mean) );
	}

	///helper random number generators: generator states
	gsl_rng *rnd_State_U, *rnd_State_G, *rnd_State_P;

	///helper random number generators: use counters (to enforce re-seeding)
	long rnd_UseCounter_U, rnd_UseCounter_G, rnd_UseCounter_P;

	///helper random number generators: how many rnd numbers before re-seeding
	const long rnd_ForceNewSeeds_U, rnd_ForceNewSeeds_G, rnd_ForceNewSeeds_P;
};
