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

#pragma once

//golaem motion includes
#include "glmSDKAPI.h"
#include "glmMotionDefinitions.h"

#include "glmGenericSkeleton.h"

//golaem core includes
#include <glmMathBase.h>
#include <glmAttribute.h>
#include "glmAccessorsAttribute.h"

#include <glmGolaemMotionDeclarations.h> // GolaemMotion declarations

namespace glm
{
	struct GenericPostureV0;
	struct GenericPostureV1;
	struct GenericPostureV2;
	struct GenericPostureV3;
	struct GenericPostureV4;
	struct GenericAnimationV11;
	class BlendTree;

	struct MirrorMethod
	{
	public:
		enum Value
		{
			EULER_DECOMPOSITION = 0,
			QUATERNION_DECOMPOSITION,
			END
		};
	};

	/**
	* \brief Represent a generic normalized posture
	*
	* \author dev@golaem.com
	*
	* \note The posture contains a given number of DOF:
	* - a global world position (WP) and a global world orientation (WO)
	* - per bone:
	*				- a posture orientation: PO(bone)
	*				- a length ratio: LR(bone)
	*				- an end of bone posture orientation EPO(bone)
	*
	* The end of bone world orientation of a bone in the posture is computed as follow:
	*	EWO(bone) = WO(bone) * EPO(bone)
	* The world orientation of a bone in the posture is computed as follow:
	*	WO(bone) = EWO(bone.father) * bone.getLocalOri() * PO(bone)
	*/
	class GLM_SDK_API GenericPostureV5
	{
		GLM_RTTI( "b8358d50-f8a8-4101-a21e-29823b0f48fd", glm::GenericPostureV5, addAttributes );

		friend class BlendTree;
		friend struct GenericAnimationV11;
		friend class GenericAnimationV12;
		friend class GenericSpecificConverter;
		friend class AnimationPlayBN;

	public:
		GLM_CLASS_STAT_ID( glm::MemoryStatIds::GolaemShared)

		GenericPostureV5();
		GenericPostureV5( const GenericPostureV5& ) ;
		~GenericPostureV5();

		void createFromV0(GenericPostureV0& genericPostureV0, const GenericSkeleton* genericSkeleton);
		void createFromV1(GenericPostureV1& genericPostureV1, const GenericSkeleton* genericSkeleton);
		void createFromV2(GenericPostureV2& genericPostureV2, const GenericSkeleton* genericSkeleton);
		void createFromV3(GenericPostureV3& genericPostureV3, const GenericSkeleton* genericSkeleton);
		void createFromV4(GenericPostureV4& genericPostureV4, const GenericSkeleton* genericSkeleton);

		bool operator==(const GenericPostureV5& source) const;

		//************************************************************
		//! @name Initialization & construction
		//************************************************************
		//@{
		/**
		* \brief Initialize the posture with a skeleton
		* \param normalizedSkeleton skeleton to use. The skeleton should not be destroyed before the posture.
		* \param genericSkeletonOverride skeleton override to use. The override should not be destroyed before the posture.
		*/
		void init(const GenericSkeleton* genericSkeleton, float genericSkeletonScale, const GenericSkeletonV4::Override* genericSkeletonOverride=NULL);

		/**
		* \brief This method set the generic skeleton and resize the postures arrays if required
		*/
		const GenericSkeleton* getGenericSkeleton() const { return _genericSkeleton; }
		void setGenericSkeleton(const GenericSkeleton* genericSkeleton);

		/**
		* \brief Allocates arrays to be filled later
		*/
		void init(size_t bonesCount, int blindDataCount );
		void initWorldCache() ;

	protected:
		void resize(size_t newBoneSize, size_t newBlindDataSize);

	public:
		/**
		* \brief Undo the init and initWorldCache: reset the posture's state and remove the link with the normalized skeleton
		*/
		void finish() ;
		//@}

		//************************************************************
		//! @name Posture set
		//************************************************************
		//@{
		/**
		* \brief copy with array size checks, reallocate if needed
		*/
		GenericPostureV5& operator=(const GenericPostureV5& source);

