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

#pragma once

#include "glmVector3.h"
#include "glmQuaternion.h"
#include "glmRotationMatrix.h"
#include "glmAxisAngle.h"
#include "glmQuaternionConversions.h"
#include "glmDistance.h"

namespace glm 
{ 
	/**
	* \brief Function class used for inverse kinematic computing.
	*
	* \ingroup functions
	*
	* \author Golaem <dev@golaem.com>
	*
	* \param float Manipulated scalar type (\ref Expression, double, float...)
	*/
	class InverseKinematic
	{
	public:
		/**
		* \brief Enum handling results of the calculations.
		*/
		enum IKResult 
		{
			IK_Reachable, //!< Constraint reachable
			IK_Unreachable,	//!< Constraint unreachable (Kinematic chained tensed)
			IK_Error //!< Error (Singularity) during the calculation
		};

		/**
		* \brief Retrieve a point on a circle of solutions given a direction.
		* 
		* This function returns a M point such as
		* - M is on a distances radius and vzn normal C circle, 
		* - M is in the Y axis direction (vyn = vzn ^ vxn).
		*
		* If vxn and vzn are colinear, the whole circle is a valid solution, the function returns false.
		*
		* \param[out] m Returned value.
		* \param[in] cPosition Center of the circle.
		* \param[in] cRadius Radius of the circle.
		* \param[in] vzn Circle normal.
		* \param[in] vxn Normal of the plan formed by (vzn,CM) , CM is colinear to the Y axis.
		*/
		static bool getNearestPointOnCircle(
			Vector3 & m, 
			const Vector3& cPosition, 
			float cRadius, 
			const Vector3& vzn, 
			const Vector3& vxn)
		{
			Vector3 vyn(vzn^vxn);
			if (vyn.isNull())
			{
				return false;
			}
			else
			{
				m=cPosition+cRadius*vyn;
				return true;
			}
		}

		/**
		* \brief Find the bi-segment configuration preserving the distances and the first point.
		* 
		* IK calculation favoring the third point position (the closest solution to posnear).
		* If there's a solution, length are preserved as the third point can be modified.
		*
		* \param vepa The first point (preserved)
		* \param vcod The second point (modifiable)
		* \param vpoi The third point (modifiable - Z axis is defined by vepa->vpoi)
		* \param vecxnorm Normalized X axis, normal of the plan containing the spline (oriented toward the Y axis)
		* \param r1 constant distance between the first and the second point.
		* \param r2 constant distance between the second and the third point.
		* \return The result of the calculation.
		*/
		static IKResult twoLinksConstLength(
			const Vector3& vepa,
			Vector3& vcod,
			Vector3& vpoi,
			const Vector3& vecxnorm,
			float r1,
			float r2)
		{
            // prenons le cas d'un bras...
            //															vepa = position actuelle de l'epaule
            //															vpoi = position souhaitée du poignet
            //															r1= distance "constante" epaule-coude
            //															r2= distance "constante" coude-poignet
            float op= distance(vepa,vpoi);	// op=distance actuelle epaule-poignet
			if (op==0)
			{
				if (!approxEqual(r1, r2, GLM_EPSILON))
				{
					vcod = vepa + (0.5f*(r1 + r2)) * vecxnorm.computeNormalize();
					return IK_Error;	//r1 needs to be equals to r2 to reach this position correctly. (0.5f*(r1 + r2)) is the "less bad" value we can reach
				}
				else
				{
					vcod = vepa + r1 * vecxnorm.computeNormalize();
					return IK_Reachable;
				}
			}
			Vector3 vzn=(vpoi-vepa)/op;
            if (op >= r1 + r2) // la position souhaitée du poignet est trop éloignée ?
            {	
				vcod=vepa+vzn*r1;										// oui : on place le coude a distance r1 de l'epaule sur la direction op
				vpoi=vcod+vzn*r2;										// on place le poignet a distance r1+r2  de l'epaule sur la direction op
				return IK_Unreachable;
			}			
			if (op<=fabs(r1-r2))
			{	
				vcod=vepa+vzn*(-r1);
				vpoi=vcod+vzn*(r2);
				return IK_Unreachable;
			}

			float dOC=((r1*r1-r2*r2+op*op)/(2*op));
			float dR2=r1*r1-dOC*dOC;
			Vector3	vC;
			if (dR2<=0)
			{	
				dOC=((r2*r2-r1*r1+op*op)/(2*op));
				dR2=r2*r2-dOC*dOC;
				if (dR2<=0)
				{
					vcod=vepa+vzn*r1;									// oui : on place le coude a distance r1 de l'epaule sur la direction op
					vpoi=vcod+vzn*r2;									// on place le poignet a distance r1+r2  de l'epaule sur la direction op
					return IK_Unreachable;
				}
				else 
				{
					vC = (vpoi-vzn*dOC);								// C = milieu  du "elbow circle"
				}
			}
			else 
			{
				vC = (vepa+vzn*dOC);									// C = milieu  du "elbow circle"
			}

			float distances=sqrtf(dR2);								// distances = rayon   du "elbow circle"
			Angle ang = angle(vzn,vecxnorm);
            if (ang < Angle(GLM_EPSILON)) // si ang<epsilon alors v et n sont quasi alignés
            {
                return IK_Error; // donc singularité
            }
            if (!getNearestPointOnCircle(vcod,vC,distances,vzn,vecxnorm))
			{
				return IK_Error;										// sinon on trouve la position du coude en fonction de C et v
			}

            return IK_Reachable; // la position souhaitée du poignet est validée
        }

