#include "StdAfx.h"
#include "MotionInfo_Axis.h"
#include <math.h>

CMotionInfo_Axis::CMotionInfo_Axis(int nIndex)
{
	memset(this, 0, sizeof(CMotionInfo_Axis));
	m_nIndex = nIndex;
}

CMotionInfo_Axis::~CMotionInfo_Axis()
{

}

// getter
int CMotionInfo_Axis::GetIndex() const
{
	return m_nIndex;
}

double CMotionInfo_Axis::GetVelocity() const				
{ 
	return m_dVelocity;
}	

double CMotionInfo_Axis::GetAccelTime() const			
{ 
	return m_dAccelTime; 
}	

double CMotionInfo_Axis::GetDecelTime() const			
{ 
	return m_dDecelTime; 
}	

double CMotionInfo_Axis::GetStartPos() const				
{ 
	return m_dStartPos;
}	

double CMotionInfo_Axis::GetEndPos() const				
{ 
	return m_dEndPos;
}	

int	CMotionInfo_Axis::GetMoveDirection() const		
{ 
	return m_nMoveDirection; 
}	

double	CMotionInfo_Axis::GetCurVelocity() const			
{
	return (m_nMoveDirection==0) ? 0: m_dCurVelocity; 
}	

double	CMotionInfo_Axis::GetCurAccelTime() const			
{ 
	return (m_nMoveDirection==0) ? 0: m_dCurAccelTime; 
}	

double	CMotionInfo_Axis::GetCurDecelTime() const			
{ 
	return (m_nMoveDirection==0) ? 0: m_dCurDecelTime; 
}	

double	CMotionInfo_Axis::GetCurFixTime() const			
{ 
	return (m_nMoveDirection==0) ? 0: m_dCurFixTime; 
}	

double	CMotionInfo_Axis::GetCurTotalTime() const			
{ 
	return (m_nMoveDirection==0) ? 0: m_dCurTotalTime; 
}	

double	CMotionInfo_Axis::GetCurAccelDist() const			
{
	return (m_nMoveDirection==0) ? 0: m_dCurAccelDist;
}	

double	CMotionInfo_Axis::GetCurDecelDist() const			
{ 
	return (m_nMoveDirection==0) ? 0: m_dCurDecelDist;
}	

double	CMotionInfo_Axis::GetCurFixDist() const			
{ 
	return (m_nMoveDirection==0) ? 0: m_dCurFixDist; 
}	

double	CMotionInfo_Axis::GetCurTotalDist() const			
{ 
	return (m_nMoveDirection==0) ? 0: m_dCurTotalDist; 
}	

// setter
void CMotionInfo_Axis::SetVelocity(double dVelocity) 	
{ 
	if (dVelocity!=m_dVelocity)
	{
		m_dVelocity = dVelocity;
		CalculateValue();
	}
}	

void CMotionInfo_Axis::SetAccelTime(double dAccelTime) 
{ 
	if (dAccelTime!=m_dVelocity)
	{
		m_dAccelTime = dAccelTime;
		CalculateValue();
	}
}	

void CMotionInfo_Axis::SetDecelTime(double dDecelTime) 
{ 
	if (dDecelTime!=m_dVelocity)
	{
		m_dDecelTime = dDecelTime;
		CalculateValue();
	}
}	

void CMotionInfo_Axis::SetStartPos(double dStartPos) 	
{ 
	if (dStartPos!=m_dVelocity)
	{
		m_dStartPos = dStartPos;
		CalculateValue();
	}
}	

void CMotionInfo_Axis::SetEndPos(double dEndPos) 		
{ 
	if (dEndPos!=m_dVelocity)
	{
		m_dEndPos = dEndPos;
		CalculateValue();
	}
}	

void CMotionInfo_Axis::SetData(double dVelocity, double dAccelTime, double dDecelTime, double dStartPos, double dEndPos)
{
	if (dVelocity!=m_dVelocity)
	{
		m_dVelocity = dVelocity;
		m_nMoveDirection = 0;
	}

	if (dAccelTime!=m_dVelocity)
	{
		m_dAccelTime = dAccelTime;
		m_nMoveDirection = 0;
	}

	if (dDecelTime!=m_dVelocity)
	{
		m_dDecelTime = dDecelTime;
		m_nMoveDirection = 0;
	}

	if (dStartPos!=m_dVelocity)
	{
		m_dStartPos = dStartPos;
		m_nMoveDirection = 0;
	}

	if (dEndPos!=m_dVelocity)
	{
		m_dEndPos = dEndPos;
		m_nMoveDirection = 0;
	}

	if (m_nMoveDirection==0) CalculateValue();
}

