/***************************************************************************
*                                                                          *
*  Copyright (C) Golaem S.A.  All Rights Reserved.                         *
*                                                                          *
***************************************************************************/

#pragma once

#include "glmSDKAPI.h"
#include "glmMotionDefinitions.h"
#include "glmBlendTree.h"

#define	GLMABSTRACTBLENDNODE_MAXSUBNODESCOUNT 8

namespace glm
{
	/**
	* \brief Abstract reprenstation of a blend node, that can handle several (or no) sub-nodes
	*
	* A blend node is responsible for producing for 
	* 
	* \author dev@golaem.com
	*
	*/
	class GLM_SDK_API AbstractBlendNode
	{
	public:

		GLM_ABSTRACTCLASS_STAT_ID( glm::MemoryStatIds::Simulation);

		//-------------------------------------------------------------------------
		// Constraint world point
		//-------------------------------------------------------------------------
		class ConstraintChannel 
		{
		public:

			GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

				ConstraintChannel()
				: _nextWorldPosition(0, 0, 0), _lastWorldPosition(0, 0, 0), 
					_nextWorldNormal(0,0,1), _lastWorldNormal(0,0,1),
					_currentTime(0), _lastTime(FLT_MAX), _nextTime(FLT_MAX), _footSupportRatio(0.f), _index(0), _extremityBoneIndex(-1), _lastGroundCellId(-1), _nextGroundCellId(-1), _inUse(false), _supportingPhase(false)
			{
			}
			~ConstraintChannel(){}

			inline float getCurrentTimeRatio() const { return clamp(static_cast<float>((_currentTime - _lastTime) / (_nextTime - _lastTime + GLM_NUMERICAL_PRECISION_COARSE)), 0.f, 1.f); }
			inline Vector3 getBlendedWorldPosition(float ratio) const { return (_lastWorldPosition * (1.f - ratio) + _nextWorldPosition * ratio); }
			inline Vector3 getBlendedWorldNormal(float ratio) const { return (_lastWorldNormal * (1.f - ratio) + _nextWorldNormal * ratio); }

			Vector3 _nextWorldPosition;
			Vector3 _lastWorldPosition;
			
			Vector3 _nextWorldNormal; 
			Vector3 _lastWorldNormal;
			
			Time _currentTime;
			Time _lastTime;
			Time _nextTime;

			// alignment between animation local orientation and ground orientation
			float _footSupportRatio;

			int _index;
			int _extremityBoneIndex;
			int _lastGroundCellId;
			int _nextGroundCellId;

			bool _inUse;    //!<boolean set to true if the channel contains an activation sequence
			bool _supportingPhase;
		};
		
		//-------------------------------------------------------------------------
		// Sync channel
		//-------------------------------------------------------------------------
		struct SyncChannel
		{
			float _stanceNTimeInMotionCycle;	//!< The point in the motion cycle when the leg is standing most firmly and neutrally on the ground. In normalized time (% of the animation duration, between 0 and 1)
			float _phaseOffNTimeInStanceCycle;		//!< Foot off time, in normalized time in stance cycle (between 0 and 1, 0 being the stance time and 1 being the next stance time)
			float _phaseStrikeNTimeInStanceCycle;	//!< Foot strike time, in normalized time in stance cycle (between 0 and 1, 0 being the stance time and 1 being the next stance time)

			GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

			SyncChannel() :
				_stanceNTimeInMotionCycle(-1)
				,_phaseOffNTimeInStanceCycle(-1)
				,_phaseStrikeNTimeInStanceCycle(-1)
			{
			}

