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

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

	m_nCurStatus = 1;
	m_nIndex = nIndex;
}

CMotionInfo_Axis::~CMotionInfo_Axis()
{

}

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

double CMotionInfo_Axis::GetMoveVelocity() const				
{ 
	return m_dMoveVelocity;
}	

double CMotionInfo_Axis::GetMoveAccelTime() const			
{ 
	return m_dMoveAccelTime; 
}	

double CMotionInfo_Axis::GetMoveDecelTime() const			
{ 
	return m_dMoveDecelTime; 
}	

double CMotionInfo_Axis::GetMoveStartPos() const				
{ 
	return m_dMoveStartPos;
}	

double CMotionInfo_Axis::GetMoveEndPos() const				
{ 
	return m_dMoveEndPos;
}	

double CMotionInfo_Axis::GetJogVelocity() const
{
	return m_dJogVelocity;
}

double CMotionInfo_Axis::GetJogAccelTime() const
{
	return m_dJogAccelTime;
}

double CMotionInfo_Axis::GetJogDecelTime() const
{
	return m_dJogDecelTime;
}

double CMotionInfo_Axis::GetJogStartPos() const
{
	return m_dJogStartPos;
}

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::SetMoveVelocity(double dVelocity) 	
{ 
	if (dVelocity!=m_dMoveVelocity)
	{
		m_dMoveVelocity = dVelocity;
		CalculateMoveValue();
	}
}	

void CMotionInfo_Axis::SetMoveAccelTime(double dAccelTime) 
{ 
	if (dAccelTime!=m_dMoveAccelTime)
	{
		m_dMoveAccelTime = dAccelTime;
		CalculateMoveValue();
	}
}	

void CMotionInfo_Axis::SetMoveDecelTime(double dDecelTime) 
{ 
	if (dDecelTime!=m_dMoveDecelTime)
	{
		m_dMoveDecelTime = dDecelTime;
		CalculateMoveValue();
	}
}	

void CMotionInfo_Axis::SetMoveStartPos(double dStartPos) 	
{ 
	if (dStartPos!=m_dMoveStartPos)
	{
		m_dMoveStartPos = dStartPos;
		CalculateMoveValue();
	}
}	

void CMotionInfo_Axis::SetMoveEndPos(double dEndPos) 		
{ 
	if (dEndPos!=m_dMoveEndPos)
	{
		m_dMoveEndPos = dEndPos;
		CalculateMoveValue();
	}
}	

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

	if (dAccelTime!=m_dMoveAccelTime)
	{
		m_dMoveAccelTime = dAccelTime;
		m_nMoveDirection = 0;
	}

	if (dDecelTime!=m_dMoveDecelTime)
	{
		m_dMoveDecelTime = dDecelTime;
		m_nMoveDirection = 0;
	}

	if (dStartPos!=m_dMoveStartPos)
	{
		m_dMoveStartPos = dStartPos;
		m_nMoveDirection = 0;
	}

	if (dEndPos!=m_dMoveEndPos)
	{
		m_dMoveEndPos = dEndPos;
		m_nMoveDirection = 0;
	}

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

void CMotionInfo_Axis::SetIndex( int nIndex )
{
	m_nIndex = nIndex;
}

void CMotionInfo_Axis::SetJogVelocity( double dVelocity )
{
	if (dVelocity!=m_dJogVelocity)
	{
		m_dJogVelocity = dVelocity;
		CalculateJogValue();
	}
}

void CMotionInfo_Axis::SetJogAccelTime( double dAccelTime )
{
	if (dAccelTime!=m_dJogAccelTime)
	{
		m_dJogAccelTime = dAccelTime;
		CalculateJogValue();
	}
}

void CMotionInfo_Axis::SetJogDecelTime( double dDecelTime )
{
	if (dDecelTime!=m_dJogDecelTime)
	{
		m_dJogDecelTime = dDecelTime;
		CalculateJogValue();
	}
}

