/*

-----------------------------------------------------------------------------

This source file is part of OGRE

    (Object-oriented Graphics Rendering Engine)

For the latest info, see http://www.ogre3d.org/

Copyright (c) 2000-2014 Torus Knot Software Ltd

Permission is hereby granted, free of charge, to any person obtaining a copy

of this software and associated documentation files (the "Software"), to deal

in the Software without restriction, including without limitation the rights

to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

copies of the Software, and to permit persons to whom the Software is

furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in

all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

THE SOFTWARE.

-----------------------------------------------------------------------------

*/

#include "OgreStableHeaders.h"

namespace Ogre {

    Node::QueuedUpdates Node::msQueuedUpdates;

    //-----------------------------------------------------------------------

    Node::Node(const String& name)

        :mParent(0),

        mName(name),

        mNeedParentUpdate(false),

        mNeedChildUpdate(false),

        mParentNotified(false),

        mQueuedForUpdate(false),

        mInheritOrientation(true),

        mInheritScale(true),

        mCachedTransformOutOfDate(true),

        mOrientation(Quaternion::IDENTITY),

        mPosition(Vector3::ZERO),

        mScale(Vector3::UNIT_SCALE),

        mDerivedOrientation(Quaternion::IDENTITY),

        mDerivedPosition(Vector3::ZERO),

        mDerivedScale(Vector3::UNIT_SCALE),

        mInitialPosition(Vector3::ZERO),

        mInitialOrientation(Quaternion::IDENTITY),

        mInitialScale(Vector3::UNIT_SCALE),

        mListener(0)

    {

        needUpdate();

    }

    //-----------------------------------------------------------------------

    Node::~Node()

    {

        // Call listener (note, only called if there's something to do)

        if (mListener)

        {

            mListener->nodeDestroyed(this);

        }

        removeAllChildren();

        if(mParent)

            mParent->removeChild(this);

        if (mQueuedForUpdate)

        {

            // Erase from queued updates

            QueuedUpdates::iterator it =

                std::find(msQueuedUpdates.begin(), msQueuedUpdates.end(), this);

            assert(it != msQueuedUpdates.end());

            if (it != msQueuedUpdates.end())

            {

                // Optimised algorithm to erase an element from unordered vector.

                *it = msQueuedUpdates.back();

                msQueuedUpdates.pop_back();

            }

        }

    }

    //-----------------------------------------------------------------------

    void Node::setParent(Node* parent)

    {

        bool different = (parent != mParent);

        mParent = parent;

        // Request update from parent

        mParentNotified = false ;

        needUpdate();

        // Call listener (note, only called if there's something to do)

        if (mListener && different)

        {

            if (mParent)

                mListener->nodeAttached(this);

            else

                mListener->nodeDetached(this);

        }

    }

    //-----------------------------------------------------------------------

    const Affine3& Node::_getFullTransform(void) const

    {

        if (mCachedTransformOutOfDate)

        {

#if OGRE_NODE_INHERIT_TRANSFORM

            Affine3 tr;

            tr.makeTransform(mPosition, mScale, mOrientation);

            if(mParent == NULL)

            {

                mCachedTransform = tr;

            }

            else if(mInheritOrientation && mInheritScale) // everything is inherited

            {

                mCachedTransform = mParent->_getFullTransform() * tr;

            }

            else if(!mInheritOrientation && !mInheritScale) // only position is inherited

            {

                mCachedTransform = tr;

                mCachedTransform.setTrans(tr.getTrans() + mParent->_getFullTransform().getTrans());

            }

            else // shear is inherited together with orientation, controlled by mInheritOrientation

            {

                const Affine3& parentTr = mParent->_getFullTransform();

                Vector3 parentScale(

                    parentTr.transformDirection(Vector3::UNIT_X).length(),

                    parentTr.transformDirection(Vector3::UNIT_Y).length(),

                    parentTr.transformDirection(Vector3::UNIT_Z).length());

                assert(mInheritOrientation ^ mInheritScale);

                mCachedTransform = (mInheritOrientation ? Affine3::getScale(1.0f / parentScale)  * parentTr : Affine3::getScale(parentScale)) * tr;

            }

#else

            // Use derived values

            mCachedTransform.makeTransform(

                _getDerivedPosition(),

                _getDerivedScale(),

                _getDerivedOrientation());

#endif

            mCachedTransformOutOfDate = false;

        }

        return mCachedTransform;

    }

