/*================================================================
File:			Dynamic.h
Prupose:		Dynamic parameters of a connection
Author:			Yann Le Tensorer
Creation Date:	29 january 1997
------------------------------------------------------------------
Revisions:		1.01	Yann Le Tensorer
						4 february 1997 
						Added function fn_vDynamicObject_GetParams
=================================================================*/
#ifdef _AI_LIB_
#include "AIUseCPA.h"
#include "dynamic.h"
#include "IAOption.h"
#include "IAMacros.h"
/* AI memory and error management */
#include "MemIA.h"
#include "ErrIA.h"
#else /*_AI_LIB_*/
#include "acp_base.h"
#include "dynamic.h"
#include "malloc.h"
#endif /*_AI_LIB_*/

/*============================================================
Memory management macros, to be replaced with correct memory management
============================================================*/
#ifdef _AI_LIB_
#define M_malloc(pointer,cast,size) M_IAAlloc(pointer,cast,size)
#define M_free(pointer) M_IAFree(pointer)
#else /*_AI_LIB_*/
#define M_malloc(pointer,cast,size) (pointer=(cast)malloc(size))
#define M_free(pointer) (free(pointer))
#endif /*_AI_LIB_*/

/*==========================================================
Function name:	fn_vDynamicObject_Calculate
Description:	Caculates the speeds at sampled points, according to dynamic parameters
Input:			p_stDynaParam:	pointer to a pre-created tdstDynaParam structure
Output:			none
Author:			Yann Le Tensorer
Date:			30 january 1997
Revision:
==========================================================*/
void fn_vDynamicObject_Calculate(ACP_tdstDynaParam* p_stDynaParam)
{
	MTH_tdxReal *SpeedList;
	MTH_tdxReal *SpeedListEnd;
	MTH_tdxReal CurSpeed;
	MTH_tdxReal inc;
	MTH_tdxReal MaxSpeed;
	MTH_tdxReal CurAngle;

	if (p_stDynaParam==0) return; /* avoid unexepected crash */

	switch (p_stDynaParam->ucDynamicType)
	{
	case C_ucNone: 
	case C_ucConst:
				break;
	case C_ucLinear:
				inc=(p_stDynaParam->xEndSpeed-p_stDynaParam->xStartSpeed)/p_stDynaParam->ucSamplingRate;
				SpeedList	=	p_stDynaParam->d_stListOfSpeeds;									/* start adress */
				if (SpeedList==0) break; /* avoid unexpected crash */
				SpeedListEnd=	p_stDynaParam->d_stListOfSpeeds+p_stDynaParam->ucSamplingRate;		/* end adress */
				CurSpeed	=	p_stDynaParam->xStartSpeed;	

				for (;SpeedList<=SpeedListEnd;SpeedList++)
				{
					*SpeedList=CurSpeed;
					CurSpeed+=inc;
				}
				break;
	case C_ucSinus:
				MaxSpeed=(p_stDynaParam->xMaxSpeed)/2;
				inc=(p_stDynaParam->xEndAngle-p_stDynaParam->xStartAngle)/p_stDynaParam->ucSamplingRate;
				SpeedList	=	p_stDynaParam->d_stListOfSpeeds;									/* start adress */
				if (SpeedList==0) break; /* avoid unexpected crash */
				SpeedListEnd=	p_stDynaParam->d_stListOfSpeeds+p_stDynaParam->ucSamplingRate;		/* end adress */
				CurAngle	=	p_stDynaParam->xStartAngle;	

				for (;SpeedList<=SpeedListEnd;SpeedList++)
				{
					*SpeedList=(MTH_M_xSin(CurAngle)+1)*MaxSpeed;
					CurAngle+=inc;
				}
				break;
				
				
	}
}


/*==========================================================
Function name:	fn_vDynamicObject_Create
Description:	Creates a dynamic parameters object
Input:			p_stDynaParam:	pointer to a preallocated tdstDynaParam structure
				ucSamplingRate:	sampling rate
				ucDynamicType:	dynamic type (C_NONE,C_CONST,C_LINEAR,C_SINUS)
				xFirst,xSecond,xThird:	See table below

				Type:	|	C_NONE	|	C_CONST	|	C_LINEAR	|	C_SINUS
				===========================================================
				xFirst	|	  -		|	speed	|	start speed	|	start angle
						|			|			|				|
				xSecond	|	  -		|	  -		|	end speed	|	end angle
						|			|			|				|
				xThird	|	  -		|	  -		|	    -		|	max speed

				
Output:			none
Author:			Yann Le Tensorer
Date:			30 january 1997
Revision:		5 february 1997 : corrected minor bugs
==========================================================*/