void CMotionInfo_Axis::SetJogStartPos( double dStartPos )
{
	if (dStartPos!=m_dJogStartPos)
	{
		m_dJogStartPos = dStartPos;
		CalculateJogValue();
	}
}

void CMotionInfo_Axis::SetJogData( double dVelocity, double dAccelTime, double dDecelTime, double dStartPos)
{
	bool bChange = false;
	if (dVelocity!=m_dJogVelocity)
	{
		m_dJogVelocity = dVelocity;
		bChange = true;
	}

	if (dAccelTime!=m_dJogAccelTime)
	{
		m_dJogAccelTime = dAccelTime;
		bChange = true;
	}

	if (dDecelTime!=m_dJogDecelTime)
	{
		m_dJogDecelTime = dDecelTime;
		bChange = true;
	}

	if (dStartPos!=m_dJogStartPos)
	{
		m_dJogStartPos = dStartPos;
		bChange = true;
	}

	CalculateJogValue();
}


int CMotionInfo_Axis::CalculateMoveMotion(double dTime, double& dPos, double& dVelocity, UINT& nStatus) 
{
	// 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 (CalculateMoveValue()==0)
		{
			m_dCurPosition	= m_dMoveStartPos;
			m_nCurStatus	= 1;
			return 0;
		}
	}

	nStatus = 0;
	if (m_dCurTotalDist==0.0) // À̵¿°Å¸®°¡ '0' À̸é
	{
		dPos		= m_dCurPosition = m_dMoveStartPos;
		nStatus		= m_nCurStatus = 1;
		dVelocity	= 0.0;
		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_dCurPosition =m_dMoveEndPos;
		nStatus		= m_nCurStatus = 1;
		dVelocity	= 0.0;
		return 1;
	}

	// set direction
	dPos	= m_dCurPosition = m_dMoveStartPos + (dPos * m_nMoveDirection);
	nStatus = m_nCurStatus = 0;

	return 1;
}

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

	if (m_dMoveVelocity<0.0) return 0;

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

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

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

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

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

	// ÇöÀç °¡°¨¼Ó ½Ã°£ ¹× °Å¸® Àç°è»ê
	m_dCurVelocity = m_dMoveVelocity;
	if ( (nMoveDirection*m_dCurTotalDist) < (m_dCurAccelDist+m_dCurDecelDist) )				// °¡°¨¼Ó½Ã À̵¿°Å¸®°¡ ÃÑ À̵¿°Å¸®º¸´Ù Ŭ¶§
	{
		double dA	= (m_dMoveVelocity / 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_dMoveVelocity;											// Á¤¼Ó½Ã À̵¿½Ã°£
	}	

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

	m_nMoveDirection = nMoveDirection;

	return 1;
}

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

	nStatus = 0;
	if (nJogDir==0)
	{
		dPos		= m_dCurPosition;
		nStatus		= m_nCurStatus = 1;
		dVelocity	= 0.0;
		return 1;
	}

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

	dPos = m_dCurPosition = m_dJogStartPos + (dPos * nJogDir); // set direction
	nStatus = m_nCurStatus = 0;
		
	return 1;
}

int CMotionInfo_Axis::CalculateJogValue()
{
	if (m_dJogVelocity<0.0) return 0;

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

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

	// ÇöÀç °¡°¨¼Ó ½Ã°£ ¹× °Å¸® Àç°è»ê
	m_dCurVelocity = m_dJogVelocity;

	return 1;
}

UINT CMotionInfo_Axis::GetCurStatus() const
{
	return m_nCurStatus;
}

double CMotionInfo_Axis::GetCurPosition() const
{
	return m_dCurPosition;
}

int CMotionInfo_Axis::GetEmergencyStop() const
{
	return m_nEmergencyStop;
}

void CMotionInfo_Axis::SetEmergencyStop(int nValue)
{
	m_nEmergencyStop = nValue;
}