#include "StdAfx.h"
#include "MotionInfo_Gantry.h"

CMotionInfo_Gantry::CMotionInfo_Gantry( int nIndex ) : m_nIndex(nIndex)
{
	m_vecMotionInfo.resize(AxisType_Count);
}

CMotionInfo_Gantry::~CMotionInfo_Gantry(void)
{
}

const CMotionInfo_Axis* CMotionInfo_Gantry::GetMotionInfo_Axis( int nAxisType ) const
{
	if (nAxisType<0 || nAxisType>=(int)m_vecMotionInfo.size()) return NULL;
	return &(m_vecMotionInfo[nAxisType]);
}

CMotionInfo_Axis* CMotionInfo_Gantry::GetMotionInfo_Axis( int nAxisType )
{
	if (nAxisType<0 || nAxisType>=(int)m_vecMotionInfo.size()) return NULL;
	return &(m_vecMotionInfo[nAxisType]);
}

int CMotionInfo_Gantry::GetMotionInfo_AxisCount() const
{
	return (int)m_vecMotionInfo.size();
}

int CMotionInfo_Gantry::GetIndex() const
{
	return m_nIndex;
}

double CMotionInfo_Gantry::GetVelocity( int nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return 0.0;
	return pNode->GetVelocity();
}

double CMotionInfo_Gantry::GetAccelTime( int nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return 0.0;
	return pNode->GetAccelTime();
}

double CMotionInfo_Gantry::GetDecelTime( int nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return 0.0;
	return pNode->GetDecelTime();
}

double CMotionInfo_Gantry::GetStartPos( int nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return 0.0;
	return pNode->GetStartPos();
}

double CMotionInfo_Gantry::GetEndPos( int nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return 0.0;
	return pNode->GetEndPos();
}

double CMotionInfo_Gantry::GetCurVelocity( int& nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) 
	{
		return GetCurVelocity_Min(nAxisType);
	}
	return pNode->GetVelocity();
}

double CMotionInfo_Gantry::GetCurAccelTime( int& nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) 
	{
		return GetCurAccelTime_Max(nAxisType);
	}
	return pNode->GetCurAccelTime();
}

double CMotionInfo_Gantry::GetCurDecelTime( int& nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) 
	{
		return GetCurDecelTime_Max(nAxisType);
	}
	return pNode->GetCurDecelTime();
}

double CMotionInfo_Gantry::GetCurFixTime( int& nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) 
	{
		return GetCurFixTime_Max(nAxisType);
	}
	return pNode->GetCurFixTime();
}

double CMotionInfo_Gantry::GetCurTotalTime( int& nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) 
	{
		return GetCurTotalTime_Max(nAxisType);
	}
	return pNode->GetCurTotalTime();
}

double CMotionInfo_Gantry::GetCurAccelDist( int& nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) 
	{
		return GetCurAccelDist_Max(nAxisType);
	}
	return pNode->GetCurAccelDist();
}

double CMotionInfo_Gantry::GetCurDecelDist( int& nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) 
	{
		return GetCurDecelDist_Max(nAxisType);
	}
	return pNode->GetCurDecelDist();
}

double CMotionInfo_Gantry::GetCurFixDist( int& nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) 
	{
		return GetCurFixDist_Max(nAxisType);
	}
	return pNode->GetCurFixDist();
}

double CMotionInfo_Gantry::GetCurTotalDist( int& nAxisType ) const
{
	const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) 
	{
		return GetCurTotalDist_Max(nAxisType);
	}
	return pNode->GetCurTotalDist();
}