		/**
		* \brief Find the bi-segment configuration preserving the proportions and the first and third points.
		* 
		* IK calculation favoring the third point position (the closest solution to posnear).
		* If there's a solution, proportions are preserved.
		*
		* \param vepa The first point (preserved)
		* \param vcod The second point (modifiable)
		* \param vpoi The third point (preserved - Z axis is defined by vepa->vpoi)
		* \param vecxnorm Normalized X axis, normal of the plan containing the spline (oriented toward the Y axis)
		* \param r1 constant distance between the first and the second point.
		* \param r2 constant distance between the second and the third point.
		* \return The result of the calculation.
		*/
		static IKResult twoLinksConstProportions(
			const Vector3 & vepa,
			Vector3 & vcod,
			const Vector3 & vpoi,
			const Vector3 & vecxnorm,
			float r1,
			float r2)
		{
            // prenons le cas d'un bras...
            //														vepa = position actuelle de l'epaule
            //														vpoi = position souhaitée du poignet
            //														r1= distance "constante" epaule-coude
            //														r2= distance "constante" coude-poignet
            float op= distance(vepa,vpoi);	// op= distance actuelle epaule-poignet
			if (op==0)
			{
				return IK_Error;
			}
			Vector3 vzn=(vpoi-vepa)/op;

            if (op >= r1 + r2) // la position souhaitée du poignet est trop éloignée ?
            {
				vcod=vepa+vzn*(r1/(r1+r2));							// oui : on place le coude a distance r1/(r1+r2) de l'epaule sur la direction op
				return IK_Unreachable;
			}
			if (op<=fabs(r1-r2))
			{																			
				if (r1>r2)
				{
					vcod=(vepa+vpoi+vzn*(r1+r2))*0.5f;
				}
				else
				{
					vcod=(vepa+vpoi-vzn*(r1+r2))*0.5f;
				}
				return IK_Unreachable;
			}

			float dOC=((r1*r1-r2*r2+op*op)/(2*op));
			Vector3 vC(vepa+vzn*dOC);					// C = milieu  du "elbow circle"
			float dR2=r1*r1-dOC*dOC;
            if (dR2 <= GLM_EPSILON) // la position souhaitée du poignet est trop éloignée ?
            {
				vcod=vepa+vzn*(r1/(r1+r2));							// oui : on place le coude a distance r1/(r1+r2) de l'epaule sur la direction op
				return IK_Unreachable;
			}
			float distances=sqrtf(dR2);							// distances = rayon   du "elbow circle"
			Angle angle = glm::angle(vzn,vecxnorm);
            if (angle < GLM_EPSILON) // si ang<epsilon alors v et n sont quasi alignés
            {
                return IK_Error; // donc singularité
            }

            if (!getNearestPointOnCircle(vcod,vC,distances,vzn,vecxnorm))
			{
				return IK_Error;									// sinon on trouve la position du coude en fonction de C et v
			}
            return IK_Reachable; // la position souhaitée du poignet est validée
        }