		/**
		* \brief copy with absolutely no checks
		*/
		void setPosture(const GenericPostureV5& source);

		/**
		* \brief Reset the posture's DOF to identity
		*/
		void clearPosturePelvis();
		void clearPosture();
		void clearPostureWithDefaultLengthRatios();	//will set the length ratios that corresponds to the skeleton posture (not fully extended)

		/**
		* \brief Mirror the posture according to the skeleton's mirror sources
		*/
		void mirrorPosturePelvis(GenericPostureV5& destPosture) const;
		void mirrorPosture(GenericPostureV5& destPosture, MirrorMethod::Value mirrorMethod) const;

		/**
		* \brief Compute the difference with another posture (and store the result in this)
		*/
		void computeSelfDifferenceWith(const GenericPostureV5& otherPosture, int lastBoneIndex);
		float computeSummedSquareDitanceWith(const GenericPostureV5& otherPosture) const;

		void boneAdd(const GenericPostureV5& otherPosture, const float positionOrientationInterpolationWeight, const float interpolationWeight);
		void boneAdd(const GenericPostureV5& otherPosture, const float positionOrientationInterpolationWeight, const float interpolationWeights[]);
		void blindDataAdd(const GenericPostureV5& otherPosture, const float interpolationWeight);
		void blindDataAdd(const GenericPostureV5& otherPosture, const float interpolationWeight[]);

		/**
		* \brief interpolate the posture with another
		* \note posture = (1-weight)*posture + weight*otherPosture
		*/
		void boneBlend(const GenericPostureV5& otherPosture, const float positionOrientationInterpolationWeight, const float interpolationWeight, int lastBoneIndex, bool usePreciseIKBlend = false);
		void boneBlend(const GenericPostureV5& otherPosture, const float positionOrientationInterpolationWeight, const float interpolationWeights[], int lastBoneIndex, bool usePreciseIKBlend = false);
		void blindDataBlend(const GenericPostureV5& otherPosture, const float interpolationWeight);
		void blindDataBlend(const GenericPostureV5& otherPosture, const float interpolationWeights[]);

		/**
		* \brief interpolate the posture with another, using different weight per bone
		* \note posture = posture + weight*otherPosture
		* \warning: the posture cache has to be computed after a call to this method
		*/
		void boneAddBlend(const GenericPostureV5& otherPosture, const float positionOrientationInterpolationWeight, const float interpolationWeight, int lastBoneIndex, bool usePreciseIKBlend = false);
		void boneAddBlend(const GenericPostureV5& otherPosture, const float positionOrientationInterpolationWeight, const float interpolationWeights[], int lastBoneIndex, bool usePreciseIKBlend = false);
		void boneAddBlend_noPelvis(const GenericPostureV5& otherPosture, const float interpolationWeights[], int firstBoneIndex, int lastBoneIndex, bool usePreciseIKBlend=false);
		//@}

		static void computePreciseIKBlend(float& lengthRatioResult, float& ikNormalOffsetResult, float lengthRatioA, float ikNormalOffsetA, float lengthRatioB, float ikNormalOffsetB, float interpolationWeight);

		//************************************************************
		//! @name Accessors
		//! Relation between RestRelativeOrientaiton( _restRelativeOriArray ) and RelativeOrientation(RO)
		//! RO(bone) = bone.getLocalOrientation() * _restRelativeOriArray(bone)
		//************************************************************
		//@{
		/**
		* \brief This method normalize the _pelvisOrientation quaternion and all quaternions in the _restRelativeOriArray and _restRelativeEOBOriArray
		*/
		void normalizeRestRelativeOrientations() ;
		void normalizeCachedWorldOrientations() ;

		const Vector3& getPelvisWorldPosition() const {	return _pelvisWorldPosition;	}
		void setPelvisWorldPosition(const Vector3& position);

		const Vector3& getPelvisNormalizedPosition() const {	return _pelvisNormalizedPosition;	}
		void setPelvisNormalizedPosition(const Vector3& position);

		const Quaternion& getPelvisWorldOrientation() const{	return _pelvisOrientation;	}
		void setPelvisWorldOrientation(const Quaternion& newOrientation);