double CMotionInfo_Gantry::GetCurVelocity_Min( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MAX;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue > it->GetCurVelocity())
		{
			dTempValue = it->GetCurVelocity();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurAccelTime_Min( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MAX;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue > it->GetCurAccelTime())
		{
			dTempValue = it->GetCurAccelTime();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurDecelTime_Min( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MAX;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue > it->GetCurDecelTime())
		{
			dTempValue = it->GetCurDecelTime();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurFixTime_Min( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MAX;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue > it->GetCurFixTime())
		{
			dTempValue = it->GetCurFixTime();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurTotalTime_Min( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MAX;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue > it->GetCurTotalTime())
		{
			dTempValue = it->GetCurTotalTime();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurAccelDist_Min( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MAX;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue > it->GetCurAccelDist())
		{
			dTempValue = it->GetCurAccelDist();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurDecelDist_Min( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MAX;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue > it->GetCurDecelDist())
		{
			dTempValue = it->GetCurDecelDist();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurFixDist_Min( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MAX;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue > it->GetCurFixDist())
		{
			dTempValue = it->GetCurFixDist();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurTotalDist_Min( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MAX;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue > it->GetCurTotalDist())
		{
			dTempValue = it->GetCurTotalDist();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurVelocity_Max( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MIN;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue < it->GetCurVelocity())
		{
			dTempValue = it->GetCurVelocity();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurAccelTime_Max( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MIN;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue < it->GetCurAccelTime())
		{
			dTempValue = it->GetCurAccelTime();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurDecelTime_Max( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MIN;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue < it->GetCurDecelTime())
		{
			dTempValue = it->GetCurDecelTime();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurFixTime_Max( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MIN;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue < it->GetCurFixTime())
		{
			dTempValue = it->GetCurFixTime();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurTotalTime_Max( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MIN;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue < it->GetCurTotalTime())
		{
			dTempValue = it->GetCurTotalTime();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurAccelDist_Max( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MIN;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue < it->GetCurAccelDist())
		{
			dTempValue = it->GetCurAccelDist();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurDecelDist_Max( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MIN;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue < it->GetCurDecelDist())
		{
			dTempValue = it->GetCurDecelDist();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurFixDist_Max( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MIN;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue < it->GetCurFixDist())
		{
			dTempValue = it->GetCurFixDist();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

double CMotionInfo_Gantry::GetCurTotalDist_Max( int& nAxisType ) const
{
	nAxisType = -1;
	double dTempValue = DBL_MIN;
	for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
	{
		if (dTempValue < it->GetCurTotalDist())
		{
			dTempValue = it->GetCurTotalDist();
			nAxisType = it->GetIndex();
		}
	}
	return dTempValue;
}

void CMotionInfo_Gantry::SetData( int nAxisType, double dVelocity, double dAccelTime, double dDecelTime, double dStartPos, double dEndPos )
{
	CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return;
	pNode->SetData(dVelocity, dAccelTime, dDecelTime, dStartPos, dEndPos);
}

void CMotionInfo_Gantry::SetVelocity( int nAxisType, double dVelocity )
{
	CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return;
	pNode->SetVelocity(dVelocity);
}

void CMotionInfo_Gantry::SetAccelTime( int nAxisType, double dAccelTime )
{
	CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return;
	pNode->SetAccelTime(dAccelTime);
}

void CMotionInfo_Gantry::SetDecelTime( int nAxisType, double dDecelTime )
{
	CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return;
	pNode->SetDecelTime(dDecelTime);
}

void CMotionInfo_Gantry::SetStartPos( int nAxisType, double dStartPos )
{
	CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return;
	pNode->SetStartPos(dStartPos);
}

void CMotionInfo_Gantry::SetEndPos( int nAxisType, double dEndPos )
{
	CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return;
	pNode->SetEndPos(dEndPos);
}

int CMotionInfo_Gantry::CalculateMotion( int nAxisType, double dTime, double& dPos, double& dVelocity, BOOL& bEnd )
{
	CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
	if (pNode==NULL) return 0;
	return pNode->CalculateMotion(dTime, dPos, dVelocity, bEnd);
}

int CMotionInfo_Gantry::CalculateMotion( double dTime, VectorDouble& vecPos, VectorDouble& vecVelocity, VectorBOOL& vecEnd )
{
	double dPos, dVelocity; BOOL bEnd;

	vecPos.clear(); vecVelocity.clear(); vecEnd.clear();

	int nReturn = 1;
	for (int nAxisIdx=0; nAxisIdx<GetMotionInfo_AxisCount(); nAxisIdx++)
	{
		bEnd = FALSE;
		dPos = dVelocity = 0.0;
		
		nReturn = nReturn * CalculateMotion(nAxisIdx, dTime, dPos, dVelocity, bEnd);

		vecPos.push_back(dPos);
		vecVelocity.push_back(dVelocity);
		vecEnd.push_back(bEnd);
	}

	return nReturn;
}