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

#pragma once

#include "glmAbstractBlendNode.h"
#include "glmIKTarget.h"
#include "glmRamp.h"

namespace glm
{
	class BoneChain
	{
	public:

		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation)

		BoneChain() {}
		~BoneChain() {}

		float _weight;
		int _boneIndex;
	};

	/*
	 * \brief Aim At Request
	 *
	 * \author dev@golaem.com
	 *
	 */
	class GLM_SDK_API AimAtRequest
	{
	public: //methods
		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation)

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

		~AimAtRequest();
		//@}

		//************************************************************
		//! @name Accessors
		//************************************************************
		//@{
		inline const glm::IKTarget& getTarget() const { return *_target; }
		inline glm::IKTarget& touchTarget() { return *_target; }
		inline void setTarget(glm::IKTarget* newTarget) { _target = newTarget; }
		void setActivationCoeff(float activationCoeff) { _activationCoeff = activationCoeff; }
		void setPriority(int priority) { _priority = priority; }
		void setAimingAxis(const Vector3& axis) { _aimingAxis = axis; }
		const Vector3& getFrontAxis() const { return _aimingAxis; }
		//@}

		/**
		* \brief extremity is end of bone: is true, 1st weight is for EOB else, 1st weight is for Bone.
		*/
		void setAimingBone(const BlendTree * tree, int boneIndex, glm::PODArray<float>& adaptationWeights);
		bool targetReached() const { return _targetReached; }

	public: //data
		Vector3 _pelvisToBone;

		Angle	_maxAngularYaw;
		Angle	_maxAngularPitch;

		Angle	_maxSpeed;
		Ramp    _maxSpeedRamp;

	protected: //data
		Vector3		_aimingAxis;
		//
		int _aimingBone;
		int _priority;
		glm::IKTarget::SP  _target;	//!< target to reach

		glm::Array<BoneChain> _boneChain;
		glm::Array<Quaternion> _previousBoneCorrection;	//!< previous bone orientations in father coordinate system for the complete chain

		float _activationCoeff;
		bool _targetReached;
		friend class AimAtBN;
	};

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

	public:

		class AimAtBNPerEntityData
		{

		public:

			GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

			AimAtBNPerEntityData();

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

		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

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

		~AimAtBN();
		//@}


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

		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
		//************************************************************
		//@{
		AimAtRequest * addRequest(int64_t entityIndex);
		void removeRequest(int64_t entityIndex, AimAtRequest * 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;
		//@}

		void setAimingBone(int genericBoneIndex, const glm::PODArray<float>& adaptationWeights);

	public://data
		glm::Angle _alignementAttitudeOffset;

	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 computeAimAtPosture(int64_t entityIndex, AimAtBNPerEntityData& entityData, BlendTreePosture& posture, glm::Time dt);

		/**
		* \brief sort requests
		*/
		static bool sortAimAtRequests(AimAtRequest *rA, AimAtRequest *rB);

	private:
		//************************************************************
		//! @name 
		//************************************************************
		//{@

		//@}

		//bool _enabled;

		glm::Array< AimAtBNPerEntityData > _perEntityData;
	};

	//------------------------------------------------------------
	// getConstraintChannels
	//------------------------------------------------------------
	inline const AbstractBlendNode::ConstraintChannel *const
		AimAtBN::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);
	}
}