		/**
		* \brief Return the moving direction in the pelvis coordinate system (use getPelvisWorldOrientation()*getMovingDirection() to obtain the moving direction in world coordinate system )
		*/
		const Vector3 getMovingDirection() const{	return _movingDirection;	}
		const void setMovingDirection(Vector3 movingDirection) { _movingDirection = movingDirection; }

		inline const Quaternion& getRestRelativeOrientation(const int genericBoneIndex) const;
		inline void setRestRelativeOrientation(const int genericBoneIndex, const Quaternion& orientation);

		inline const Angle& getIkNormalOffset(const int genericBoneIndex) const;
		inline void setIkNormalOffset(const int genericBoneIndex, const Angle& orientation);

		inline const Quaternion& getEndOfBoneRestRelativeOrientation(const int genericBoneIndex) const;
		inline void setEndOfBoneRestRelativeOrientation(const int genericBoneIndex, const Quaternion& orientation);

		void getRelativeOrientation(const GenericBone& bone, Quaternion& result) const;
		void setRelativeOrientation(const GenericBone& bone, const Quaternion& relativeOrientation);

		inline float getLengthRatio(const int genericBoneIndex) const;
		inline void setLengthRatio(const int genericBoneIndex, float newLengthRatio);		
 
		inline float getSquashStretchRatio(const int genericBoneIndex) const;
		inline void setSquashStretchRatio(const int genericBoneIndex, float newSquashStretchRatio);

		inline float getEndOfBoneSquashStretchRatio(const int genericBoneIndex) const;
		inline void setEndOfBoneSquashStretchRatio(const int genericBoneIndex, float newSquashStretchEOBRatio);	

		/**
		* \brief size of initialized arrays
		*/
		inline int getSkeletonBoneSize() const;
		
		inline void setGenericSkeletonScaleAndOverride(float globalSkeletonScale, const GenericSkeleton::Override* skeletonOverride);
		inline const float getGenericSkeletonScale() const { return _genericSkeletonScale; }
		inline const GenericSkeleton::Override* getGenericSkeletonOverride() const { return _genericSkeletonOverride; }

		/**
		* \brief mirror world position for one bone
		*/
		void mirrorWorldPosition(const int genericBoneIndex) { _worldPosArray[genericBoneIndex].y = -_worldPosArray[genericBoneIndex].y; _EOBworldPosArray[genericBoneIndex].y = -_EOBworldPosArray[genericBoneIndex].y; }
		//@}

		static Vector3 mirror( const Vector3& v ) {	return Vector3(v.x, -v.y, v.z ); }
		static Quaternion mirror( const Quaternion& q) { return Quaternion( q.x, -q.y, q.z, -q.w ); }

		static Vector3 mirror(const Vector3& v, const Quaternion& upToZ, const Quaternion& zToUp);
		static Quaternion mirror(const Quaternion& q, const Quaternion& upToZ, const Quaternion& zToUp);

		//************************************************************
		//! @name World accessors are valid only once computeCacheWorldFromLocals has been called
		//! Relation between RestRelativeOrientation( _restRelativeOriArray ), RelativeOrientation(RO), WorldOrientaiton(WO) and EndOfBoneWorldOrientaiton (EOB_WO)
		//! WO(bone) = EOB_WO(bone->getFather()) * bone.getRelativeOri() * _restRelativeOriArray(bone);	which is equivalent to WO(bone) = EOB_WO(bone->getFather()) * RO(bone)
		//! and:
		//! EOB_WO(bone) = WO(bone) * bone.getEndOfBoneRelativeOri() * _restRelativeEOBOriArray(bone);
		//************************************************************
		//@{
		void computeCacheWorldFromLocals(int firstBoneIndex, int lastBoneIndex) ;
		void computeCacheLocalAndWorldFromWorld(int firstBoneIndex, int lastBoneIndex) ;