void fn_vDynamicObject_Create(	ACP_tdstDynaParam*	p_stDynaParam,
								unsigned char		ucSamplingRate,
								unsigned char		ucDynamicType,
								MTH_tdxReal			xFirst,
								MTH_tdxReal			xSecond,
								MTH_tdxReal			xThird)
{
	if (p_stDynaParam==0) return;

	p_stDynaParam->ucSamplingRate=ucSamplingRate;
	if (p_stDynaParam->ucSamplingRate<2) p_stDynaParam->ucSamplingRate=2;

	p_stDynaParam->ucDynamicType=ucDynamicType;
	switch (ucDynamicType)
	{
	case C_ucNone: 
				p_stDynaParam->xSpeed=0;
				p_stDynaParam->xEndSpeed=0;
				p_stDynaParam->xMaxSpeed=0;
				p_stDynaParam->d_stListOfSpeeds=0;
				break;
	case C_ucConst:
				p_stDynaParam->xSpeed=xFirst;
				p_stDynaParam->xEndSpeed=0;
				p_stDynaParam->xMaxSpeed=0;
				p_stDynaParam->d_stListOfSpeeds=0;
				break;
	case C_ucLinear:
				p_stDynaParam->xStartSpeed=xFirst;
				p_stDynaParam->xEndSpeed=xSecond;
				p_stDynaParam->xMaxSpeed=0;
				M_malloc(p_stDynaParam->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParam->ucSamplingRate+1));
				break;
	case C_ucSinus:
				p_stDynaParam->xStartAngle=xFirst;
				p_stDynaParam->xEndAngle=xSecond;
				p_stDynaParam->xMaxSpeed=xThird;
				M_malloc(p_stDynaParam->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParam->ucSamplingRate+1));
				break;
				
	}
	fn_vDynamicObject_Calculate(p_stDynaParam);
}

/*==========================================================
Function name:	fn_vDynamicObject_ChangeSamplingRate
Description:	Changes the sampling rate of a dynamic object
Input:			p_stDynaParam:	pointer to a preallocated tdstDynaParam structure
				ucSamplingRate:	sampling rate
Output:			none
Author:			Yann Le Tensorer
Date:			5 february 1997
Revision:
==========================================================*/

void fn_vDynamicObject_ChangeSamplingRate(	ACP_tdstDynaParam*	p_stDynaParams,
											unsigned char		ucSamplingRate)
								
{
	if (p_stDynaParams==0) return;

	p_stDynaParams->ucSamplingRate=ucSamplingRate;
	if (p_stDynaParams->ucSamplingRate<2) p_stDynaParams->ucSamplingRate=2;

	switch (p_stDynaParams->ucDynamicType)
	{
	case C_ucNone: 
				p_stDynaParams->d_stListOfSpeeds=0;
				break;
	case C_ucConst:
				p_stDynaParams->d_stListOfSpeeds=0;
				break;
	case C_ucLinear:
				if (p_stDynaParams->d_stListOfSpeeds!=0) 
					M_free(p_stDynaParams->d_stListOfSpeeds);
				M_malloc(p_stDynaParams->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParams->ucSamplingRate+1));
				break;
	case C_ucSinus:
				if (p_stDynaParams->d_stListOfSpeeds!=0) 
					M_free(p_stDynaParams->d_stListOfSpeeds);
				M_malloc(p_stDynaParams->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParams->ucSamplingRate+1));
				break;
				
	}
	fn_vDynamicObject_Calculate(p_stDynaParams);
}


/*==========================================================
Function name:	fn_vDynaParams_Free
Description:	removes the allocated ram for DynaParams
Input:			p_stDynaParam:	pointer to a pre-created tdstDynaParam structure
Output:			none
Author:			Yann Le Tensorer
Date:			30 january 1997
Revision:
==========================================================*/
void fn_vDynamicObject_Free(ACP_tdstDynaParam* p_stDynaParams)
{
	if (p_stDynaParams!=0)
		if (p_stDynaParams->d_stListOfSpeeds!=0) 
			M_free(p_stDynaParams->d_stListOfSpeeds);
}