			//************************************************************
			//! @name Conversion methods
			/**
			* The following methods are helpers to convert different time values:
			*	- TimeInSeconds is the number of seconds since the beginning of the animation
			*	- NormalizedTimeInAnimationCycle is the percent of time since the beginning of the animation. 0 means the animation is played at 0%, 1 means it's played at 100%, 2 means it was already played 2 times (200%)
			*	- NormalizedTimeInStanceCycle is the percent of time since the stance time of the animation. 0 means the animation is exactly at stance time, 1 means it's at the next stance time (100%)
			*	- NormalizedLocalTimeInStanceCycle is like NormalizedTimeInStanceCycle but is clamped between 0 and 1
			*	- PhaseState reprensents different parts of the animation: 0 means after strike time, 1 means between off and strike (in support phase), 2 means before off time
			*	- NormalizedTimeInSupportCycle is the percent of time since in the current support phase : 0 means the support phase just started, 1 means the support phase is completelly played and the next support phase is gonna start (NormalizedTimeInSupportCycle is between 0 and 1 only)
			*/
			//************************************************************
			//@{
			inline float computeNormalizedLocalTimeInStanceCycleFromNormalizedTimeInStanceCycle(float* stanceCycleIndex, float normalizedTimeInStanceCycle) ;
			int computePhaseStateFromNormalizedLocalTimeInStanceCycle(float normalizedLocalTimeInStanceCycle) const { return (int)(normalizedLocalTimeInStanceCycle<_phaseOffNTimeInStanceCycle) + (int)(normalizedLocalTimeInStanceCycle<_phaseStrikeNTimeInStanceCycle); }	//phase state: 0 means after strike time, 1 means between off and strike (in support phase), 2 means before off time
			inline float computeNormalizedTimeInSupportCycleFromNormalizedTimeInStanceCycle(float normalizedLocalTimeInStanceCycle) const ;

			inline float computeNormalizedTimeInStanceCycleFromNormalizedTimeInSupportCycle(float normalizedTimeInSupportCycle, int phaseState) const ;

			inline void getPhaseBeginEndDuration(float& phaseBegin, float& phaseEnd, float& phaseDuration, int phaseState) const ;
			//@}
		};

		//-------------------------------------------------------------------------
		// Node sync data
		//-------------------------------------------------------------------------
		struct SyncTime
		{
			bool _externallySynchronized;
			float _externalCycleTime;

			GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

			SyncTime() : _externallySynchronized(false), _externalCycleTime( 0.f )
			{
			}
		};

		//************************************************************
		//! @name Constructors & destructor
		//************************************************************
		//@{
		AbstractBlendNode( int intermediatePosturesCount = 0 ) ;
		/**
		* \warning Destructor does not call finish, as it is already bone by containing tree
		*/
		virtual ~AbstractBlendNode() ;
		//@}

		//************************************************************
		//! @name Execution control
		//************************************************************
		//@{
		bool isInitialized() const ;

		virtual void init() = 0 ;
		virtual void init(int64_t entityIndex) = 0; // once per entity
		virtual void finish(int64_t entityIndex) = 0; // once per entity
		virtual void finish() = 0 ;

		virtual void connect() = 0 ; // once per entityType
		virtual void disconnect() = 0 ; // once per entityType
		/**
		 * \warning Node must have been initialized
		 */
		virtual void updateSync(int64_t entityIndex, glm::Time dt, SyncTime syncData[]) = 0;
		/**
		 * \warning Node must have been initialized
		 */
		virtual void updateTime(int64_t entityIndex, glm::Time dt, SyncTime syncData[]) = 0;
		/**
		 * \warning Node must have been initialized
		 */
		virtual void updateActiveBones(int64_t entityIndex, glm::Time dt) = 0;
		/**
		 * \warning Node must have been initialized
		 */
		virtual void updateFullPostureNeededFlag(int64_t entityIndex, glm::Time dt) = 0;
		/**
		 * \warning Node must have been initialized
		 */
		virtual void updatePosture(int64_t entityIndex, glm::Time dt) = 0;
		//@}

		//************************************************************
		//! @name Position/orientation accessors
		//************************************************************
		//@{
		virtual void setZupPelvisWorldPositionAndOrientation(int64_t entityIndex, const glm::Vector3& position, const glm::Quaternion& orientation, bool forcePosXY = false, bool forcePosZ = false, bool forceOri = false) = 0;

		virtual glm::Vector3 getZupPelvisWorldPosition(int64_t entityIndex) const = 0;
		virtual void setZupPelvisWorldPosition(int64_t entityIndex, const glm::Vector3& position, bool forcePosXY = false, bool forcePosZ = false) = 0;

