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

#pragma once

#include "glmAbstractBlendNode.h"
#include "glmGenericAnimation.h"

namespace glm
{
	class GroundManager;
	class AbstractGround;
	/**
	 * \brief Blend node handling Ground Adaptation constraints
	 *
	 * \author dev@golaem.com
	 *
	 */
	class GLM_SDK_API GroundAdaptationBN : public AbstractBlendNode
	{
	public:	//structures
		
		//-------------------------------------------------------------------------
		struct GroundEvaluation
		{
		public:
			enum Value
			{
				SAMPLED_EVERY_FRAME = 0,
				INTERPOLATED_PHASE_CHANGE,
				FOOT_LOCKING,
				END
			};
		};
		
		//-------------------------------------------------------------------------
		struct GroundValue
		{
			GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

			GroundValue() : _currentDeltaPos(0,0,0), _currentGroundNormal(0,0,1), _index(-1), _supportingPhase(false)
			{
			}
			Vector3 _lastGroundWorldNormal;
			Vector3 _lastGroundWorldPos;
			Vector3 _nextGroundWorldNormal;
			Vector3 _nextGroundWorldPos;

			Vector3 _currentDeltaPos;
			Vector3 _currentGroundNormal;

			Vector3 _lockedAnklePos = Vector3(0, 0, FLT_MAX);    //to detect non-initialized values
			Quaternion _lockedAnkleOri = Quaternion::getIdentity();
			Array<Vector3> _lockedToesPos;
			Array<Quaternion> _lockedToesOri;

			int _index; // same index as in ConstraintChannel to track phase change
			bool _supportingPhase ;
		};

		//-------------------------------------------------------------------------
		class GroundAdaptationBNPerEntityData
		{
		public:

			GLM_CLASS_STAT_ID(glm::MemoryStatIds::Simulation);

			GroundAdaptationBNPerEntityData();
			~GroundAdaptationBNPerEntityData();

			Vector3 _previousPelvisGroundNormal;
			Vector3 _previousPelvisGroundPosition;
			Vector3 _lastFloorReference;
			Vector3 _lastFloorRefBarycenter;
			GroundCellId _lastFloorReferenceCellId;

			GroundCellId _previousPelvisGroundCellId;
			glm::Array<GroundValue> _groundValues;

			const void* _ragdollEntityComponent;
			AbstractBlendNode::ConstraintChannel* _constraintChannels;

			const AbstractGround* _mainGround;
			const AbstractGround* _physicsGround;	//will be used if not NULL

			bool _enabled;
			GroundEvaluation::Value _groundEvaluation;
			bool _hasFloorRef;
		};

	public:	//methods
		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

		GroundAdaptationBN();
		~GroundAdaptationBN();

		//************************************************************
		//! @name Execution control
		//************************************************************
		//@{
		void init();
		void init(int64_t entityIndex); // once per entity
		void finish(int64_t entityIndex); // once per entity
		void finish();

		void allocatePerEntityData(int64_t entityCount) override;
		void deallocatePerEntityData() override;

		void connect() override;

		void disconnect() override;

		/**
		 * \brief Update blend node (synchronization)
		 * \warning Node must have been initialized
		 */
		void updateSync(int64_t entityIndex, glm::Time dt, SyncTime syncData[]) override;
		/**
		 * \warning Node must have been initialized
		 */
		void updateTime(int64_t entityIndex, glm::Time dt, SyncTime syncData[]) override;

		virtual void updateActiveBones(int64_t entityIndex, glm::Time dt) override;
		virtual void updateFullPostureNeededFlag(int64_t entityIndex, glm::Time dt) override;

		/**
		 * \brief Update blend node (posture)
		 * \warning Node must have been initialized
		 */
		void updatePosture(int64_t entityIndex, glm::Time dt) override;
		//@}

		//************************************************************
		//! @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) override;

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

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

		virtual float getSpeed(int64_t entityIndex) const override;
		virtual glm::Vector3 getMovingDirection(int64_t entityIndex) const override;

		bool hasFloorReference(int64_t entityIndex) const;
		const Vector3& getlastFloorReference(int64_t entityIndex) const;
		const void getlastFloorRefCellIdAndBarycenter(int64_t entityIndex, GroundCellId& cellId, Vector3& barycenter) const;
		//@}

		//************************************************************
		//! @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 override;
		//@}

		//************************************************************
		//! @name Synchronization system
		//************************************************************
		//@{
		void setChannelSyncTime(int64_t entityIndex, int channelIndex, bool externallySynchronized, float externalCycleTime) override;
		//@}

		/**
		* \brief compute arc magnitude from 2 points on ground and their normal
		*/
		static float computeArcMagnitude( const Vector3& groundPositionPrev, const Vector3& groundPositionNext, const Vector3& groundNormalPrev, const Vector3& groundNormalNext ) ;

		/**
		* \brief enabled/disable 
		*/
		inline void setEnabled(int64_t entityIndex, bool enabled, GroundEvaluation::Value groundEvaluation);
		inline bool isEnabled(int64_t entityIndex) const;

		inline void setMainGround(int64_t entityIndex, const AbstractGround* ground);
		inline void setPhysicsGround(int64_t entityIndex, const AbstractGround* ground);

