#include "curves.h"

//--------------------------------------------------
#include "trigger.h"
#include <math.h>

//--------------------------------------------------

#if (1)		// T型速度曲线加减速（四点式，三段）

//--------------------------------------------------

// 计算加减速需要的位移
// 参数: pps1 速度1
// 		 pps2 速度2
//		 ppss 加速度
// 返回:
//		位移值
u32 TTypeCalcDisplacement(float pps1, float pps2, float ppss)
{
	// 公式: d=(v2+v1)*(v2-v1)/(2a)
	//		 v1 = pps1
	//		 v2 = pps2
	// 		 a = ppss
	float temp;

	if (ppss == 0)	// 加速度为0，匀速直线运动
	{
		return 0;
	}

	if (pps1 < pps2)
	{
		temp = (pps2-pps1);
	}
	else
	{
		temp = (pps1-pps2);
	}
	temp *= (pps1+pps2);
	temp /= ppss;
	temp /= 2;
	return (temp+0.5f);
}

//--------------------------------------------------

// 功能：计算某位移下速度变化需要加速度
// 参数：
//		 disp 位移
//		 pps1 速度1
// 		 pps2 速度2
// 返回: 加速度

float TTypeCalcAddPPSS(u32 disp, float pps1, float pps2)
{
	float temp;
	// 公式: a=(v2+v1)*(v2-v1)/(2*d)
	//		 v1 = pps1
	//		 v2 = pps2
	//		 d = disp

	if (disp == 0 || pps1 == pps2)
	{
		return 0;
	}
	if (pps1 < pps2)
	{
		temp = (pps2-pps1);
	}
	else
	{
		temp = (pps1-pps2);
	}
	temp *= (pps1+pps2);
	temp /= disp;
	temp /= 2;
	return (temp);
}

//--------------------------------------------------

// 计算某位移和某加速度下，最终的速度值
// 参数：
//		 disp 位移
//		 pps1 速度1
// 		 ppss 加速度
//
// 返回:
//		 速度值

float TTypeCalcEndSpd(u32 disp, float pps1, float ppss)
{
	float temp;
	// 公式: a=(v2+v1)*(v2-v1)/(2*d)
	//		 v1 = pps1
	//		 d = disp
	// 	最终 pps2 = sqrt(2*disp*ppss + pps1*pps1)

	temp = 2.0f*disp;
	temp *= ppss;
	temp += pps1*pps1;
	if (temp > 0)
	{
		temp = sqrt(temp);
	}
	else
	{
		temp = 0;
	}

	return (temp);
}

//--------------------------------------------------
// 计算运动时间
float TTypeCalcRunTime(u32 disp, u32 startpps, u32 stoppps, u32 runpps, u32 addppss)
{
	TTypeCtrl tctrl;
	float runtime = 0;
	float time = 0;

	memset(&tctrl, 0, sizeof(TTypeCtrl));

	// 计算中间变量和结果
	tctrl.movement = disp;
	tctrl.begPPS = startpps;				// 本段初始速度
	tctrl.endPPS = stoppps;					// 本段结束速度
	tctrl.runPPS = runpps;					// 本段运行速度
	tctrl.maxPPSS = addppss;				// 最大的加速度
	TTypeCalcRunParas(&tctrl);				// 计算加减速参数

	{
		s32 newPPSS1, newPPSS2;

		newPPSS1 = tctrl.newAddPPSS;
		newPPSS2 = tctrl.newAddPPSS;

		if (tctrl.begPPS > tctrl.newRunPPS)
		{
			newPPSS1 *= -1;
		}
		if (tctrl.endPPS > tctrl.newRunPPS)
		{
			newPPSS2 *= -1;
		}

		runtime = 0;
		if (tctrl.mvmtseg[0] != 0 && newPPSS1 != 0)
		{
			time = (int)tctrl.newRunPPS - (int)tctrl.begPPS;
			time /= newPPSS1;
			runtime += time;
		}

		if (tctrl.mvmtseg[1] != 0 && tctrl.newRunPPS != 0)
		{
			time = tctrl.mvmtseg[1];
			time /= (int)tctrl.newRunPPS;
			runtime += time;
		}

		if (tctrl.mvmtseg[2] != 0 && newPPSS2 != 0)
		{
			time = (int)tctrl.newRunPPS - (int)tctrl.endPPS;
			time /= newPPSS2;
			runtime += time;
		}
	//	printf("run time = %.6f\r\n", runtime);
	}

	return runtime;
}