/*==========================================================
Function name:	fn_vDynamicObject_ChangeParams
Description:	changes the dynamic parameters  (except sampling rate)
Input:			p_stDynaParam:	pointer to a preallocated tdstDynaParam structure
				ucDynamicType:	dynamic type (C_ucNone,C_ucConst,C_ucLinear,C_ucSinus)
				xFirst,xSecond,xThird:	See table below

				Type:	| C_ucNone	| C_ucConst	|	C_ucLinear	|	C_ucSinus
				===========================================================
				xFirst	|	  -		|	speed	|	start speed	|	start angle
						|			|			|				|
				xSecond	|	  -		|	  -		|	end speed	|	end angle
						|			|			|				|
				xThird	|	  -		|	  -		|	    -		|	max speed

				
Output:			none
Author:			Yann Le Tensorer
Date:			30 january 1997
Revision:
==========================================================*/

void fn_vDynamicObject_ChangeParams(	ACP_tdstDynaParam*	p_stDynaParam,
						  				unsigned char		ucDynamicType,
										MTH_tdxReal			xFirst,
										MTH_tdxReal			xSecond,
										MTH_tdxReal			xThird)
{
	p_stDynaParam->ucDynamicType=ucDynamicType;
	switch (ucDynamicType)
	{
	case C_ucNone: 
				p_stDynaParam->xSpeed=0;
				p_stDynaParam->xEndSpeed=0;
				p_stDynaParam->xMaxSpeed=0;
				if (p_stDynaParam->d_stListOfSpeeds!=0)
				{
					M_free(p_stDynaParam->d_stListOfSpeeds);
					p_stDynaParam->d_stListOfSpeeds=0;
				}
				break;
	case C_ucConst:
				p_stDynaParam->xSpeed=xFirst;
				p_stDynaParam->xEndSpeed=0;
				p_stDynaParam->xMaxSpeed=0;
				if (p_stDynaParam->d_stListOfSpeeds!=0)
				{
					M_free(p_stDynaParam->d_stListOfSpeeds);
					p_stDynaParam->d_stListOfSpeeds=0;
				}
				break;
	case C_ucLinear:
				p_stDynaParam->xStartSpeed=xFirst;
				p_stDynaParam->xEndSpeed=xSecond;
				p_stDynaParam->xMaxSpeed=0;
				if (p_stDynaParam->d_stListOfSpeeds==0)
					M_malloc(p_stDynaParam->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParam->ucSamplingRate+1));
				
				break;
	case C_ucSinus:
				p_stDynaParam->xStartAngle=xFirst;
				p_stDynaParam->xEndAngle=xSecond;
				p_stDynaParam->xMaxSpeed=xThird;
				if (p_stDynaParam->d_stListOfSpeeds==0)
					M_malloc(p_stDynaParam->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParam->ucSamplingRate+1));
				break;
				
				
	}
	fn_vDynamicObject_Calculate(p_stDynaParam);

}

/*==========================================================
Function name:	fn_vDynamicObject_GetParams
Description:	Gets the dynamic parameters  (including sampling rate)
Input:			p_stDynaParam:	pointer to a preallocated tdstDynaParam structure
				p_ucDynamicType:pointer to get dynamic type (C_ucNone,C_ucConst,C_ucLinear,C_ucSinus)
				p_SamplingRate:	pointer to get sampling rate
				p_xFirst,p_xSecond,p_xThird:pointers to parameters to get See table below

				Type:	| C_ucNone	| C_ucConst	|	C_ucLinear	|	C_ucSinus
				===========================================================
				xFirst	|	  -		|	speed	|	start speed	|	start angle
						|			|			|				|
				xSecond	|	  -		|	  -		|	end speed	|	end angle
						|			|			|				|
				xThird	|	  -		|	  -		|	    -		|	max speed

				
Output:			none
Author:			Yann Le Tensorer
Date:			04 february 1997
Revision:

Note:			Programmer may call the function with null pointers.
				Only non-null pointers will be affected...
==========================================================*/