		/**
		* \brief Find the n-segment configuration preserving the proportions and the first and last points.
		*
		* \param[in] nbLinks The number of links
		* \param[in,out] positions a pointer to an array of	nbLinks+1 Vector3 (z axis = positions.x->positions[nbLinks]). The first and last positions must be initialized, the other positions will be filled by the method.
		* \param[in] normal normal of the plan containing the links
		* \param[in] lengths a pointer on an array of nbLinks scalars containing the lengths of each link
		* \return The result of the calculation.
		*/
		static IKResult nLinksConstLength(
			const unsigned int nbLinks, 
			Vector3* positions,
			const Vector3& normal, 
			const float*	lengths)
		{
			float maxlen;
			float curlen;
			float alpha;
			unsigned int i;
			Vector3	pStart;
			Vector3 pLast;

			pStart = positions[0];
			pLast = positions[nbLinks];

			for (maxlen=0.f,i=0;i<nbLinks;i++)
			{
				maxlen+=lengths[i];
			}
			curlen = distance(pLast,pStart);

			Vector3 startToLastNormalized = pLast-pStart;
			startToLastNormalized.normalizeSelf();
			startToLastNormalized *= lengths[0];
			positions[1]=pStart+startToLastNormalized;
			alpha=lengths[0]/maxlen;
			int result(0);
			for (i=2;i<nbLinks+1;i++)
			{
				//alpha+=(distances[i]+distances[i-1])/(2*maxlen-distances[nb-1]);
				alpha+=lengths[i-1]/maxlen;
				result|=(int)twoLinksConstLength(pStart,positions[i],positions[i-1],-normal,alpha*curlen,lengths[i-1]);
				//if (TwoLink_constLen(pStart,positions[i],positions[i-1],-vxnorm,alpha*curlen,distances[i]) != IK_Reachable)
				//	ok = false;
			}

			// S'il n'y a eu aucune erreur
			if (result==0)
				//if (ok)
			{
				Angle angle = glm::angle(((positions[nbLinks]-pStart)/curlen),(pLast-pStart)/curlen,normal);
				Quaternion qrot;
				toQuaternion(qrot, AxisAngle(normal, angle));
				for (i=1;i<nbLinks+1;i++)
				{
					positions[i]=pStart + qrot * (positions[i]-pStart);
				}
				positions[nbLinks]=pLast;
			}
			else
			{
				Vector3 tempStartToPos = (positions[nbLinks]-pStart);
				tempStartToPos.normalizeSelf();
				Angle angle = glm::angle(tempStartToPos,(pLast-pStart)/curlen,normal);
				Quaternion qrot;
				toQuaternion(qrot, AxisAngle(normal, angle));
				for (i=1;i<nbLinks+1;i++)
				{
					positions[i]=pStart + qrot * (positions[i]-pStart);
				}
			}

			return IKResult(result);//ok;//result;
		}

		/**
		* \brief Find the n-segment configuration preserving the proportions and the first and third points.
		* \note uses FABRIK algorithm (Forward And Backward Reaching Inverse Kinematics), Andreas ARISTIDOU, Joan LASENBY, University of Cambridge
		* \param[in] nbLinks The number of links
		* \param[in,out] positions a pointer to an array of	nbLinks+1 Vector3 (z axis = positions.x->positions[nbLinks]). The first and last positions must be initialized, the other positions will be filled by the method.
		* \param[in] normal normal and side of the plan containing the links. The half part of the plan in which the chain will be contained is defined as the half plane in the direction of (positions[nbLinks]-positions.x)^normal
		* \param[in] lengths a pointer on an array of nbLinks scalars containing the lengths of each link
		* \param[in] quality % of IK resolution
		* \param[in] maxIterations max number of iterations of the inverse kinematic algorithm
		* \param[in] baseAngles base initialisation angles (define the state of the chain before iterating)
		* \return true if the final position's difference with the target pos is under tolerance
		*/
		static bool nLinksConstLength_FABRIK(
			const unsigned int nbLinks,
			Vector3* positions,
			const Vector3& normal,
			const float* lengths,
			const float quality = 0.99,
			const unsigned int maxIterations = 10,
			const Angle baseAngles = Angle(0.1f)
		)
		{
			Vector3 targetPos(positions[nbLinks]);
			Vector3 beginPos(positions[0]);

			//initial position: set some angle between each position, ensuring all points will be on the plane, and that future rotations will keep the plane
			Vector3 Zaxis(targetPos - positions[0]);
			Vector3* iPosition = positions;
			Vector3* iNextPosition = positions + 1;

			for (unsigned int i = 0; i < nbLinks; ++i)
			{
				Quaternion rotationQuat;
				toQuaternion(rotationQuat, AxisAngle(normal, Angle(max(-GLM_PI_DIV_2, -(int)(nbLinks - i) * baseAngles))));
				rotateVecByQuat(*iNextPosition, rotationQuat, Zaxis);
				//*iNextPosition = rotationQuat * Zaxis;

				iNextPosition->normalizeSelf();
				(*iNextPosition) *= lengths[i];
				*iNextPosition += *iPosition;
				++iNextPosition;
				++iPosition;
			}

			return nLinksConstLength_FABRIK_keepInitialPositions(nbLinks, positions, targetPos, lengths, quality, maxIterations);
		}