//--------------------------------------------------

// 梯形速度计算
int TTypeCalcRunParas(TTypeCtrl * pTTypeCtrl)
{
#define	TCDEBUG_OUT			0	// 输出调试信息

	int rslt;
// 已知量
// 条件，当前位移段的运动不能换向（速度在一个区间，不会过零）

// 用到的变量
	u32 movement;					// 本段位移量
	u32 begPPS, endPPS, runPPS;
	u32 addPPSS;					// 加速度

	movement = pTTypeCtrl->movement;
	begPPS = pTTypeCtrl->begPPS;
	endPPS = pTTypeCtrl->endPPS;
	runPPS = pTTypeCtrl->runPPS;
	addPPSS = pTTypeCtrl->maxPPSS;

	// 保证运行速度大于最低速度
	if (runPPS < begPPS || runPPS < endPPS)
	{
		runPPS = begPPS > endPPS ? begPPS:endPPS;
	}

	rslt = 0;
	// 1. 判断是否达到最大速度，并计算各个段的位移
	{
		u32 mvmt1, mvmt2, mvmt3;

		mvmt1 = TTypeCalcDisplacement(begPPS, runPPS, addPPSS);
		mvmt3 = TTypeCalcDisplacement(endPPS, runPPS, addPPSS);

		if (mvmt1 + mvmt3 >= movement)		// 不能达到运行速度
		{
			float tmpv;
			tmpv = movement * 2;
			tmpv = sqrt((tmpv*addPPSS + begPPS*begPPS + endPPS*endPPS)/2.0);		// 能够达到的峰值速度
			runPPS = (tmpv);
#if (TCDEBUG_OUT == 1)
			printf("TTypeCalcRunParas runPPS=%d\r\n", runPPS);
#endif
			if (runPPS < begPPS || runPPS < endPPS)		// 如果重新计算的速度小于启动停止速度
			{
				u32 tpmvmt;
				tpmvmt = TTypeCalcDisplacement(begPPS, endPPS, addPPSS);		// 直接变速最小位移
				if (tpmvmt > movement)		// 所需要位移太大，不足以完成加减速，规划失败
				{
					addPPSS = TTypeCalcAddPPSS(movement, begPPS, endPPS);		// 重新计算加速度
#if (TCDEBUG_OUT == 1)
					printf("TTypeCalcRunParas addPPSS=%d\r\n", addPPSS);
#endif
					if (begPPS <= endPPS)
					{
						mvmt1 = movement;
						mvmt2 = 0;
						mvmt3 = 0;
					}
					else
					{
						mvmt1 = 0;
						mvmt2 = 0;
						mvmt3 = movement;
					}
				}
				else
				{
					if (begPPS <= endPPS)
					{
						mvmt1 = tpmvmt;
						mvmt2 = movement - tpmvmt;
						mvmt3 = 0;
					}
					else
					{
						mvmt1 = 0;
						mvmt2 = movement - tpmvmt;
						mvmt3 = tpmvmt;
					}
				}
			}
			else
			{
				mvmt1 = TTypeCalcDisplacement(begPPS, runPPS, addPPSS);
				mvmt3 = TTypeCalcDisplacement(endPPS, runPPS, addPPSS);		// 重新计算位移

				if (mvmt1 + mvmt3 > movement)
				{
					mvmt2 = 0;
					mvmt3 = movement - mvmt1;
				}
				else
				{
					mvmt2 = movement - mvmt1 - mvmt3;
				}
			}
		}
		else		// 可以达到运行速度
		{
			mvmt2 = movement - mvmt1 - mvmt3;
		}

		pTTypeCtrl->mvmtseg[0] = mvmt1;	// 第一段
		pTTypeCtrl->mvmtseg[1] = mvmt2;	// 第二段
		pTTypeCtrl->mvmtseg[2] = mvmt3;	// 第三段
		pTTypeCtrl->newRunPPS = runPPS;
		pTTypeCtrl->newAddPPSS = addPPSS;
	}
#if (TCDEBUG_OUT == 1)
	printf("TTypeCalcRunParas: seg[0]=%d, seg[2]=%d, seg[1]=%d, newRunPPS[1]=%.1f, newAddPPSS=%.1f\r\n",
		pTTypeCtrl->mvmtseg[0], pTTypeCtrl->mvmtseg[2], pTTypeCtrl->mvmtseg[1], pTTypeCtrl->newRunPPS, pTTypeCtrl->newAddPPSS);
#endif

	return rslt;
}
//--------------------------------------------
// 计算分频系数表
int TTypeCalcDivTab(u32 fin, TTypeCtrl * pTTypeCtrl, u32 * pFidvTab, u32 * pTablen)
{
#define	TDEBUG_OUT	0	// 输出调试信息

#if (TDEBUG_OUT == 1)
	u32 time, tptm, lstm;
#endif
	int rslt;
	int totalclk = 0;		// 运行时间（单位是1个时间片）
	u32 movement;

	if (fin <= 0 || pTTypeCtrl == NULL || pFidvTab == NULL || pTablen == NULL)
	{
		return -1;
	}

#if (TDEBUG_OUT == 1)
	time = GetUsSoftTimer();
	lstm = time;
#endif

	// 计算运动参数
	TTypeCalcRunParas(pTTypeCtrl);

	*pTablen = 0;
	rslt = 0;
	// 计算分频系数，生成每个脉冲的分频系数表
	{
		u32 i;
		float begPPS;			// 本段初始速度
		float endPPS;			// 本段结束速度
		float runPPS;			// 本段运行速度
		float newPPSS1, newPPSS2;

		float pps;			// 速度值
		float add;
		float rpps1, rpps2;	// 单精度能够显著提高运算速度
		int runclk;
		int coef;
		u32 * pBuf;

		movement = pTTypeCtrl->movement;
		begPPS = pTTypeCtrl->begPPS;
		endPPS = pTTypeCtrl->endPPS;
		runPPS = pTTypeCtrl->newRunPPS;
		newPPSS1 = pTTypeCtrl->newAddPPSS;
		newPPSS2 = pTTypeCtrl->newAddPPSS;

		if (begPPS > runPPS)
		{
			newPPSS1 *= -1;
		}
		if (endPPS > runPPS)
		{
			newPPSS2 *= -1;
		}

#if (TDEBUG_OUT == 1)
		lstm = GetUsSoftTimer();
#endif
		pps = begPPS;
		rpps1 = pps;
		runclk = 0;
		add = newPPSS1;
		add /= fin;
		pBuf = &(pFidvTab[0]);
		for (i = 0; i < pTTypeCtrl->mvmtseg[0]; i++)		// 第一段
		{
			if (pps > runPPS)
			{
				pps = runPPS;
			}
			if (newPPSS1 < 0 && pps < endPPS)
			{
				pps = endPPS;
			}
			coef = fin / pps;				// 根据速度值得到系数
			*pBuf++ = coef;					// 记录分频系数
			runclk += coef;					// 根据分频系数得到下一个脉冲产生的时间点
			pps = begPPS + runclk * add;	// 根据速度规划，计算下一个脉冲产生时刻的速度值
		}
		rpps1 = pps;
		totalclk += runclk;

#if (TDEBUG_OUT == 1)
		tptm = GetUsSoftTimer();
		printf("TCDiv seg1 tm=%uus\r\n", tptm-lstm);
		lstm = tptm;
#endif
		// 倒序产生第三段
		pps = endPPS;
		rpps2 = pps;
		runclk = 0;
		add = newPPSS2;
		add /= fin;
		pBuf = &(pFidvTab[movement - 1]);
		for (i = 0; i < pTTypeCtrl->mvmtseg[2]; i++)
		{
			if (pps > runPPS)
			{
				pps = runPPS;
			}
			if (newPPSS2 < 0 && pps < endPPS)
			{
				pps = endPPS;
			}
			coef = fin / pps;				// 根据速度值得到系数
			*pBuf-- = coef;					// 记录分频系数
			runclk += coef;					// 根据分频系数得到下一个脉冲产生的时间点
			pps = endPPS + runclk * add;	// 根据速度规划，计算下一个脉冲产生时刻的速度值
		}
		rpps2 = pps;
		totalclk += runclk;
#if (TDEBUG_OUT == 1)
		tptm = GetUsSoftTimer();
		printf("TCDiv seg3 tm=%uus\r\n", tptm-lstm);
		lstm = tptm;
#endif
		// 匀速段
		pps = (rpps1 + rpps2) / 2;
		if (pps > runPPS)
		{
			pps = runPPS;
		}
		if (newPPSS2 < 0 && pps < endPPS)
		{
			pps = endPPS;
		}
		runclk = 0;
		pBuf = &(pFidvTab[pTTypeCtrl->mvmtseg[0]]);
		coef = fin / pps;				// 根据速度值得到系数
		for (i = 0; i < pTTypeCtrl->mvmtseg[1]; i++)
		{
			*pBuf++ = coef;	// 记录分频系数
		}
		runclk = pTTypeCtrl->mvmtseg[1] * coef;
		totalclk += runclk;
#if (TDEBUG_OUT == 1)
		tptm = GetUsSoftTimer();
		printf("TCDiv seg2 tm=%uus\r\n", tptm-lstm);
		lstm = tptm;
#endif
	}

	if (rslt < 0)
	{
		rslt++;
	}
	else
	{
		*pTablen = movement;
		rslt = totalclk;
	}
#if (TDEBUG_OUT == 1)
	tptm = GetUsSoftTimer();
	printf("TCDiv tm=%uus\r\n", tptm-time);
#endif

	return rslt;
}

