/*==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 "hsGeometry3.h"

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

#include <algorithm>
#include <vector>

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) override { fPosition = newPosition; }

        // find all close-by objects (determined by center and radius)
        void FindNeighbors(const hsPoint3 &center, const float radius, std::vector<T> & results) override
        {
            // 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() {}

    // destructor
    virtual ~pfBasicProximityDatabase() {}

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

    // return the number of tokens currently in the database
    int Size() {return fGroup.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
    bool        fHasLastPos; // does the last position make sense?
public:
    pfBoidGoal()
        : fSpeed(0), fHasLastPos()
    { }
    virtual ~pfBoidGoal() = default;

    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?
    bool      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, const hsPoint3 &pos);
    pfBoid(pfProximityDatabase &pd, pfObjectFlocker *flocker, plKey key, const hsPoint3 &pos, float speed, const hsVector3 &forward, const hsVector3 &side, const 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
    void RegenerateLocalSpace(const hsVector3 &newVelocity, const float elapsedTime) override;
};

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 );

    void SetTarget(plSceneObject* so) override;
    bool MsgReceive(plMessage* msg) override;

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

    void SetNumBoids(uint8_t val);
    void SetBoidKey(plKey key) { fBoidKey = std::move(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);}

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

protected:
    constexpr static uint8_t fFileVersion = 1; // so we don't have to update the global version number when we change

    pfFlock fFlock;
    uint8_t fNumBoids;
    plKey fBoidKey;

    bool fUseTargetRotation;
    bool fRandomizeAnimationStart;

    bool IEval(double secs, float del, uint32_t dirty) override;
};

#endif
