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

#pragma once

//golaem motion includes
#include "glmSDKAPI.h"
#include "glmMotionDefinitions.h"
#include <glmVector3.h>
#include <glmQuaternion.h>
#include "glmGenericSkeleton.h"
//#include "glmLimbConfiguration.h"
//#include "glmGroundSupportSequence.h"
#include <glmGolaemMotionDeclarations.h> // GolaemMotion declarations

namespace glm
{
	/**
	 * \brief Describe a normalized bone (a link with changeable length)
	 * Our normalized bones are Z aligned, they possess a length and a realtive position/orientation
	 *
	 * \author dev@golaem.com
	 * 
	 */
	class GLM_SDK_API GenericBoneV0
	{
		friend class GenericSkeletonV4;
		friend struct GenericSkeletonV1;
		friend struct GenericSkeletonV2;
		friend struct GenericSkeletonV3;

		GLM_RTTI( "39c0c74b-a8ff-4b42-aa87-9a271debd332", glm::GenericBoneV0, addAttributes )

	public:

		GLM_CLASS_STAT_ID( glm::MemoryStatIds::GolaemShared)

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

		//************************************************************
		//! @name Misc. accessors
		//************************************************************
		//@{
#ifndef NDEBUG
		/**
		 * \brief get the bone name
		 */
		const char* getId() const { return _name.c_str(); }

		/**
		 * \brief the a new name for the bone
		 * \param newName 
		 */
		void setName(const char* newName) { _name=newName; }
#endif

		/**
		 * \brief get the bone type
		 */
		SkeletonPartType::Value getType() const { return SkeletonPartType::Value(_type); }
		
		/**
		 * \brief get the father of the bone
		 * \return a csont pointer on the father bone, or NULL if the bone is a root bone
		 */
		const GenericBoneV0* getFather() const { return _father; }

		/**
		 * \brief Get the number of children of the bone
		 * \return the number of children of the bone
		 */
		inline int getNbChildren() const { return _childrenCount; }

		/**
		 * \brief get all the children of the bone
		 * \return a const vector containing pointers on the children
		 */
		inline const GenericBoneV0*const* getChildren() const { return _children; }

		/**
		 * \brief get a specific child of the bone
		 * \return a const pointer on the desired child
		 */
		inline const GenericBoneV0* getChild(int indice) const;
		//@}

		//************************************************************
		//! @name Morphology accessors
		//************************************************************
		//@{
		/**
		 * \brief Get the index of the bone in the skeleton array
		 */
		int getGenericBoneIndex() const { return _genericBoneIndex; }

		inline const Vector3& getRelativeIkNormalPlane() const { return _relativeIkNormalPlane; }

		float getDefaultLengthRatio() const 	{ return _defaultLengthRatio; }

		/**
		 * \brief get the orientation of the end of the bone in the bone coordinate system
		 * \return a reference on the end fo bone relative orientation
		 */
		inline const Quaternion& getEndOfBoneRelativeOri() const	{ return _endOfBoneLocalOrientation; }
		//@}

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

		//************************************************************
		//! @name Morphology construction
		//************************************************************
		//@{
		/**
		 * \brief set the father of the bone
		 */
		inline void setFather(GenericBoneV0* father);

		/**
		* \brief set the children of the bone
		*/
		void setChildren(GenericBoneV0** children, int childrenCount);

		/**
		 * \brief set the bone type
		 */
		void setType(SkeletonPartType::Value type) { _type = type; }

		/**
		 * \brief set the orientation of the end of the bone relative to the beginning of the bone
		 * \param new ori to set
		 * \warning the relative orientation should usually be set by the GenericSkeleton::setMorphology method, and not modified after. Use this method only if you know exactly what you are doing.
		 */
		inline void setEndOfBoneRelativeOri(const Quaternion& newOri);

		/**
		 * \brief Set the index of the bone in the skeleton array
		 */
		void setGenericBoneIndex(int tabIndex) { _genericBoneIndex = tabIndex; }
		//@}

		//************************************************************
		//! @name not const accessors and setters
		//************************************************************
		//@{
		GenericBoneV0* touchFather() { return _father; }
		GenericBoneV0** touchChildren() { return _children; }
		inline GenericBoneV0* touchChild(const int indice);			
		//@}

		//************************************************************
		//! @name un cached world accessors (called by the skeleton only, other classes should get world acccessors of the skeleton itself)
		//************************************************************
		//@{
		/**
		 * \brief get the orientation of the bone in the world coordinate system
		 * \return the world orientation of the bone (ie combined with the orientation of the bone hierarchy)
		 */
		inline Quaternion getWorldOrientation() const;

		/**
		 * \brief get the orientation of the end of the bone in the world coordinate system
		 * \return the world orientation of the end of the bone (ie combined with the orientation of the bone hierarchy)
		 */
		inline Quaternion getEndOfBoneWorldOrientation() const;
		//@}
		
		inline Vector3 computeEndPosUnscaled() const;
		inline Vector3 getWorldPositionUnscaled() const;
		inline Vector3 getEndWorldPositionUnscaled() const;