		/**
		* \brief Find the n-segment configuration preserving the proportions and the first and third points.
		* \note uses FABRIK algorithm (Forward And Backward Reaching Inverse Kinematics), Andreas ARISTIDOU, Joan LASENBY, University of Cambridge
		* \param[in] nbLinks The number of links
		* \param[in,out] positions a pointer to an array of	nbLinks+1 Vector3 (z axis = positions.x->positions[nbLinks]). The first and last positions must be initialized, the other positions will be filled by the method.
		* \param[in] normal normal and side of the plan containing the links. The half part of the plan in which the chain will be contained is defined as the half plane in the direction of (positions[nbLinks]-positions.x)^normal
		* \param[in] lengths a pointer on an array of nbLinks scalars containing the lengths of each link
		* \param[in] quality % of IK resolution
		* \param[in] maxIterations max number of iterations of the inverse kinematic algorithm
		* \param[in] baseAngles base initialisation angles (define the state of the chain before iterating)
		* \return true if the final position's difference with the target pos is under tolerance
		*/
		static bool nLinksConstLength_FABRIK_keepInitialPositions(
			const unsigned int nbLinks, 
			Vector3* positions,
			Vector3 targetPos,
			const float* lengths,
			const float quality=0.99,
			const unsigned int maxIterations=10
			)
		{
			Vector3 beginPos(positions[0]);

			Vector3* iPosition = positions;
			Vector3* iNextPosition = positions+1;

			float totalPercentErrorResolved(0);
			float originalDistToTarget((positions[nbLinks]-targetPos).norm());
			if (originalDistToTarget < GLM_EPSILON) return true;
			for(unsigned int currentIt=0; currentIt<maxIterations; ++currentIt)
			{
				//STAGE 1: FORWARD REACHING
				positions[nbLinks] = targetPos;
				iNextPosition = positions+nbLinks;
				iPosition = iNextPosition-1;
				for(int i=nbLinks-1; i>=0; --i)
				{
					//positions[i] = positions[i+1] + (positions[i]-positions[i+1]).computeNormalize(lengths[i]);
					*iPosition -= *iNextPosition;
					iPosition->normalizeSelf();
					(*iPosition) *= lengths[i];
					*iPosition += *iNextPosition;
					--iPosition;
					--iNextPosition;
				}

				//STAGE 2: BACKWARD REACHING
				positions[0]=beginPos;
				iPosition = positions;
				iNextPosition = positions+1;
				for(int i=0; i<(int)nbLinks; ++i)
				{
					//positions[i+1] = positions[i] + (positions[i+1]-positions[i]).computeNormalize(lengths[i]);
					*iNextPosition -= *iPosition;
					iNextPosition->normalizeSelf();
					(*iNextPosition) *= lengths[i];
					*iNextPosition += *iPosition;
					++iNextPosition;
					++iPosition;
				}

				//if the end position is already under tolerance, no more iteration
				float distToTarget = (positions[nbLinks]-targetPos).norm();
				totalPercentErrorResolved = 1-(distToTarget/originalDistToTarget);

				if(totalPercentErrorResolved>quality)
					break;
			}

			return totalPercentErrorResolved>quality;
		}