		virtual glm::Quaternion getZupPelvisWorldOrientation(int64_t entityIndex) const = 0;
		virtual void setZupPelvisWorldOrientation(int64_t entityIndex, const glm::Quaternion& orientation, bool forceOri = false) = 0;

		virtual float getSpeed(int64_t entityIndex) const = 0;
		virtual glm::Vector3 getMovingDirection(int64_t entityIndex) const = 0;
		//@}

		//************************************************************
		//! @name Trajectory extrapolation system
		//************************************************************
		//@{
		/**
		* \brief Get an array of expected root positions in the future
		*/
		virtual void computeTrajectoryNormalizedDeltasPrediction(Vector3* retDeltaPositions, Quaternion* retDeltaOrientations, int64_t entityIndex, unsigned int predictionSampleCount, glm::Time predictionSampleDt) const;

		static void blendTrajectoryDeltasPrediction(Vector3* retDeltaPositions, Quaternion* retDeltaOrientations, unsigned int blendNodesCount, unsigned int trajectoryPointsCount, const float* blendingWeights, const Vector3* subNodesDeltaPositions, const Quaternion* subNodesDeltaOrientations, unsigned int subNodesPointsCount);
		static void trajectoryNormalizedDeltasPredictionToWorldTrajectory(Vector3* outTrajectoryPositions, Quaternion* outTrajectoryOrientations, const Vector3* inDeltaPositions, const Quaternion* inDeltaOrientations, unsigned int trajectoryPointsCount, const Vector3& pelvisWorldPos, const Quaternion& pelvisWorldOri, float denormalizationFactor);
		//@}

		//************************************************************
		//! @name Constraint system
		//************************************************************
		//@{
		/**
		* \brief Get an array of cosntraint channels
		* \note size of the array is _tree->_genericSpecificConverter->_converterMapping->getLimbBones().size()
		*/
		virtual const ConstraintChannel* const getConstraintChannels(int64_t entityIndex) const;
		//@}

		//************************************************************
		//! @name Synchronization system
		//************************************************************
		//@{
		virtual glm::Time getCycleDuration(int64_t entityIndex) const;

		/**
		* \brief Get an array of sync channels
		* \note size of the array is _tree->_genericSpecificConverter->_converterMapping->getLimbBones().size()
		*/
		virtual const SyncChannel* getSyncChannels(int64_t index) const;

		/**
		* \brief Set the per channel cycle time
		*/
		virtual void setChannelSyncTime(int64_t entityIndex, int channelIndex, bool externallySynchronized, float externalCycleTime) = 0;
		//@}

		//************************************************************
		//! @name Postures
		//************************************************************
		//@{
		inline int getIntermediatePosturesCount() const ;

		inline bool isFullPostureUpdateNeeded(int64_t entityIndex) const;
		virtual void setFullPostureUpdateNeeded(int64_t entityIndex, bool fullPostureUpdateNeeded);
		//@}

		//************************************************************
		//! @name Active Bones
		//************************************************************
		//@{
		inline const size_t getActiveBoneCount() const;
		/*
		* \note Active bones mask is stored as float for convenience (faster use of multiplication), values stored are always 0.0f (false) or 1.0f (true)
		*/
		inline const float* getActiveBones(int64_t entityIndex) const;
		inline float* touchActiveBones(int64_t entityIndex);
		//@}

		inline const size_t getActiveBlindDataCount() const;
		/**
		* \brief Active blend shapes
		* \note Active blend shapes mask is stored as float for convenience (faster use of multiplication), values stored are always 0.0f (false) or 1.0f (true)
		*/
		inline const float* getActiveBlindData(int64_t entityIndex) const;
		inline float* touchActiveBlindData(int64_t entityIndex);
		
		virtual void allocatePerEntityData(int64_t entityCount) = 0;
		virtual void deallocatePerEntityData()=0;

	protected:

		void allocateActiveBones(const size_t activeBoneCount);
		void deallocateActiveBones();
		void clearActiveBones();
		void clearActiveBones(int64_t entityIndex);
		void copyActiveBones(int64_t entityIndex, const size_t sourceActiveBoneCount, const float* sourceActiveBones);