//--------------------------------------------

#endif

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

#if (0)		// S型速度曲线加减速（七段）


typedef struct
{
// 输入参数
	u32 movement;			// 本段位移量

	float begPPS;			// 本段初始速度
	float endPPS;			// 本段结束速度
	float runPPS;			// 本段运行速度（v）
	float maxPPSS;			// 最大的加速度（a）
	float maxPPSSS;			// 最大的加加速度（j）

// 计算中间变量和结果
	u32 timeseg[7];			// 七段时间长度, 单位为1个时间片
	u32 mvmtseg[7];			// 七段位移, 分段的位移量 (如果位移量不为零, 说明存在这个段)
	float newRunPPS;		// 新的运行速度

}STypeCtrl;

// 计算位移量
u32 STypeCalcDisplacement(float pps1, float pps2, float ppss);

// 计算加速度
float STypeCalcAddPPSS(u32 disp, float pps1, float pps2);

// 计算运动结束的速度
float STypeCalcEndSpd(u32 disp, float pps1, float ppss);

// 计算运动的时间
float STypeCalcRunTime(u32 disp, u32 startpps, u32 stoppps, u32 runpps, u32 addppss);

// 计算运动相关参数
int STypeCalcRunParas(STypeCtrl * pSTypeCtrl);

// 计算生成分频系数表
int STypeCalcDivTab(u32 fin, STypeCtrl * pSTypeCtrl, u32 * pFidvTab, u32 * pTablen);










#endif