    //-----------------------------------------------------------------------

    void Node::_update(bool updateChildren, bool parentHasChanged)

    {

        // always clear information about parent notification

        mParentNotified = false;

        // See if we should process everyone

        if (mNeedParentUpdate || parentHasChanged)

        {

            // Update transforms from parent

            _updateFromParent();

        }

        if(updateChildren)

        {

            if (mNeedChildUpdate || parentHasChanged)

            {

                ChildNodeMap::iterator it, itend;

                itend = mChildren.end();

                for (it = mChildren.begin(); it != itend; ++it)

                {

                    Node* child = *it;

                    child->_update(true, true);

                }

            }

            else

            {

                // Just update selected children

                ChildUpdateSet::iterator it, itend;

                itend = mChildrenToUpdate.end();

                for(it = mChildrenToUpdate.begin(); it != itend; ++it)

                {

                    Node* child = *it;

                    child->_update(true, false);

                }

            }

            mChildrenToUpdate.clear();

            mNeedChildUpdate = false;

        }

    }

    //-----------------------------------------------------------------------

    void Node::_updateFromParent(void) const

    {

        updateFromParentImpl();

        // Call listener (note, this method only called if there's something to do)

        if (mListener)

        {

            mListener->nodeUpdated(this);

        }

    }

    //-----------------------------------------------------------------------

    void Node::updateFromParentImpl(void) const

    {

        mCachedTransformOutOfDate = true;

        if (mParent)

        {

#if OGRE_NODE_INHERIT_TRANSFORM

            // Decompose full transform to position, orientation and scale, shear is lost here.

            _getFullTransform().decomposition(mDerivedPosition, mDerivedScale, mDerivedOrientation);

#else

            // Update orientation

            const Quaternion& parentOrientation = mParent->_getDerivedOrientation();

            if (mInheritOrientation)

            {

                // Combine orientation with that of parent

                mDerivedOrientation = parentOrientation * mOrientation;

            }

            else

            {

                // No inheritance

                mDerivedOrientation = mOrientation;

            }

            // Update scale

            const Vector3& parentScale = mParent->_getDerivedScale();

            if (mInheritScale)

            {

                // Scale own position by parent scale, NB just combine

                // as equivalent axes, no shearing

                mDerivedScale = parentScale * mScale;

            }

            else

            {

                // No inheritance

                mDerivedScale = mScale;

            }

            // Change position vector based on parent's orientation & scale

            mDerivedPosition = parentOrientation * (parentScale * mPosition);

            // Add altered position vector to parents

            mDerivedPosition += mParent->_getDerivedPosition();

#endif

        }

        else

        {

            // Root node, no parent

            mDerivedOrientation = mOrientation;

            mDerivedPosition = mPosition;

            mDerivedScale = mScale;

        }

        mNeedParentUpdate = false;

    }

    //-----------------------------------------------------------------------

    Node* Node::createChild(const Vector3& inTranslate, const Quaternion& inRotate)

    {

        Node* newNode = createChildImpl();

        newNode->setPosition(inTranslate);

        newNode->setOrientation(inRotate);

        this->addChild(newNode);

        return newNode;

    }

    //-----------------------------------------------------------------------

    Node* Node::createChild(const String& name, const Vector3& inTranslate, const Quaternion& inRotate)

    {

        OgreAssert(!name.empty(), "");

        Node* newNode = createChildImpl(name);

        newNode->setPosition(inTranslate);

        newNode->setOrientation(inRotate);

        this->addChild(newNode);

        return newNode;

    }

    //-----------------------------------------------------------------------

    void Node::addChild(Node* child)

    {

        if (child->mParent)

        {

            OGRE_EXCEPT(Exception::ERR_INVALIDPARAMS,

                "Node '" + child->getName() + "' already was a child of '" +

                child->mParent->getName() + "'.",

                "Node::addChild");

        }

        mChildren.push_back(child);

        child->setParent(this);

    }

    //-----------------------------------------------------------------------

    Node* Node::getChild(unsigned short index) const

    {

        if( index < mChildren.size() )

        {

            return mChildren[index];

        }

        else

            return NULL;

    }

