/*==LICENSE==*

CyanWorlds.com Engine - MMOG client, server and tools
Copyright (C) 2011 Cyan Worlds, Inc.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.

Additional permissions under GNU GPL version 3 section 7

If you modify this Program, or any covered work, by linking or
combining it with any of RAD Game Tools Bink SDK, Autodesk 3ds Max SDK,
NVIDIA PhysX SDK, Microsoft DirectX SDK, OpenSSL library, Independent
JPEG Group JPEG library, Microsoft Windows Media SDK, or Apple QuickTime SDK
(or a modified version of those libraries),
containing parts covered by the terms of the Bink SDK EULA, 3ds Max EULA,
PhysX SDK EULA, DirectX SDK EULA, OpenSSL and SSLeay licenses, IJG
JPEG Library README, Windows Media SDK EULA, or QuickTime SDK EULA, the
licensors of this Program grant you additional
permission to convey the resulting work. Corresponding Source for a
non-source form of such a combination shall include the source code for
the parts of OpenSSL and IJG JPEG Library used as well as that of the covered
work.

You can contact Cyan Worlds, Inc. by email legal@cyan.com
 or by snail mail at:
      Cyan Worlds, Inc.
      14617 N Newport Hwy
      Mead, WA   99021

*==LICENSE==*/
#ifndef OBJECT_FLOCKER_H
#define OBJECT_FLOCKER_H

#include "../pnModifier/plSingleModifier.h"

class hsStream;
class hsResMgr;
class plRandom;
class pfObjectFlocker;

// Database tokens for our prox database
template <class T>
class pfTokenForProximityDatabase
{
public:
	virtual ~pfTokenForProximityDatabase() {}

	// call this when your position changes
	virtual void UpdateWithNewPosition(const hsPoint3 &newPos) = 0;

	// find all close-by objects (determined by center and radius)
	virtual void FindNeighbors(const hsPoint3 &center, const float radius, std::vector<T> &results) = 0;
};

// A basic prox database (might need to be optimized in the future)
template <class T>
class pfBasicProximityDatabase
{
public:
	class tokenType;
	typedef std::vector<tokenType*> tokenVector;
	typedef typename tokenVector::const_iterator tokenIterator;

	// "token" to represent objects stored in the database
	class tokenType: public pfTokenForProximityDatabase<T>
	{
	private:
		tokenVector& fTokens;
		T fParent;
		hsPoint3 fPosition;

	public:
		// constructor
		tokenType(T parentObject, tokenVector& tokens) : fParent(parentObject), fTokens(tokens)
		{
			fTokens.push_back(this);
		}

		// destructor
		virtual ~tokenType()
		{
			// remove this token from the database's vector
			fTokens.erase(std::find(fTokens.begin(), fTokens.end(), this));
		}

		// call this when your position changes
		void UpdateWithNewPosition(const hsPoint3 &newPosition) {fPosition = newPosition;}

		// find all close-by objects (determined by center and radius)
		void FindNeighbors(const hsPoint3 &center, const float radius, std::vector<T> & results)
		{
			// take the slow way, loop and check every one
			const float radiusSquared = radius * radius;
			for (tokenIterator i = fTokens.begin(); i != fTokens.end(); i++)
			{
				const hsVector3 offset(&center, &((**i).fPosition));
				const float distanceSquared = offset.MagnitudeSquared();

				// push onto result vector when within given radius
				if (distanceSquared < radiusSquared)
					results.push_back((**i).fParent);
			}
		}
	};

private:
	// STL vector containing all tokens in database
	tokenVector fGroup;

public:
	// constructor
	pfBasicProximityDatabase(void) {}

	// destructor
	virtual ~pfBasicProximityDatabase() {}

	// allocate a token to represent a given client object in this database
	tokenType *MakeToken(T parentObject) {return TRACKED_NEW tokenType(parentObject, fGroup);}

	// return the number of tokens currently in the database
	int Size(void) {return group.size();}
};

// A basic vehicle class that handles accelleration, braking, and turning
class pfVehicle
{
private:
	hsPoint3	fPos; // position in meters
	hsPoint3	fLastPos; // the last position we had
	hsPoint3	fSmoothedPosition;

	hsVector3	fVel; // velocity in meters/second
	hsVector3	fSmoothedAcceleration;