		void allocateActiveBlindData(const size_t activeBlindDataCount);
		void deallocateActiveBlindData();
		void clearActiveBlindData();
		void clearActiveBlindData(int64_t entityIndex);
		void copyActiveBlindData(int64_t entityIndex, const size_t sourceActiveBlindDataCount, const float* sourceActiveBlindData);

		void allocateFullPostureUpdateFlag();
		void deallocateFullPostureUpdateFlag();

	public:
		//************************************************************
		//! @name Containenment
		//************************************************************
		//@{
		BlendTree* _tree ;
		int _id ;
		//@}

	protected:
		//************************************************************
		//! @name Execution control
		//************************************************************
		//@{
		bool _isInitialized ;
		//@}

		//************************************************************
		//! @name Active Bones
		//************************************************************
		//@{
		size_t _activeBoneCount;
		//@}

		/**
		* \brief Active blind data
		*/
		size_t _activeBlindDataCount;

	private:

		//************************************************************
		//! @name Postures
		//************************************************************
		//@{
		int _intermediatePosturesCount ;
		//@}

		glm::PODArray<float> _activeBones;	// perEntity data : activeBoneCount * nb Entities
		glm::PODArray<float> _activeBlindData; // perEntity data : _activeBlindDataCount * nb Entities
		glm::PODArray<bool> _needsFullPostureUpdate;	// perEntity data : nb Entities
	};

	//------------------------------------------------------------
	// computeNormalizedLocalTimeInStanceCycleFromNormalizedTimeInStanceCycle
	//------------------------------------------------------------
	inline float
		AbstractBlendNode::SyncChannel::computeNormalizedLocalTimeInStanceCycleFromNormalizedTimeInStanceCycle(float* stanceCycleIndex, float normalizedTimeInStanceCycle)
	{
		float localNormalizedTimeInStanceCycle = modff(normalizedTimeInStanceCycle, stanceCycleIndex);
		while(localNormalizedTimeInStanceCycle<0)
		{
			localNormalizedTimeInStanceCycle+=1;
			(*stanceCycleIndex)-=1;
		}
		return localNormalizedTimeInStanceCycle;
	}

	//------------------------------------------------------------
	// computeNormalizedTimeInSupportCycleFromNormalizedTimeInStanceCycle
	//------------------------------------------------------------
	inline float
		AbstractBlendNode::SyncChannel::computeNormalizedTimeInSupportCycleFromNormalizedTimeInStanceCycle(float normalizedLocalTimeInStanceCycle) const
	{
		GLM_DEBUG_ASSERT(normalizedLocalTimeInStanceCycle>=0 && normalizedLocalTimeInStanceCycle<=1); 
		float phaseBegin = 0.f ;
		float phaseEnd = 0.f ;
		float phaseDuration = 0.f ;
		getPhaseBeginEndDuration(phaseBegin, phaseEnd, phaseDuration, computePhaseStateFromNormalizedLocalTimeInStanceCycle(normalizedLocalTimeInStanceCycle));
		GLM_DEBUG_ASSERT(phaseDuration>0);	//this method should not be called when the phase duration is null
		return (normalizedLocalTimeInStanceCycle-phaseBegin)/phaseDuration;
	}

	//------------------------------------------------------------
	// computeNormalizedTimeInStanceCycleFromNormalizedTimeInSupportCycle
	//------------------------------------------------------------
	inline float
		AbstractBlendNode::SyncChannel::computeNormalizedTimeInStanceCycleFromNormalizedTimeInSupportCycle(float normalizedTimeInSupportCycle, int phaseState) const
	{
		float phaseBegin = 0.f ;
		float phaseEnd = 0.f ;
		float phaseDuration = 0.f ;
		getPhaseBeginEndDuration(phaseBegin, phaseEnd, phaseDuration, phaseState);
		GLM_DEBUG_ASSERT(phaseDuration>0);	//this method should not be called when the phase duration is null
		return phaseBegin+normalizedTimeInSupportCycle*phaseDuration;
	}