		/**
		* \brief Retrieve the tri-segment of a long finger. 
		* Mcp position is fixed. 
		* Tip position ist given but can bve modified if necessary.
		*
		* \param[in] mcpPos Mcp's position.
		* \param[out] pipPos Pip's position.
		* \param[out] dipPos Dip's position.
		* \param[in,out] tipPos Tip's position.
		* \param[in] planeNorm Normal vector to the plan containg the finger's chain. 
		* \param[in] alpha Angle at mcp around the X axis.
		* \param[in] p1 length of mcp->pip.
		* \param[in] p2 length of pip->dip.
		* \param[in] p3 length of dip->tip.
		* \param[in] side Human body side used for calculation.
		* \return The result of the calculation.
		*/
		static IKResult handFingerConstLength(
			const Vector3& mcpPos, 
			Vector3& pipPos,
			Vector3& dipPos,
			Vector3& tipPos,
			const Vector3& planeNorm,
			Angle alpha,
			float p1,
			float p2,
			float p3,
			const Side::Value side)
		{
			//TODO : traiter les cas particluliers (retour 1 et 2)

            // l'axe x de la main gauche est orienté vers la paume, et celui de la main droite vers l'exterieur
            int sideFactor = ( side == Side::RIGHT )?(-1):(1);

			//Traitement du cas 1 : point inatteignalble : doigt tendu

            // on construit le repère / mcp
            Vector3 vY = planeNorm; 
			vY.normalizeSelf();
			Vector3 vZ = -(tipPos - mcpPos);
			vZ.normalizeSelf();
			Vector3 vX = (vY^vZ);
			vX.normalizeSelf();
			vY = (vZ^vX);
			vY.normalizeSelf();

			//contruction du quaternion du repere (Set) du mcp
			//grace auquel on pourra exprimer des vecteurs dans le 
			//repere global
			RotationMatrix mcpSetMatrix;
			mcpSetMatrix.setAxis_Zy(vZ, vY);
			Quaternion mcpSet;
			toQuaternion(mcpSet, mcpSetMatrix);

            // calcul des coordonnées du point pip, sachant que :
            //	* l'angle de mcp->pip avec mcp->tip est de alpha
            //	* pip appartient au plan de normal vX
            //   * il n'y a qu'une solution étant donné les ddl de mcp

            Vector3 vP(0,0,-(p1 * cosf(alpha)));  //projection du pip sur mcp->tip

			Vector3 vUp(-(p1 * sinf(alpha)) * sideFactor, 0,0); //composante en X

			//position de pip (on exprime le vecteur (vP + vUp) dans le repere du monde
			pipPos = mcpPos + mcpSet * (vP + vUp);

            // on construit le repère / pip
            Vector3 vYPip = vY;
			Vector3 vZPip = -(tipPos-pipPos);
			vZPip.normalizeSelf();
			Vector3 vXPip = (vYPip^vZPip);
			vXPip.normalizeSelf();
			vYPip = (vZPip^vXPip);
			vYPip.normalizeSelf();

			//Traitement du cas 1 : point inatteignalble : doigt tendu
			float distPipToTip = distance(pipPos,tipPos);
			if ( distPipToTip >= ( p2 + p3 ) )
			{
				dipPos = pipPos + ( -p2 * vZPip );
				tipPos = pipPos + ( -(p2+p3) *  vZPip );
				return IK_Unreachable; 
			}
			else
			{
				//contruction du quaternion du repere (Set) du Pip
				//grace auquel on pourra exprimer des vecteurs dans le 
				//repere global
				RotationMatrix pipSetMatrix;
				pipSetMatrix.setAxis_Zy(vZPip, vYPip);
				Quaternion pipSet;
				toQuaternion(pipSet, pipSetMatrix);

				float dz =((p2*p2-p3*p3 + distPipToTip*distPipToTip)/(2*distPipToTip));										
				float dx = p2*p2-dz*dz;
				float dxRoot = sqrtf(dx);

				Vector3 v1(0,0,-dz); 
				Vector3 v2(-dxRoot * sideFactor,0,0);

				//position du dip (on exprime le vecteur (v1 + v2) dans le repere du monde
				dipPos = pipPos + pipSet*(v1 + v2);

                // on construit le repère / dip
                Vector3 vYDip = vY;
				Vector3 vZDip = -(tipPos-dipPos);
				vZDip.normalizeSelf();
				Vector3 vXDip = (vYDip^vZDip);
				vXDip.normalizeSelf();
				vYDip = (vZDip^vXDip);
				vYDip.normalizeSelf();

				//contruction du quaternion du repere (Set) du Dip
				//grace auquel on pourra exprimer des vecteurs dans le 
				//repere global
				RotationMatrix dipSetMatrix;
				dipSetMatrix.setAxis_Zy(vZDip, vYDip);
				Quaternion dipSet;
				toQuaternion(dipSet, dipSetMatrix);

				//on recalcule le tip (on exprime le vecteur (-p3 * vzDip) dans le repere du monde
				tipPos = dipPos + dipSet * (Vector3(0,0,-p3));

				return IK_Reachable;
			}
		}
	};
}