	hsVector3	fForward; // forward vector (unit length)
	hsVector3	fLastForward; // the last forward vector we had
	hsVector3	fSide; // side vector (unit length)
	hsVector3	fUp; // up vector (unit length)

	float		fSpeed; // speed (length of velocity vector)
	float		fMass; // mass of the object (defaults to 1)
	float		fMaxForce; // the maximum steering force that can be applied
	float		fMaxSpeed; // the maximum speed of this vehicle

	float		fCurvature;
	float		fSmoothedCurvature;

	float		fRadius;

	// measure the path curvature (1/turning radius), maintain smoothed version
	void IMeasurePathCurvature(const float elapsedTime);
public:
	pfVehicle() {Reset();}
	virtual ~pfVehicle() {}

	void Reset();

	// get/set attributes
	float Mass() const {return fMass;}
	float SetMass(float m) {return fMass = m;}

	hsVector3 Forward() const {return fForward;}
	hsVector3 SetForward(hsVector3 forward) {return fForward = forward;}
	hsVector3 Side() const {return fSide;}
	hsVector3 SetSide(hsVector3 side) {return fSide = side;}
	hsVector3 Up() const {return fUp;}
	hsVector3 SetUp(hsVector3 up) {return fUp = up;}

	hsPoint3 Position() const {return fPos;}
	hsPoint3 SetPosition(hsPoint3 pos) {return fPos = pos;}

	hsVector3 Velocity() const {return Forward() * fSpeed;}

	float Speed() const {return fSpeed;}
	float SetSpeed(float speed) {return fSpeed = speed;}

	float MaxForce() const {return fMaxForce;}
	float SetMaxForce(float maxForce) {return fMaxForce = maxForce;}

	float MaxSpeed() const {return fMaxSpeed;}
	float SetMaxSpeed(float maxSpeed) {return fMaxSpeed = maxSpeed;}

	float Curvature() const {return fCurvature;}

	float SmoothedCurvature() {return fSmoothedCurvature;}
	float ResetSmoothedCurvature(float value = 0);

	hsVector3 SmoothedAcceleration() {return fSmoothedAcceleration;}
	hsVector3 ResetSmoothedAcceleration(const hsVector3 &value = hsVector3(0,0,0));

	hsPoint3 SmoothedPosition() {return fSmoothedPosition;}
	hsPoint3 ResetSmoothedPosition(const hsPoint3 &value = hsPoint3(0,0,0));

	float Radius() const {return fRadius;}
	float SetRadius(float radius) {return fRadius = radius;}

	// Basic geometry functions

	// Reset local space to identity
	void ResetLocalSpace();

	// Set the side vector to a normalized cross product of forward and up
	void SetUnitSideFromForwardAndUp();

	// Regenerate orthonormal basis vectors given a new forward vector (unit length)
	void RegenerateOrthonormalBasisUF(const hsVector3 &newUnitForward);

	// If the new forward is NOT known to have unit length
	void RegenerateOrthonormalBasis(const hsVector3 &newForward)
	{hsVector3 temp = newForward; temp.Normalize(); RegenerateOrthonormalBasisUF(temp);}

	// For supplying both a new forward, and a new up
	void RegenerateOrthonormalBasis(const hsVector3 &newForward, const hsVector3 &newUp)
	{fUp = newUp; RegenerateOrthonormalBasis(newForward);}

	// Keep forward parallel to velocity, change up as little as possible
	virtual void RegenerateLocalSpace(const hsVector3 &newVelocity, const float elapsedTime);

	// Keep forward parallel to velocity, but "bank" the up vector
	void RegenerateLocalSpaceForBanking(const hsVector3 &newVelocity, const float elapsedTime);

	// Vehicle physics functions

	// apply a steering force to our momentum and adjust our
	// orientation to match our velocity vector
	void ApplySteeringForce(const hsVector3 &force, const float deltaTime);

	// adjust the steering force passed to ApplySteeringForce (so sub-classes can refine)
	// by default, we won't allow backward-facing steering at a low speed
	virtual hsVector3 AdjustRawSteeringForce(const hsVector3 &force, const float deltaTime);

	// apply a braking force
	void ApplyBrakingForce(const float rate, const float deltaTime);

	// predict the position of the vehicle (assumes constant velocity)
	hsPoint3 PredictFuturePosition(const float predictionTime);
};