    //-----------------------------------------------------------------------

    Node* Node::removeChild(unsigned short index)

    {

        OgreAssert(index < mChildren.size(), "");

        ChildNodeMap::iterator i = mChildren.begin();

        i += index;

        Node* ret = *i;

        // cancel any pending update

        cancelUpdate(ret);

        std::swap(*i, mChildren.back());

        mChildren.pop_back();

        ret->setParent(NULL);

        return ret;

    }

    //-----------------------------------------------------------------------

    Node* Node::removeChild(Node* child)

    {

        if (child)

        {

            ChildNodeMap::iterator i = std::find(mChildren.begin(), mChildren.end(), child);

            if(i != mChildren.end() && *i == child)

            {

                // cancel any pending update

                cancelUpdate(child);

                std::swap(*i, mChildren.back());

                mChildren.pop_back();

                child->setParent(NULL);

            }

        }

        return child;

    }

    //-----------------------------------------------------------------------

    void Node::setOrientation( const Quaternion & q )

    {

#ifndef OGRE_FAST_MATH

        OgreAssertDbg(!q.isNaN(), "Invalid orientation supplied as parameter");

#endif

        mOrientation = q;

        mOrientation.normalise();

        needUpdate();

    }

    //-----------------------------------------------------------------------

    void Node::setOrientation( Real w, Real x, Real y, Real z)

    {

        setOrientation(Quaternion(w, x, y, z));

    }

    //-----------------------------------------------------------------------

    void Node::resetOrientation(void)

    {

        mOrientation = Quaternion::IDENTITY;

        needUpdate();

    }

    //-----------------------------------------------------------------------

    void Node::setPosition(const Vector3& pos)

    {

#ifndef OGRE_FAST_MATH

        assert(!pos.isNaN() && "Invalid vector supplied as parameter");

#endif

        mPosition = pos;

        needUpdate();

    }

    //-----------------------------------------------------------------------

    Matrix3 Node::getLocalAxes(void) const

    {

        Matrix3 ret;

        mOrientation.ToRotationMatrix(ret);

        return ret;

    }

    //-----------------------------------------------------------------------

    void Node::translate(const Vector3& d, TransformSpace relativeTo)

    {

        switch(relativeTo)

        {

        case TS_LOCAL:

            // position is relative to parent so transform downwards

            mPosition += mOrientation * d;

            break;

        case TS_WORLD:

            // position is relative to parent so transform upwards

            if (mParent)

            {

                mPosition += mParent->convertWorldToLocalDirection(d, true);

            }

            else

            {

                mPosition += d;

            }

            break;

        case TS_PARENT:

            mPosition += d;

            break;

        }

        needUpdate();

    }

    //-----------------------------------------------------------------------

    void Node::rotate(const Quaternion& q, TransformSpace relativeTo)

    {

        switch(relativeTo)

        {

        case TS_PARENT:

            // Rotations are normally relative to local axes, transform up

            mOrientation = q * mOrientation;

            break;

        case TS_WORLD:

            // Rotations are normally relative to local axes, transform up

            mOrientation = mOrientation * _getDerivedOrientation().Inverse()

                * q * _getDerivedOrientation();

            break;

        case TS_LOCAL:

            // Note the order of the mult, i.e. q comes after

            mOrientation = mOrientation * q;

            break;

        }

        // Normalise quaternion to avoid drift

        mOrientation.normalise();

        needUpdate();

    }

    //-----------------------------------------------------------------------

    void Node::_setDerivedPosition( const Vector3& pos )

    {

        //find where the node would end up in parent's local space

        if(mParent)

            setPosition( mParent->convertWorldToLocalPosition( pos ) );

    }

    //-----------------------------------------------------------------------

    void Node::_setDerivedOrientation( const Quaternion& q )

    {

        //find where the node would end up in parent's local space

        if(mParent)

            setOrientation( mParent->convertWorldToLocalOrientation( q ) );

    }

    //-----------------------------------------------------------------------

    const Quaternion & Node::_getDerivedOrientation(void) const

    {

        if (mNeedParentUpdate)

        {

            _updateFromParent();

        }

        return mDerivedOrientation;

    }

    //-----------------------------------------------------------------------