		//************************************************************
		// Protected data that ensure that the value is accedded from the GenericSkeleton::Override instead
		//************************************************************
		//@{

		/**
		 * \brief position of the bone in the father bone coordinate system
		 */
		inline const Vector3& getLocalPositionUnscaled() const { return _localPosition; }
		void setRelativePosUnscaled(const Vector3& newPos) { _localPosition = newPos; }

		/**
		 * \brief orientation of the bone relativ to it's father
		 */
		inline void setRelativeOri(const Quaternion& newOri);
		inline const Quaternion& getLocalOrientation() const { return _localOrientation; }

		/**
		 * \brief Length of a bone
		 */
		float getLengthUnscaled() const { return _length; }
		void setLengthUnscaled(float newLength) { _length = newLength; }
		void setDefaultLengthRatio(float newLengthRatio) { _defaultLengthRatio = newLengthRatio; }
		//@}


	private:
		//************************************************************
		// Data
		//************************************************************
		//@{
#ifndef NDEBUG
		glm::GlmString			_name;
#endif
		int					_type;
		int					_genericBoneIndex;

		Vector3		_localPosition;	//!< position of the bone relative to its father
		Quaternion	_localOrientation;	//!< orientation of the bone relative to its father
		Vector3		_relativeIkNormalPlane;//!< half plan on which doing the IK resolution. The half plan is exprimed in the bone coordinate system (_localOrientation)
		float		_length;		//!< bone length
		float		_defaultLengthRatio;
		Quaternion	_endOfBoneLocalOrientation;	//!< orientaiton of the end of the bone (relative to the begin of bone)

		GenericBoneV0*		_father;		//!< pointer to the father bone
		GenericBoneV0**		_children;		//!< pointer to first children bone pointer
		int					_childrenCount;
		//@}

	};








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

	//------------------------------------------------------------
	// getChild
	//------------------------------------------------------------
	inline const GenericBoneV0* 
	GenericBoneV0::getChild(int indice) const
	{
		GLM_ASSERT_MSG( indice<_childrenCount, "invalid child index" );
		return _children[indice];
	}

	//------------------------------------------------------------
	// touchChild
	//------------------------------------------------------------
	inline GenericBoneV0*
	GenericBoneV0::touchChild(int indice)
	{
		GLM_ASSERT_MSG( indice<_childrenCount, "invalid child index" );
		return _children[indice];
	}

	//------------------------------------------------------------
	// setRelativeOri
	//------------------------------------------------------------
	inline void
	GenericBoneV0::setRelativeOri(const Quaternion& newOri)
	{
		_localOrientation = newOri;
		_localOrientation.normalizeSelf();
	}
	
	//------------------------------------------------------------
	// setEndOfBoneRelativeOri
	//------------------------------------------------------------
	inline void
	GenericBoneV0::setEndOfBoneRelativeOri(const Quaternion& newOri)
	{
		_endOfBoneLocalOrientation = newOri;
		_endOfBoneLocalOrientation.normalizeSelf();
	}

	//------------------------------------------------------------
	// setFather
	//------------------------------------------------------------
	inline void 
	GenericBoneV0::setFather( GenericBoneV0* father )
	{			
		_father = father;
		GLM_DEBUG_ASSERT(_father!=this);
	}

	//------------------------------------------------------------
	inline Vector3 
	GenericBoneV0::getWorldPositionUnscaled() const
	{
		if ( !_father )
			return _localPosition;
		Vector3 result;
		Quaternion::mulAdd(result, _father->getEndWorldPositionUnscaled(), _father->getEndOfBoneWorldOrientation(), _localPosition);
		return result;
	}

	//------------------------------------------------------------
	inline Vector3
	GenericBoneV0::computeEndPosUnscaled() const
	{
		return _localOrientation*Vector3(0,0,getLengthUnscaled());
	}

	//------------------------------------------------------------
	inline Vector3
	GenericBoneV0::getEndWorldPositionUnscaled() const
	{
		Vector3 result;
		Quaternion::mulAdd(result, getWorldPositionUnscaled(), getWorldOrientation(), Vector3(0, 0, getLengthUnscaled()));
		return result;
	}

	//------------------------------------------------------------
	// getWorldOrientation
	//------------------------------------------------------------
	inline Quaternion
	GenericBoneV0::getWorldOrientation() const
	{
		if (!_father )
			return _localOrientation;
		return _father->getEndOfBoneWorldOrientation()*_localOrientation;				
	}

	//------------------------------------------------------------
	// getEndOfBoneWorldOrientation
	//------------------------------------------------------------
	inline Quaternion
	GenericBoneV0::getEndOfBoneWorldOrientation() const
	{
		return getWorldOrientation()*_endOfBoneLocalOrientation;				
	}

	//------------------------------------------------------------
	//setChildren
	//------------------------------------------------------------
	inline void 
	GenericBoneV0::setChildren( GenericBoneV0** children, int childrenCount )
	{
		_children = children;
		_childrenCount = childrenCount;
	}
}