void fn_vDynamicObject_GetParams(	ACP_tdstDynaParam*	p_stDynaParam,
						  			unsigned char*		p_ucDynamicType,
									unsigned char*		p_ucSamplingRate,
									MTH_tdxReal*		p_xFirst,
									MTH_tdxReal*		p_xSecond,
									MTH_tdxReal*		p_xThird)
{
	if (p_stDynaParam==0) return;

	if (p_ucDynamicType!=0)		*p_ucDynamicType=p_stDynaParam->ucDynamicType;
	if (p_ucSamplingRate!=0)	*p_ucSamplingRate=p_stDynaParam->ucSamplingRate;
	switch (p_stDynaParam->ucDynamicType)
	{
	case C_ucNone: 
				if (p_xFirst!=0)	*p_xFirst	=0;
				if (p_xSecond!=0)	*p_xSecond	=0;
				if (p_xThird!=0)	*p_xThird	=0;
				break;
	case C_ucConst:
				if (p_xFirst!=0)	*p_xFirst	=p_stDynaParam->xSpeed;
				if (p_xSecond!=0)	*p_xSecond	=0;
				if (p_xThird!=0)	*p_xThird	=0;
				break;
	case C_ucLinear:
				if (p_xFirst!=0)	*p_xFirst	=p_stDynaParam->xStartSpeed;
				if (p_xSecond!=0)	*p_xSecond	=p_stDynaParam->xEndSpeed;
				if (p_xThird!=0)	*p_xThird	=0;
				break;
	case C_ucSinus:
				if (p_xFirst!=0)	*p_xFirst	=p_stDynaParam->xStartAngle;
				if (p_xSecond!=0)	*p_xSecond	=p_stDynaParam->xEndAngle;
				if (p_xThird!=0)	*p_xThird	=p_stDynaParam->xMaxSpeed;
				break;
	default:	
				break;
				
				
	}
	
}

/*=========================================================
Function name:	fn_xDynamicObject_GetSpeed
Description:	returns the speed at a given sample (n° of the point)
Input:			p_stDynaParam:	pointer to a pre-created tdstDynaParam structure
				iSampleNo:		n° of the point to get the speed from.
Output:			return value is the speed.
Author:			Yann Le Tensorer
Date:			31 january 1997
==========================================================*/
MTH_tdxReal fn_xDynamicObject_GetSpeed(ACP_tdstDynaParam* p_stDynaParams,unsigned char ucSampleNo)
{

	MTH_tdxReal xReturn;

	if (p_stDynaParams==0) return 0;

	switch (p_stDynaParams->ucDynamicType)
	{
	case C_ucNone: 
				xReturn=0;
				break;
	case C_ucConst:
				xReturn=p_stDynaParams->xSpeed;
				break;
	case C_ucLinear:
	case C_ucSinus:
				if (ucSampleNo<=p_stDynaParams->ucSamplingRate && p_stDynaParams->d_stListOfSpeeds!=0)
					xReturn=p_stDynaParams->d_stListOfSpeeds[ucSampleNo];
				else
					xReturn=0;
				
				break;
	default:	xReturn=0;			
	}

	return xReturn;

	
}

/*==========================================================
Function name:	fn_vDynamicObject_CreateNotSampled
Description:	Creates a dynamic parameters object for not sampled type
Input:			p_stDynaParam:	pointer to a preallocated tdstDynaParam structure
				ucDynamicType:	dynamic type (C_NONE,C_CONST,C_LINEAR,C_SINUS)
				xFirst,xSecond,xThird:	See table below

				Type:	 |	C_NONE	|	C_CONST	|	C_LINEAR	   |	C_SINUS
				===========================================================
				xFirst |	  -		  |	speed	  |	start speed	|	start angle
						     |			     |			      |				         |
				xSecond|	  -		  |	  -		   |	end speed	  |	end angle
						     |			     |			      |				         |
				xThird	|	  -		  |	  -		   |	    -		     |	max speed

				
Output:			none
Author:			Albert Pais
Date:			February 04, 1997
Revision:
==========================================================*/
void fn_vDynamicObject_CreateNotSampled
(
	ACP_tdstDynaParam*	p_stDynaParam,
	unsigned char		ucDynamicType,
	MTH_tdxReal			xFirst,
	MTH_tdxReal			xSecond,
	MTH_tdxReal			xThird
)
{
	p_stDynaParam->ucDynamicType=ucDynamicType;
	switch (ucDynamicType)
	{
	case C_ucNone: 
				p_stDynaParam->xSpeed=0;
				p_stDynaParam->xEndSpeed=0;
				p_stDynaParam->xMaxSpeed=0;
				p_stDynaParam->d_stListOfSpeeds=0;
				break;
	case C_ucConst:
				p_stDynaParam->xSpeed=xFirst;
				p_stDynaParam->xEndSpeed=0;
				p_stDynaParam->xMaxSpeed=0;
				p_stDynaParam->d_stListOfSpeeds=0;
				break;
	case C_ucLinear:
				p_stDynaParam->xStartSpeed=xFirst;
				p_stDynaParam->xEndSpeed=xSecond;
				p_stDynaParam->xMaxSpeed=0;
				break;
	case C_ucSinus:
				p_stDynaParam->xStartAngle=xFirst;
				p_stDynaParam->xEndAngle=xSecond;
				p_stDynaParam->xMaxSpeed=xThird;
				break;				
	}
}