	//------------------------------------------------------------
	// getPhaseBeginEndDuration
	//------------------------------------------------------------
	inline void
		AbstractBlendNode::SyncChannel::getPhaseBeginEndDuration(float& phaseBegin, float& phaseEnd, float& phaseDuration, int phaseState) const
	{
		switch(phaseState)
		{
		case 0:
			{
				phaseBegin=_phaseStrikeNTimeInStanceCycle;
				phaseEnd=_phaseOffNTimeInStanceCycle+1;
			}
			break;
		case 2:
			{
				phaseBegin=_phaseStrikeNTimeInStanceCycle-1;
				phaseEnd=_phaseOffNTimeInStanceCycle;
			}
			break;
		case 1:
			{
				phaseBegin=_phaseOffNTimeInStanceCycle;
				phaseEnd=_phaseStrikeNTimeInStanceCycle;
			}
			break;
		}
		phaseDuration = phaseEnd - phaseBegin;
	}

	//------------------------------------------------------------
	inline bool 
		AbstractBlendNode::isInitialized() const
	{
		return _isInitialized ;
	}

	//------------------------------------------------------------
	inline void AbstractBlendNode::computeTrajectoryNormalizedDeltasPrediction(Vector3* retDeltaPositions, Quaternion* retDeltaOrientations, int64_t entityIndex, unsigned int predictionSampleCount, glm::Time predictionSampleDt) const
	{
		if (_tree->getSubNodesCount(_id) > 0)
		{
			AbstractBlendNode* sourceBN = _tree->touchNode(_tree->getSubNode(_tree->getFirstSubNodePos(_id)));
			if (sourceBN != NULL)
				sourceBN->computeTrajectoryNormalizedDeltasPrediction(retDeltaPositions, retDeltaOrientations, entityIndex, predictionSampleCount, predictionSampleDt);
		}
	}

	//------------------------------------------------------------
	inline const AbstractBlendNode::ConstraintChannel* const 
		AbstractBlendNode::getConstraintChannels(int64_t /*entityIndex*/) const
	{
		return NULL;
	}

	//------------------------------------------------------------
	inline glm::Time 
		AbstractBlendNode::getCycleDuration(int64_t /*entityIndex*/) const
	{
		return 0;
	}

	//------------------------------------------------------------
	inline const AbstractBlendNode::SyncChannel*
		AbstractBlendNode::getSyncChannels(int64_t /*entityIndex*/) const
	{
		return NULL;
	}

	//------------------------------------------------------------
	inline int
		AbstractBlendNode::getIntermediatePosturesCount() const
	{
		return _intermediatePosturesCount ;
	}

	//------------------------------------------------------------
	inline bool
		AbstractBlendNode::isFullPostureUpdateNeeded(int64_t entityIndex) const
	{
		return _needsFullPostureUpdate[entityIndex];
	}

	//------------------------------------------------------------
	inline const size_t
		AbstractBlendNode::getActiveBoneCount() const
	{
		return _activeBoneCount;
	}

	//------------------------------------------------------------
	inline const float* 
		AbstractBlendNode::getActiveBones(int64_t entityIndex) const
	{
		return &(_activeBones[_activeBoneCount * entityIndex]) ;
	}

	//------------------------------------------------------------
	inline const size_t
		AbstractBlendNode::getActiveBlindDataCount() const
	{
		return _activeBlindDataCount;
	}

	//------------------------------------------------------------
	inline const float* 
		AbstractBlendNode::getActiveBlindData(int64_t entityIndex) const
	{
		return &(_activeBlindData[_activeBlindDataCount*entityIndex]);
	}

	//------------------------------------------------------------
	inline float* 
		AbstractBlendNode::touchActiveBones(int64_t entityIndex)
	{
		return &(_activeBones[_activeBoneCount * entityIndex]) ;
	}

	//------------------------------------------------------------
	inline float* 
		AbstractBlendNode::touchActiveBlindData(int64_t entityIndex)
	{
		return &(_activeBlindData[_activeBlindDataCount*entityIndex]);
	}
}
