// Motionplanner.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "Motionplanner.h"
#define MOTIONPLANNER_CYCLE_TIME 2
struct tagMotorMove tMM;
int AdjustTA_TS(double* pdTA,double* pdTS)
{
	if(pdTA&&pdTS)
	{
		if(*pdTA < 0)
			*pdTA = -*pdTA;
		if(*pdTS < 0)
			*pdTS = -*pdTS;
		if(*pdTS<MOTIONPLANNER_CYCLE_TIME)
			*pdTS = 0;
		if(*pdTA<MOTIONPLANNER_CYCLE_TIME)
			*pdTA = MOTIONPLANNER_CYCLE_TIME;
		if(*pdTA<*pdTS*2)
			*pdTA = *pdTS*2;
		return 0;
	}
	else
	{
		return -1; //Null point
	}
}

int GetCurrentPVA(double* pdA0,double* pdV0,double* pdP0)
{
	return 0;
}
int MoveVelToZero(double dA0,double dV0,double dP0,double dTA,double dTS,double* pdJ1,double* pdJ2,double* pdA1,double* pdP1 )
{
	double dVTemp,dATemp;
	if(dTS>0)
	{
		*pdJ1 = -dA0/dTA;
		dVTemp = -(dV0+0.5*dA0*dTA);
		dATemp = dVTemp/dTA;
		*pdJ2 = dATemp/dTS;
		*pdA1 = 0;
		*pdP1 = dP0+0.5*dATemp*dTA*dTA+0.166666667**pdJ1*dTA*dTA*dTA+dV0*dTA+0.5*dA0*dTA*dTA;
	}
	else
	{
		*pdJ1 = 0;
		*pdJ2 = 0;
		*pdA1 = -dV0/dTA;
		*pdP1 = dP0+0.5*dV0*dTA;
	}
	return 0;
}
int InitialJogMove(void)
{
	double dA0=0,dV0=0,dP0=0;
	double dJ1,dJ2,dA1,dP1;

	GetCurrentPVA(&dA0,&dV0,&dP0);
	MoveVelToZero(dA0,dV0,dP0,dTA,dTS,&dJ1,&dJ2,&dA1,&dP1);
	return 0;
}
int MotorJogMove(struct tagMotorMove* pMM)
{
	switch(pMM->nSeg )
	{
		  
	}
	return 0;
}
int main(int argc, char* argv[])
{
	
	double dTA,dTS;
	dTA = -2;dTS=-3;
	if(AdjustTA_TS(&dTA,&dTS)>0)
	{
		InitialMove();		
	}
	while(MotorJogMove() < JOG_END)
	{

	}
	return 0;
}