		inline void setRagdollEntityComponent(int64_t entityIndex, const void *ragdollEntityComponent);
		inline const void * getRagdollEntityComponent(int64_t entityIndex) const;

	public:	//data
		//************************************************************
		//! @name 
		//************************************************************
		//{@
		float _maxBoneRatioDeltaAdaptation;	// 0 = no delta = no adaptation. 1 = max height delta is same length as bone length
		float _maxSpineHeightDeltaRatio;	//max ratio of bone length that is adapted by spine
		float _raycastLengthFactor;

		float _footStrikeOffset = 0;
		float _footOffOffset = 0;
		float _footOffDuration = 0.2f;
		float _footLockCorrectionsStartRatio = 0.9f;
		float _footLockMaxFootRollCorrection = 90 * GLM_DEG_2_RAD;
		float _footLockMaxSnSCorrection = 1.1f;

		float _orientPelvisAttitudeOnGroundNormal = 0.f;
		float _orientPelvisBankOnGroundNormal = 0.f;
		float _orientPelvisAttitudeOnGroundContacts = 0.f;
		float _orientPelvisBankOnGroundContacts = 0.f;
		float _orientPelvisOnGroundMaxVelocity = 1.f;
		bool _compensatePelvisOriOnSupportChannels;
		bool _compensatePelvisOriOnOtherChannels;

		float _samplingRadius;
		int _samplingRayCount;
		float	_rootHeightRatio;
		const glm::Array<GroundValue>& getGroundValues(int64_t entityIndex) const { return _perEntityData[entityIndex]._groundValues; }

		float _nextFootprintRecomputationThresholdRatio;
		bool _doGroundAdaptationIK;
		//@}

	protected:	//methods
		/**
		* \warning Must be called only if initialized
		*/
		void computeActiveBones() ;

		/**
		* \warning Must be called only if initialized
		*/
		void computeActiveBlindData();

	private:	//structures
		struct SpineConstraint
		{
			Quaternion _groundPrintWorldDeltaOri;

			Vector3 _ankleWorldPos;
			Quaternion _ankleWorldOri;
			float _footLockStretchCorrection = 1.f;

			float _hDesired;
			float _hMax;

			int _spineConverterBone;
			int _channelIndex;
		};

	private:	//methods
		static int sortConstraintsPointsPerSpineBone( const void *arg1, const void *arg2 );

		bool sampleGrounds(int64_t entityIndex,
			const Vector3& evaluationPosition,
			float groundRayCastOffset,
			const Vector3& evaluationUpVector,
			const Vector3& previousPosition,
			const GroundCellId& previousGroundCellId,
			AbstractGround::GroundPinMode::Value pinMode,
			Vector3& groundPosition,
			Vector3& groundNormal,
			int specificBoneIndex,
			const void *userPtr) const;
			
		void constraintsAdapt(int64_t entityIndex, BlendTreePosture& posture, const glm::Time& dt);

		void computeGroundNormalFromGroundNormal(Vector3& retFrontDirection, Vector3& retSideDirection, int64_t entityIndex, const BlendTreePosture& posture);
		void computeGroundNormalFromLimbsContacts(Vector3& retFrontDirection, Vector3& retSideDirection, int64_t entityIndex, const BlendTreePosture& posture, int* constrainedChannels, int constrainedChannelsCount);

	private:	//data
		glm::Array< GroundAdaptationBNPerEntityData > _perEntityData;
	};

	//------------------------------------------------------------
	inline const AbstractBlendNode::ConstraintChannel *const 
		GroundAdaptationBN::getConstraintChannels(int64_t entityIndex) const
	{
		return _perEntityData[entityIndex]._constraintChannels;
	}

	//------------------------------------------------------------
	inline void GroundAdaptationBN::setEnabled(int64_t entityIndex, bool enabled, GroundEvaluation::Value groundEvaluation)
	{
		_perEntityData[entityIndex]._enabled = enabled ;
		_perEntityData[entityIndex]._groundEvaluation = groundEvaluation;
	}

	//------------------------------------------------------------
	inline bool GroundAdaptationBN::isEnabled(int64_t entityIndex)  const
	{
		return _perEntityData[entityIndex]._enabled ;
	}

	//------------------------------------------------------------
	inline void GroundAdaptationBN::setPhysicsGround(int64_t entityIndex, const AbstractGround* ground)
	{
		_perEntityData[entityIndex]._physicsGround = ground;
	}

	//------------------------------------------------------------
	inline void GroundAdaptationBN::setMainGround(int64_t entityIndex, const AbstractGround* ground)
	{
		_perEntityData[entityIndex]._mainGround = ground;
	}

	//------------------------------------------------------------
	inline void GroundAdaptationBN::setRagdollEntityComponent(int64_t entityIndex, const void *ragdollEntityComponent)
	{
		_perEntityData[entityIndex]._ragdollEntityComponent = ragdollEntityComponent;
	}

	//------------------------------------------------------------
	inline const void* GroundAdaptationBN::getRagdollEntityComponent(int64_t entityIndex) const
	{
		return _perEntityData[entityIndex]._ragdollEntityComponent;
	}
}