int CMotionInfo_Axis::CalculateMotion(double dTime, double& dPos, double& dVelocity, BOOL& bEnd) 
{
	// return : 1 °è»ê¼º°ø, 0 °è»ê½ÇÆÐ, -1 ½Ã°£¿À¹ö
	// [in] dTime : ¼Ò¿ä½Ã°£ (sec)
	// [out] dPos : ÇöÀçÀ§Ä¡ (mm)
	// [out] dSpeed : ÇöÀç ¼Óµµ (mm/sec)
	// [out] bEnd : ¿Ï·áÀ¯¹«
	if (m_nIndex==-1) return 0;

	if (m_nMoveDirection==0) // is not calculated
	{
		if (CalculateValue()==0)
		{
			return 0;
		}
	}

	if (m_dCurTotalDist==0.0) // À̵¿°Å¸®°¡ '0' À̸é
	{
		dPos		= m_dStartPos;
		dVelocity	= 0.0;
		bEnd		= TRUE;
		return 1;
	}

	// 1. ±¸°£º° °è»ê
	if (dTime < m_dCurAccelTime) // [°¡¼Ó±¸°£]
	{
		double dA	= (m_dCurVelocity / m_dCurAccelTime);
		dVelocity	= (dA * dTime);																// ¼Óµµ
		dPos		= (dVelocity * dTime) / 2.0;												// À§Ä¡
	}
	else if (dTime < (m_dCurAccelTime + m_dCurFixTime)) // [Á¤¼Ó±¸°£]
	{
		dTime		= dTime - m_dCurAccelTime;
		dVelocity	= m_dCurVelocity;															// ¼Óµµ
		dPos		= m_dCurAccelDist + (dTime * dVelocity);									// À§Ä¡
	}
	else if (dTime < m_dCurTotalTime) // [°¨¼Ó±¸°£]
	{
		double dA	= (m_dCurVelocity / m_dCurDecelTime);
		dTime		= dTime - (m_dCurAccelTime + m_dCurFixTime);
		dVelocity	= m_dCurVelocity - (dA * dTime);											// ¼Óµµ
		double dV	= (m_dCurVelocity-dVelocity);
		dPos		= m_dCurAccelDist + m_dCurFixDist + ((dVelocity*dTime) + (dV*dTime)/2.0);	// À§Ä¡

	}
	else // [À̵¿¿Ï·á]
	{
		dPos		= m_dEndPos;
		dVelocity	= 0.0;
		bEnd		= TRUE;
		return 1;
	}

	dPos *= m_nMoveDirection; // set direction

	return 1;
}

int CMotionInfo_Axis::CalculateValue()
{
	m_nMoveDirection = 0;

	if (m_dVelocity<0.0) return 0;

	m_dCurTotalDist = (m_dEndPos-m_dStartPos);						// ÃÑÀ̵¿°Å¸®

	if (m_dCurTotalDist==0.0) // À̵¿°Å¸®°¡ 0 À̸é
	{
		return 1;
	}

	// 0. °ü·Ã º¯¼ö °è»ê
	int nMoveDirection = (m_dCurTotalDist>0.0) ? 1: -1;						// À̵¿¹æÇâ

	// 1.ÇöÀç °¡¼Ó ½Ã°£ ¹× °Å¸®
	m_dCurAccelTime = m_dAccelTime;
	m_dCurAccelDist = 0.0;												// °¡¼Ó À̵¿°Å¸®
	if (m_dCurAccelTime!=0.0)											// °¡¼Ó ½Ã°£ÀÌ 0 ÀÌ ¾Æ´Ï¸é
	{
		m_dCurAccelDist = (m_dCurAccelTime * m_dVelocity) / 2.0;		// °¡¼Ó½Ã À̵¿°Å¸®
	}

	// 2.ÇöÀç °¨¼Ó ½Ã°£ ¹× °Å¸®
	m_dCurDecelTime = m_dDecelTime;
	m_dCurDecelDist = 0.0;												// °¨¼Ó À̵¿°Å¸®				
	if (m_dCurDecelTime!=0.0)											// °¨¼Ó ½Ã°£ÀÌ 0 ÀÌ ¾Æ´Ï¸é
	{
		m_dCurDecelDist = (m_dCurDecelTime * m_dVelocity) / 2.0;		// °¨¼Ó À̵¿°Å¸®
	}

	// ÇöÀç °¡°¨¼Ó ½Ã°£ ¹× °Å¸® Àç°è»ê
	m_dCurVelocity = m_dVelocity;
	if ( (nMoveDirection*m_dCurTotalDist) < (m_dCurAccelDist+m_dCurDecelDist) )				// °¡°¨¼Ó½Ã À̵¿°Å¸®°¡ ÃÑ À̵¿°Å¸®º¸´Ù Ŭ¶§
	{
		double dA	= (m_dVelocity / m_dCurAccelTime);

		m_dCurAccelDist = m_dCurDecelDist = ((nMoveDirection*m_dCurTotalDist) / 2.0);

		m_dCurAccelTime = sqrt( (2.0*m_dCurAccelDist) / dA );
		m_dCurDecelTime = sqrt( (2.0*m_dCurDecelDist) / dA );

		m_dCurVelocity = (2.0*m_dCurAccelDist) / m_dCurAccelTime;
	}

	// 3.ÇöÀç µî¼Ó ½Ã°£ ¹× °Å¸®
	m_dCurFixTime = 0.0;
	m_dCurFixDist = (nMoveDirection*m_dCurTotalDist) - (m_dCurAccelDist+m_dCurDecelDist);	// Á¤¼Ó½Ã À̵¿°Å¸® °è»ê
	if (m_dCurFixDist!=0.0)																// Á¤¼Ó½Ã À̵¿°Å¸®°¡ 0 ÀÌ ¾Æ´Ò¶§
	{
		m_dCurFixTime = m_dCurFixDist / m_dVelocity;											// Á¤¼Ó½Ã À̵¿½Ã°£
	}	

	// 4. ÃÑ À̵¿ ½Ã°£.
	m_dCurTotalTime = m_dCurAccelTime + m_dCurFixTime + m_dCurDecelTime;				// ÃÑ À̵¿½Ã°£

	m_nMoveDirection = nMoveDirection;

	return 1;
}