// A goal object, basically keeps track of a scene object so we can get velocity from it
class pfBoidGoal
{
private:
	hsPoint3	fLastPos;
	hsPoint3	fCurPos;
	hsVector3	fForward;
	float		fSpeed; // in meters/sec
	hsBool		fHasLastPos; // does the last position make sense?
public:
	pfBoidGoal();
	~pfBoidGoal() {}

	void Update(plSceneObject *goal, float deltaTime);

	hsPoint3 Position() const {return fCurPos;}
	float Speed() const {return fSpeed;}
	hsVector3 Forward() const {return fForward;}
	hsPoint3 PredictFuturePosition(const float predictionTime);
};

typedef pfTokenForProximityDatabase<pfVehicle*> pfProximityToken;
typedef pfBasicProximityDatabase<pfVehicle*> pfProximityDatabase;

// The actual "flocking following" (not really a boid, but whatever)
class pfBoid: public pfVehicle
{
private:
	plKey		fObjKey;

	float		fWanderSide;
	float		fWanderUp;

	float		fGoalWeight;
	float		fRandomWeight;

	float		fSeparationRadius;
	float		fSeparationAngle;
	float		fSeparationWeight;

	float		fCohesionRadius;
	float		fCohesionAngle;
	float		fCohesionWeight;

	pfProximityToken* fProximityToken;
	std::vector<pfVehicle*> fNeighbors;

	// Set our flocking settings to default
	void IFlockDefaults();

	// Setup our prox database token
	void ISetupToken(pfProximityDatabase &pd);

	// Are we in the neighborhood of another boid?
	hsBool IInBoidNeighborhood(const pfVehicle &other, const float minDistance, const float maxDistance, const float cosMaxAngle);
	// Wander steering
	hsVector3 ISteerForWander(float timeDelta);
	// Seek the target point
	hsVector3 ISteerForSeek(const hsPoint3 &target);
	// Steer the boid toward our goal
	hsVector3 ISteerToGoal(pfBoidGoal &goal, float maxPredictionTime);
	// Steer to keep separation
	hsVector3 ISteerForSeparation(const float maxDistance, const float cosMaxAngle, const std::vector<pfVehicle*> &flock);
	// Steer to keep the flock together
	hsVector3 ISteerForCohesion(const float maxDistance, const float cosMaxAngle, const std::vector<pfVehicle*> &flock);

public:
	pfObjectFlocker *fFlockerPtr;

	pfBoid(pfProximityDatabase &pd, pfObjectFlocker *flocker, plKey &key);
	pfBoid(pfProximityDatabase &pd, pfObjectFlocker *flocker, plKey &key, hsPoint3 &pos);
	pfBoid(pfProximityDatabase &pd, pfObjectFlocker *flocker, plKey &key, hsPoint3 &pos, float speed, hsVector3 &forward, hsVector3 &side, hsVector3 &up);
	virtual ~pfBoid();

	// Get/set functions
	float GoalWeight() const {return fGoalWeight;}
	float SetGoalWeight(float goalWeight) {return fGoalWeight = goalWeight;}
	float WanderWeight() const {return fRandomWeight;}
	float SetWanderWeight(float wanderWeight) {return fRandomWeight = wanderWeight;}

	float SeparationWeight() const {return fSeparationWeight;}
	float SetSeparationWeight(float weight) {return fSeparationWeight = weight;}
	float SeparationRadius() const {return fSeparationRadius;}
	float SetSeparationRadius(float radius) {return fSeparationRadius = radius;}

	float CohesionWeight() const {return fCohesionWeight;}
	float SetCohesionWeight(float weight) {return fCohesionWeight = weight;}
	float CohesionRadius() const {return fCohesionRadius;}
	float SetCohesionRadius(float radius) {return fCohesionRadius = radius;}

	// Update the boid's data based on the goal and time delta
	void Update(pfBoidGoal &goal, float deltaTime);
	plKey &GetKey() {return fObjKey;}

	// We're redirecting this to the "banking" function
	virtual void RegenerateLocalSpace(const hsVector3 &newVelocity, const float elapsedTime);
};

class pfFlock
{
private:
	std::vector<pfBoid*>	fBoids;
	pfBoidGoal				fBoidGoal;
	pfProximityDatabase		*fDatabase;