		const Vector3& getWorldPosition(int genericBoneIndex) const	{ GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize());	return _worldPosArray[genericBoneIndex]; }
		const Quaternion& getWorldOrientation(int genericBoneIndex) const	{ GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize()); return _worldOriArray[genericBoneIndex]; }
		const Vector3& getEndOfBoneWorldPosition(int genericBoneIndex) const	{ GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize()); return _EOBworldPosArray[genericBoneIndex]; }
		const Quaternion& getEndOfBoneWorldOrientation(int genericBoneIndex) const	{ GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize()); return _EOBworldOriArray[genericBoneIndex]; }

		inline bool isWorldCacheInitialized() const { return !(getSkeletonBoneSize()>0 && _worldPosArray == NULL); }

		bool isNormalized() const;
		//@}

		inline size_t getBlindDataCount()
		{
			return _blindData.size();
		}

	private:
		/**
		* \brief register the inspectable attributes to the class RTTI
		*/
		static void addAttributes() ;

		void setGenericSkeleton_serialization(const GenericSkeleton* genericSkeleton);

		//************************************************************
		//! @name World setters compute the rest relative orientations to get the desired world orientations
		//************************************************************
		//@{
		void computeWorldOrientation(const GenericBone& bone, Quaternion& result) const;
		void computeEndOfBoneWorldOrientation(const GenericBone& bone, Quaternion& result) const;
		//@}

	public:
		//************************************************************
		//! \brief Public Data
		//************************************************************
		//@{
		glm::PODArray<float>		_blindData;
		//@}	

	protected:
		const GenericSkeletonV4* _genericSkeleton;

		float _genericSkeletonScale;
		const GenericSkeletonV4::Override* _genericSkeletonOverride;

	private:
		//************************************************************
		//! @name Data
		//************************************************************
		//@{
		Vector3		_pelvisNormalizedPosition;
		Vector3		_pelvisWorldPosition;
		Quaternion	_pelvisOrientation;
		Vector3		_movingDirection;

		glm::Array<Quaternion>	_restRelativeOriArray;
		glm::PODArray<Angle>		_ikNormalOffsetArray;
		glm::PODArray<float>		_lengthRatioArray;
		glm::Array<Quaternion>	_restRelativeEOBOriArray;
		glm::PODArray<float>		_squashStretchRatioArray;
		glm::PODArray<float>		_squashStretchEOBRatioArray;
		int			_skeletonBonesSize;
		int			_realArraysSize;
		//@}

		//************************************************************
		//! @name Cache data
		//************************************************************
		//@{
		Vector3* _worldPosArray;
		Quaternion*	_worldOriArray;
		Vector3*	_EOBworldPosArray;
		Quaternion*	_EOBworldOriArray;
		//@}
	};

	//************************************************************
	// Functions using GenericPostureV5
	//************************************************************
	bool GLM_SDK_API approxEqual(const GenericPostureV5& posture1, const GenericPostureV5& posture2, float precision);



	//************************************************************
	// Definition of inline methods
	//************************************************************

	//------------------------------------------------------------
	// getRestRelativeOrientation
	//------------------------------------------------------------
	inline const Quaternion&
		GenericPostureV5::getRestRelativeOrientation(const int genericBoneIndex) const
	{
		return _restRelativeOriArray[genericBoneIndex];
	}

	//------------------------------------------------------------
	// setRestRelativeOrientation
	//------------------------------------------------------------
	inline void
		GenericPostureV5::setRestRelativeOrientation(const int genericBoneIndex, const Quaternion& orientation)
	{
		GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize());
		GLM_DEBUG_ASSERT(orientation.isNormalized());

		_restRelativeOriArray[genericBoneIndex] = orientation;
	}

	//------------------------------------------------------------
	// getIkNormalOffset
	//------------------------------------------------------------
	inline const Angle&
		GenericPostureV5::getIkNormalOffset(const int genericBoneIndex) const
	{
		return _ikNormalOffsetArray[genericBoneIndex];
	}

	//------------------------------------------------------------
	// setIkNormalOffset
	//------------------------------------------------------------
	inline void
		GenericPostureV5::setIkNormalOffset(const int genericBoneIndex, const Angle& orientation)
	{
		GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize());
		GLM_DEBUG_ASSERT(glmFinite(orientation));

		_ikNormalOffsetArray[genericBoneIndex] = orientation;
	}

	//------------------------------------------------------------
	// getEndOfBoneRestRelativeOrientation
	//------------------------------------------------------------
	inline const Quaternion&
		GenericPostureV5::getEndOfBoneRestRelativeOrientation(const int genericBoneIndex) const
	{
		return _restRelativeEOBOriArray[genericBoneIndex];
	}

	//------------------------------------------------------------
	// setEndOfBoneRestRelativeOrientation
	//------------------------------------------------------------
	inline void
		GenericPostureV5::setEndOfBoneRestRelativeOrientation(const int genericBoneIndex, const Quaternion& orientation)
	{
		GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize());
		GLM_DEBUG_ASSERT(glmFinite(orientation[0]) && glmFinite(orientation[1]) && glmFinite(orientation[2]) && glmFinite(orientation[3]) );

		_restRelativeEOBOriArray[genericBoneIndex] = orientation;
	}

	//------------------------------------------------------------
	// getLengthRatio
	//------------------------------------------------------------
	inline float
		GenericPostureV5::getLengthRatio(const int genericBoneIndex) const
	{
		return _lengthRatioArray[genericBoneIndex];
	}

	//------------------------------------------------------------
	// setLengthRatio
	//------------------------------------------------------------
	inline void
		GenericPostureV5::setLengthRatio(const int genericBoneIndex, float newLengthRatio)
	{
		GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize());
		GLM_DEBUG_ASSERT(glmFinite(newLengthRatio));
		_lengthRatioArray[genericBoneIndex] = clamp(newLengthRatio, 0.f, 1.f);
	}

	//------------------------------------------------------------
	// getSquashStretchRatio
	//------------------------------------------------------------
	inline float
		GenericPostureV5::getSquashStretchRatio(const int genericBoneIndex) const
	{
		GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize());
		return _squashStretchRatioArray[genericBoneIndex];
	}

	//------------------------------------------------------------
	// setSquashStretchRatio
	//------------------------------------------------------------
	inline void
		GenericPostureV5::setSquashStretchRatio(const int genericBoneIndex, float newSquashStretchRatio)
	{
		GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize());
		GLM_DEBUG_ASSERT(glmFinite(newSquashStretchRatio));
		_squashStretchRatioArray[genericBoneIndex] = max(newSquashStretchRatio, 0.f);
	}

	//------------------------------------------------------------
	// getEndOfBoneSquashStretchRatio
	//------------------------------------------------------------
	inline float
		GenericPostureV5::getEndOfBoneSquashStretchRatio(const int genericBoneIndex) const
	{
		GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize());
		return _squashStretchEOBRatioArray[genericBoneIndex];
	}

	//------------------------------------------------------------
	// setEndOfBoneSquashStretchRatio
	//------------------------------------------------------------
	inline void
		GenericPostureV5::setEndOfBoneSquashStretchRatio(const int genericBoneIndex, float newSquashStretchEOBRatio)
	{
		GLM_DEBUG_ASSERT(genericBoneIndex>=0 && genericBoneIndex<getSkeletonBoneSize());
		GLM_DEBUG_ASSERT(glmFinite(newSquashStretchEOBRatio));
		_squashStretchEOBRatioArray[genericBoneIndex] = max(newSquashStretchEOBRatio, 0.f);
	}

	//------------------------------------------------------------
	inline int GenericPostureV5::getSkeletonBoneSize() const
	{
		return _skeletonBonesSize;
	}

	//------------------------------------------------------------
	inline void GenericPostureV5::setGenericSkeletonScaleAndOverride(float globalSkeletonScale, const GenericSkeleton::Override* skeletonOverride)
	{
		_genericSkeletonScale = globalSkeletonScale;
		_genericSkeletonOverride = skeletonOverride;
		if (_genericSkeletonOverride == NULL && _genericSkeleton!=NULL)
			_genericSkeletonOverride = &_genericSkeleton->getDefaultOverride();
	}
}