    const Vector3 & Node::_getDerivedPosition(void) const

    {

        if (mNeedParentUpdate)

        {

            _updateFromParent();

        }

        return mDerivedPosition;

    }

    //-----------------------------------------------------------------------

    const Vector3 & Node::_getDerivedScale(void) const

    {

        if (mNeedParentUpdate)

        {

            _updateFromParent();

        }

        return mDerivedScale;

    }

    //-----------------------------------------------------------------------

    Vector3 Node::convertWorldToLocalPosition( const Vector3 &worldPos )

    {

        if (mNeedParentUpdate)

        {

            _updateFromParent();

        }

#if OGRE_NODE_INHERIT_TRANSFORM

        return _getFullTransform().inverse() * worldPos;

#else

        return mDerivedOrientation.Inverse() * (worldPos - mDerivedPosition) / mDerivedScale;

#endif

    }

    //-----------------------------------------------------------------------

    Vector3 Node::convertLocalToWorldPosition( const Vector3 &localPos )

    {

        if (mNeedParentUpdate)

        {

            _updateFromParent();

        }

        return _getFullTransform() * localPos;

    }

    //-----------------------------------------------------------------------

    Vector3 Node::convertWorldToLocalDirection( const Vector3 &worldDir, bool useScale )

    {

        if (mNeedParentUpdate)

        {

            _updateFromParent();

        }

        return useScale ? 

#if OGRE_NODE_INHERIT_TRANSFORM

            _getFullTransform().inverseAffine().transformDirectionAffine(worldDir) :

            mDerivedOrientation.Inverse() * worldDir;

#else

            mDerivedOrientation.Inverse() * worldDir / mDerivedScale :

            mDerivedOrientation.Inverse() * worldDir;

#endif

    }

    //-----------------------------------------------------------------------

    Vector3 Node::convertLocalToWorldDirection( const Vector3 &localDir, bool useScale )

    {

        if (mNeedParentUpdate)

        {

            _updateFromParent();

        }

        return useScale ? _getFullTransform().linear() * localDir : mDerivedOrientation * localDir;

    }

    //-----------------------------------------------------------------------

    Quaternion Node::convertWorldToLocalOrientation( const Quaternion &worldOrientation )

    {

        if (mNeedParentUpdate)

        {

            _updateFromParent();

        }

        return mDerivedOrientation.Inverse() * worldOrientation;

    }

    //-----------------------------------------------------------------------

    Quaternion Node::convertLocalToWorldOrientation( const Quaternion &localOrientation )

    {

        if (mNeedParentUpdate)

        {

            _updateFromParent();

        }

        return mDerivedOrientation * localOrientation;

    }

    //-----------------------------------------------------------------------

    void Node::removeAllChildren(void)

    {

        for (auto *c : mChildren)

        {

            c->setParent(0);

        }

        mChildren.clear();

        mChildrenToUpdate.clear();

    }

    //-----------------------------------------------------------------------

    void Node::setScale(const Vector3& inScale)

    {

#ifndef OGRE_FAST_MATH

        assert(!inScale.isNaN() && "Invalid vector supplied as parameter");

#endif

        mScale = inScale;

        needUpdate();

    }

    //-----------------------------------------------------------------------

    void Node::setInheritOrientation(bool inherit)

    {

        mInheritOrientation = inherit;

        needUpdate();

    }

    //-----------------------------------------------------------------------

    void Node::setInheritScale(bool inherit)

    {

        mInheritScale = inherit;

        needUpdate();

    }

    //-----------------------------------------------------------------------

    void Node::scale(const Vector3& inScale)

    {

        mScale = mScale * inScale;

        needUpdate();

    }

    //-----------------------------------------------------------------------

    void Node::scale(Real x, Real y, Real z)

    {

        mScale.x *= x;

        mScale.y *= y;

        mScale.z *= z;

        needUpdate();

    }

    //-----------------------------------------------------------------------

    void Node::setInitialState(void)

    {

        mInitialPosition = mPosition;

        mInitialOrientation = mOrientation;

        mInitialScale = mScale;

    }

    //-----------------------------------------------------------------------

    void Node::resetToInitialState(void)

    {

        mPosition = mInitialPosition;

        mOrientation = mInitialOrientation;

        mScale = mInitialScale;

        needUpdate();

    }