/*==========================================================
Function name:	fn_xDynamicObject_GetSpeedNotSampled
Description:	returns the speed at a given position when it is not sampled
Input:
p_stDynaParam:	pointer to a pre-created tdstDynaParam structure
_xDist : the dist beetween the position where speed is required and the point to reach
Output:			return value is the speed.
Author:			Albert Pais
Date:			February 4,1997
Revision:
==========================================================*/
MTH_tdxReal fn_xDynamicObject_GetSpeedNotSampled
(
 ACP_tdstDynaParam* _p_stDynaParams,
 MTH_tdxReal _xDist
)
{
	MTH_tdxReal xReturn;

	switch (_p_stDynaParams->ucDynamicType)
	{
	case C_ucNone: 
				xReturn=0;
				break;
	case C_ucConst:
				xReturn=_p_stDynaParams->xSpeed;
				break;
	case C_ucLinear:
  /* Speed = InitialSpeed + CurrentDist x (FinalSpeed-InitialSpeed) / InitialDist */
  /* tip : (FinalSpeed-InitialSpeed) / InitialDist  is stored inside xSpeed field */
  xReturn = MTH_M_xAdd
    (
     _p_stDynaParams->xStartSpeed,
     MTH_M_xMul(_xDist,_p_stDynaParams->xSpeed )
    );
  break;
	case C_ucSinus:
  /* Speed = MaxSpeed x 
    (1+sin( (FinalAngle - InitialAngle)/InitialDist * CurrentDist + InitialAngle )) */
  /* tip : (FinalAngle - InitialAngle)/InitialDist  is stored inside xSpeed field */
  xReturn =  MTH_M_xMul
  (
   (_p_stDynaParams->xMaxSpeed),
   MTH_M_xAdd
   (
    MTH_C_ONE,
    MTH_M_xSin
    (
     MTH_M_xAdd
     (
      MTH_M_xMul
      (
       _p_stDynaParams->xSpeed,
       _xDist
      ),
      _p_stDynaParams->xStartAngle
     )
    )
   )
  );
		break;
	default:	xReturn=0;			
	}

	return xReturn;
}

/*==========================================================
Function name:	fn_vDynamicObject_CalculateNotSampled
Description:	Caculates specific coef when speed is not sampled
Input:
p_stDynaParam:	pointer to a pre-created tdstDynaParam structure
the initial dist beetween the initial point and the final point
Output:			none
Remark :
The xSpeed field of the structure is used to store some
parameters used at execution
Author:			Albert Pais
Date:			Frebruary 4,1997
Revision:
==========================================================*/
void fn_vDynamicObject_CalculateNotSampled
(
 ACP_tdstDynaParam* _p_stDynaParam,
 MTH_tdxReal _xInitialDist
)
{
	switch (_p_stDynaParam->ucDynamicType)
	{
	case C_ucNone: 
	case C_ucConst:
  /* nothing to compute */
				break;
	case C_ucLinear:
	  if(_xInitialDist)
   {/* xSpeed is used to store a specific coef :*/
    _p_stDynaParam->xSpeed = 
     MTH_M_xDiv
     (
      MTH_M_xSub
      (
       (_p_stDynaParam->xEndSpeed),
       (_p_stDynaParam->xStartSpeed)
      ),
      _xInitialDist
     );
   }
   else
   {
    _p_stDynaParam->xSpeed = MTH_C_ZERO;
   }
			break;
	case C_ucSinus:
  if(_xInitialDist)
  {/* xSpeed is used to store a specific coef :*/
   _p_stDynaParam->xSpeed = 
    MTH_M_xDiv
    (
     MTH_M_xSub
     (
      (_p_stDynaParam->xEndAngle),
      (_p_stDynaParam->xStartAngle)
     ),
     _xInitialDist
    );
  }
  else
  {
   _p_stDynaParam->xSpeed = MTH_C_ZERO;
  }
		break;
	}
}

