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

#pragma once

#include "glmAbstractBlendNode.h"
#include "glmIKTarget.h"
#include "glmOrientationTarget.h"

namespace glm
{
	/**
	 * \brief IK Request
	 *
	 * \author dev@golaem.com
	 *
	 */
	class GLM_SDK_API IKRequest
	{
	public:

		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

		IKRequest();
		virtual ~IKRequest();

		void setActivationCoeff(float activationCoeff);
		void setPriority(int priority);
        void setBoneLenghSmoothingFactor(float smoothingFactor);

		void setIKBone(const GenericSpecificConverterMapping* converterMapping, const int boneIndex, const size_t IKChainBoneCount);
		bool targetReached() const;

		int getIKBoneIndex() const { return _ikBone; }
		const Array<int>& getBoneChain() const { return _boneChain; }
        float getBoneLenghSmoothingFactor() const { return _boneLenghSmoothingFactor; }
		
	protected:
		int _ikBone;
		Array<int> _boneChain;

		int _priority;
		float _activationCoeff;
        float _boneLenghSmoothingFactor = 1.f;

		friend class IKBN;
	};

	class GLM_SDK_API IKRequestWithTarget : public IKRequest
	{
	public:

		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

		//************************************************************
		//! @name Constructors and destructor
		//************************************************************
		//@{
		IKRequestWithTarget();

		virtual ~IKRequestWithTarget();
		//@}

		//************************************************************
		//! @name Accessors
		//************************************************************
		//@{
		const glm::IKTarget& getTarget() const;
		glm::IKTarget& touchTarget();
		void setTarget(glm::IKTarget* newTarget);
        void setOrientationTarget(glm::OrientationTarget* newTarget);
		bool targetReached() const;

	protected:

		glm::IKTarget::SP  _target;	//!< target to reach
        glm::OrientationTarget::SP _orientationTarget; //!< target to reach
		bool _targetReached;

		friend class IKBN;
	};

	/**
	* \brief Blend node handling Aim At
	*
	* \author dev@golaem.com
	*
	*/
	class GLM_SDK_API IKBN : public AbstractBlendNode
	{
	public:

		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

		//************************************************************
		//! @name Constructors and destructor
		//************************************************************
		//@{
		IKBN();
		~IKBN();
		//@}


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

		class IKBNPerEntityData
		{
		public:

			GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

			IKBNPerEntityData();

			glm::Array< IKRequestWithTarget * > _requests;
		};


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

		//************************************************************
		//! @name Accessors
		//************************************************************
		//@{

		IKRequestWithTarget * addRequest(int64_t entityIndex);
		void removeRequest(int64_t entityIndex, IKRequestWithTarget * request);
		//@}


		//************************************************************
		//! @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 sort requests
		 */
		static bool sortIKRequests(IKRequest *rA, IKRequest* rB);
		
		/**
		 * \brief
		 */
        static void computeOneIteration(BlendTreePosture& posture, IKRequest* rq, Vector3& targetWorld);

		/**
		 * \brief recopmpute IK target from the activation coefficient and a reachable distance
		 */
		static Vector3 correctTargetPosition(BlendTreePosture& posture, IKRequest *rq, Vector3 & targetWorld);

		//************************************************************
		//! @name runtime properties
		//************************************************************
		//@{
		int _maxIterationsCount;
		float _distanceThreshold;
		//@}

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

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

		/**
		 * \brief do the job updating the posture
		 */
		void computeIKPosture(int64_t entityIndex, IKBNPerEntityData& entityData, BlendTreePosture& posture, glm::Time dt);

	private:
		// effective targets
		struct EffectiveTargets
		{
			Vector3 _worldTarget;
            Quaternion _worldOriTarget;
			IKRequestWithTarget *_request;
            bool _hasOri = false;
		};

		glm::Array< IKBNPerEntityData > _perEntityData;
	};



	//------------------------------------------------------------
	inline const AbstractBlendNode::ConstraintChannel *const
		IKBN::getConstraintChannels(int64_t entityIndex) const
	{
		GLM_DEBUG_ASSERT_MSG(_tree->getSubNodesCount(_id), "You need a blend node linked after this node.");

		const AbstractBlendNode *sourceBN = _tree->getNode(_tree->getSubNode(_tree->getFirstSubNodePos(_id)));
		return sourceBN->getConstraintChannels(entityIndex);
	}

	//------------------------------------------------------------
	inline const glm::IKTarget& IKRequestWithTarget::getTarget() const
	{
		return *_target;
	}

	//------------------------------------------------------------
	inline glm::IKTarget& IKRequestWithTarget::touchTarget()
	{
		return *_target;
	}

	//------------------------------------------------------------
	inline void IKRequestWithTarget::setTarget(glm::IKTarget* newTarget)
	{
		_target = newTarget;
	}

	//------------------------------------------------------------
    inline void IKRequestWithTarget::setOrientationTarget(glm::OrientationTarget* newOriTarget)
    {
        _orientationTarget = newOriTarget;
    }
	//------------------------------------------------------------
	inline void IKRequest::setActivationCoeff(float activationCoeff)
	{
		_activationCoeff = activationCoeff;
	}
	//------------------------------------------------------------
	inline void IKRequest::setPriority(int priority)
	{
		_priority = priority;
	}
	//------------------------------------------------------------
    inline void IKRequest::setBoneLenghSmoothingFactor(float smoothingFactor)
	{
        _boneLenghSmoothingFactor = smoothingFactor;
	}
	//------------------------------------------------------------
	inline bool IKRequestWithTarget::targetReached() const
	{
		return _targetReached;
	}
}