    //-----------------------------------------------------------------------

    struct NodeNameExists {

        const String& name;

        bool operator()(const Node* mo) {

            return mo->getName() == name;

        }

    };

    Node* Node::getChild(const String& name) const

    {

        NodeNameExists pred = {name};

        ChildNodeMap::const_iterator i = std::find_if(mChildren.begin(), mChildren.end(), pred);

        if (i == mChildren.end())

        {

            OGRE_EXCEPT(Exception::ERR_ITEM_NOT_FOUND, "Child node named " + name +

                " does not exist.", "Node::getChild");

        }

        return *i;

    }

    //-----------------------------------------------------------------------

    Node* Node::removeChild(const String& name)

    {

        OgreAssert(!name.empty(), "");

        NodeNameExists pred = {name};

        ChildNodeMap::iterator i = std::find_if(mChildren.begin(), mChildren.end(), pred);

        if (i == mChildren.end())

        {

            OGRE_EXCEPT(Exception::ERR_ITEM_NOT_FOUND, "Child node named " + name +

                " does not exist.", "Node::removeChild");

        }

        Node* ret = *i;

        // Cancel any pending update

        cancelUpdate(ret);

        std::swap(*i, mChildren.back());

        mChildren.pop_back();

        ret->setParent(NULL);

        return ret;

    }

    //-----------------------------------------------------------------------

    Node::ChildNodeIterator Node::getChildIterator(void)

    {

        return ChildNodeIterator(mChildren.begin(), mChildren.end());

    }

    //-----------------------------------------------------------------------

    Node::ConstChildNodeIterator Node::getChildIterator(void) const

    {

        return ConstChildNodeIterator(mChildren.begin(), mChildren.end());

    }

    //-----------------------------------------------------------------------

    Real Node::getSquaredViewDepth(const Camera* cam) const

    {

        Vector3 diff = _getDerivedPosition() - cam->getDerivedPosition();

        Vector3 zAxis = cam->getDerivedDirection();

        // NB use squared length to avoid square root

        return cam->getSortMode() == SM_DISTANCE ? diff.squaredLength() : Math::Sqr(zAxis.dotProduct(diff));

    }

    //-----------------------------------------------------------------------

    void Node::needUpdate(bool forceParentUpdate)

    {

        mNeedParentUpdate = true;

        mNeedChildUpdate = true;

        mCachedTransformOutOfDate = true;

        // Make sure we're not root and parent hasn't been notified before

        if (mParent && (!mParentNotified || forceParentUpdate))

        {

            mParent->requestUpdate(this, forceParentUpdate);

            mParentNotified = true ;

        }

        // all children will be updated

        mChildrenToUpdate.clear();

    }

    //-----------------------------------------------------------------------

    void Node::requestUpdate(Node* child, bool forceParentUpdate)

    {

        // If we're already going to update everything this doesn't matter

        if (mNeedChildUpdate)

        {

            return;

        }

        mChildrenToUpdate.insert(child);

        // Request selective update of me, if we didn't do it before

        if (mParent && (!mParentNotified || forceParentUpdate))

        {

            mParent->requestUpdate(this, forceParentUpdate);

            mParentNotified = true ;

        }

    }

    //-----------------------------------------------------------------------

    void Node::cancelUpdate(Node* child)

    {

        mChildrenToUpdate.erase(child);

        // Propagate this up if we're done

        if (mChildrenToUpdate.empty() && mParent && !mNeedChildUpdate)

        {

            mParent->cancelUpdate(this);

            mParentNotified = false ;

        }

    }

    //-----------------------------------------------------------------------

    void Node::queueNeedUpdate(Node* n)

    {

        // Don't queue the node more than once

        if (!n->mQueuedForUpdate)

        {

            n->mQueuedForUpdate = true;

            msQueuedUpdates.push_back(n);

        }

    }

    //-----------------------------------------------------------------------

    void Node::processQueuedUpdates(void)

    {

        for (auto *n : msQueuedUpdates)

        {

            // Update, and force parent update since chances are we've ended

            // up with some mixed state in there due to re-entrancy

            n->mQueuedForUpdate = false;

            n->needUpdate(true);

        }

        msQueuedUpdates.clear();

    }

}