	// global values so when we add a boid we can set it's parameters
	float fGoalWeight, fRandomWeight;
	float fSeparationWeight, fSeparationRadius;
	float fCohesionWeight, fCohesionRadius;
	float fMaxForce; // max steering force
	float fMaxSpeed, fMinSpeed;
public:
	pfFlock();
	~pfFlock();

	// Get/set functions (affect the whole flock, and any new boids added)
	float GoalWeight() const {return fGoalWeight;}
	void SetGoalWeight(float goalWeight);
	float WanderWeight() const {return fRandomWeight;}
	void SetWanderWeight(float wanderWeight);

	float SeparationWeight() const {return fSeparationWeight;}
	void SetSeparationWeight(float weight);
	float SeparationRadius() const {return fSeparationRadius;}
	void SetSeparationRadius(float radius);

	float CohesionWeight() const {return fCohesionWeight;}
	void SetCohesionWeight(float weight);
	float CohesionRadius() const {return fCohesionRadius;}
	void SetCohesionRadius(float radius);

	float MaxForce() const {return fMaxForce;}
	void SetMaxForce(float force);
	float MaxSpeed() const {return fMaxSpeed;}
	void SetMaxSpeed(float speed);
	float MinSpeed() const {return fMinSpeed;}
	void SetMinSpeed(float minSpeed);

	// setup/run functions
	void AddBoid(pfObjectFlocker *flocker, plKey &key, hsPoint3 &pos);
	void Update(plSceneObject *goal, float deltaTime);
	pfBoid *GetBoid(int i);

	friend class pfObjectFlocker;
};

class pfObjectFlocker : public plSingleModifier
{
public:
	pfObjectFlocker();
	~pfObjectFlocker();

	CLASSNAME_REGISTER( pfObjectFlocker );
	GETINTERFACE_ANY( pfObjectFlocker, plSingleModifier );

	virtual void SetTarget(plSceneObject* so);
	virtual hsBool MsgReceive(plMessage* msg);

	virtual void Read(hsStream* stream, hsResMgr* mgr);
	virtual void Write(hsStream* stream, hsResMgr* mgr);

	void SetNumBoids(UInt8 val);
	void SetBoidKey(plKey key) { fBoidKey = key; }

	float GoalWeight() const {return fFlock.GoalWeight();}
	void SetGoalWeight(float goalWeight) {fFlock.SetGoalWeight(goalWeight);}
	float WanderWeight() const {return fFlock.WanderWeight();}
	void SetWanderWeight(float wanderWeight) {fFlock.SetWanderWeight(wanderWeight);}

	float SeparationWeight() const {return fFlock.SeparationWeight();}
	void SetSeparationWeight(float weight) {fFlock.SetSeparationWeight(weight);}
	float SeparationRadius() const {return fFlock.SeparationRadius();}
	void SetSeparationRadius(float radius) {fFlock.SetSeparationRadius(radius);}

	float CohesionWeight() const {return fFlock.CohesionWeight();}
	void SetCohesionWeight(float weight) {fFlock.SetCohesionWeight(weight);}
	float CohesionRadius() const {return fFlock.CohesionRadius();}
	void SetCohesionRadius(float radius) {fFlock.SetCohesionRadius(radius);}

	float MaxForce() const {return fFlock.MaxForce();}
	void SetMaxForce(float force) {fFlock.SetMaxForce(force);}
	float MaxSpeed() const {return fFlock.MaxSpeed();}
	void SetMaxSpeed(float speed) {fFlock.SetMaxSpeed(speed);}
	float MinSpeed() const {return fFlock.MinSpeed();}
	void SetMinSpeed(float minSpeed) {fFlock.SetMinSpeed(minSpeed);}

	hsBool RandomizeAnimStart() const {return fRandomizeAnimationStart;}
	void SetRandomizeAnimStart(hsBool val) {fRandomizeAnimationStart = val;}
	hsBool UseTargetRotation() const {return fUseTargetRotation;}
	void SetUseTargetRotation(hsBool val) {fUseTargetRotation = val;}

protected:
	const static int fFileVersion; // so we don't have to update the global version number when we change

	pfFlock fFlock;
	int fNumBoids;
	plKey fBoidKey;

	hsBool fUseTargetRotation;
	hsBool fRandomizeAnimationStart;

	virtual hsBool IEval(double secs, hsScalar del, UInt32 dirty);
};

#endif