//////////////////////////////////////////////////////////////////////
//
// HAL_TouchProbe.h : interface for the Touch Probe related functions
//
//////////////////////////////////////////////////////////////////////

#include "StdAfx.h"

#include <math.h>

#include "HAL.h"
#include "HSI.h"
#include "HAL_Motion.h"
#include "HAL_Pendant.h"

extern CHAL *g_pHSI;
extern CHALMicroVuExt g_HSIExt;


HAL_STATUS HAL_TP_IsSupported(UINT &Type)
{
	Type = HAL_TP_SUPPORTS_TOUCH_TRIGGER;

	HAL_SendDebug(_T("HAL_TP_IsSupported %X\n"), Type);
	return HAL_STATUS_NORMAL;
}

HAL_STATUS HAL_TP_Enable(bool bProbeEnable)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	if(IsBoardPresent())
	{
		switch(bProbeEnable)
		{
		case true:
			{
				// Sometimes we get false hits when enabling, this flag will cause us to ignore them
				g_HSIExt.m_ProbeLoading = true;

				g_HSIExt.m_ProbeEnabled = true;

				// Sleeping for the debounce time will ensure any false hits are caught before we leave here
				//Sleep(DWORD(2*g_HSIExt.m_ProbeDebounceTime*1000));

				// Reset the hit expectecd flag
				g_HSIExt.m_ProbeLoading = false;
			}
			break;

		case false:
			{
				g_HSIExt.m_ProbeEnabled = false;
			}
			break;
		}
	}
	else
		Status = HAL_STATUS_NOT_SUPPORTED;
  

	return Status;
}

HAL_STATUS HAL_TP_Startup()
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	if(IsBoardPresent())
	{
		Status = HAL_TP_Enable(true);
	}
	else
		Status = HAL_STATUS_NOT_SUPPORTED;
  
	return Status;
}

HAL_STATUS HAL_TP_GetActiveProbe(int & /*nProbe*/)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	return Status;
}

HAL_STATUS HAL_TP_SetActiveProbe(int nProbe)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	switch(nProbe)
	{
	case -1:
		if(g_HSIExt.m_ProbeDisableIfNotActive && g_HSIExt.m_ProbeEnabled)
			Status = HAL_TP_Enable(false);
		break;

	default:
		g_HSIExt.m_ActiveProbe = APT_CONTACT;
		SetupPendantSpeeds();

		if(!g_HSIExt.m_ProbeEnabled)
			Status = HAL_TP_Enable(true);
		break;
	}

	return Status;
}

HAL_STATUS HAL_TP_SetManualTouchData(CHALTPManualConfig *pParams)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	// Store manual touch probe data
	if(pParams)
		g_HSIExt.m_ProbeManRetractDist = pParams->m_Retract;

	// MicroVu only accepts 1 retract distance for all axes and this is in encoder counts
	// Basing this value on the X axis resolution but this could be better
	double RetractDist = fabs(g_HSIExt.m_ProbeManRetractDist / g_HSIExt.m_AxisResolution[HAL_AXIS_X]);
  UNREFERENCED_PARAMETER(RetractDist);

	return Status;
}

HAL_STATUS HAL_TP_SetTipOffset(double /*OffsetX*/, double /*OffsetY*/, double /*OffsetZ*/)
{
	HAL_STATUS Status = HAL_STATUS_NOT_SUPPORTED;

	return Status;
}

HAL_STATUS HAL_TP_GetTipOffset(double & /*OffsetX*/, double & /*OffsetY*/, double & /*OffsetZ*/)
{
	HAL_STATUS Status = HAL_STATUS_NOT_SUPPORTED;

	return Status;
}

HAL_STATUS HAL_TP_SetupMeasurePoint(CHALTPMeasurePoint *pParams)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	if(IsBoardPresent())
	{
		if(pParams)
		{
			// Probe seek start position
			g_HSIExt.m_ProbeMeasPointStart[0] = pParams->m_MeasPoint.m_X+pParams->m_MeasPoint.m_I*pParams->m_Approach;
			g_HSIExt.m_ProbeMeasPointStart[1] = pParams->m_MeasPoint.m_Y+pParams->m_MeasPoint.m_J*pParams->m_Approach;
			g_HSIExt.m_ProbeMeasPointStart[2] = pParams->m_MeasPoint.m_Z+pParams->m_MeasPoint.m_K*pParams->m_Approach;

			// Probe seek end position
			g_HSIExt.m_ProbeMeasPointEnd[0] = pParams->m_MeasPoint.m_X-pParams->m_MeasPoint.m_I*pParams->m_Overshoot;
			g_HSIExt.m_ProbeMeasPointEnd[1] = pParams->m_MeasPoint.m_Y-pParams->m_MeasPoint.m_J*pParams->m_Overshoot;
			g_HSIExt.m_ProbeMeasPointEnd[2] = pParams->m_MeasPoint.m_Z-pParams->m_MeasPoint.m_K*pParams->m_Overshoot;

			// Probe travel speed
			g_HSIExt.m_ProbeMeasPointTSpeed = pParams->m_TravelSpeed;

			// Probe seek speed
			g_HSIExt.m_ProbeMeasPointSSpeed = pParams->m_SeekSpeed;

			// Retract distance
			g_HSIExt.m_ProbeDCCRetractDist = pParams->m_Retract;
		}
	}
	else
		Status = HAL_STATUS_NOT_SUPPORTED;

	return Status;
}

HAL_STATUS HAL_TP_DoMeasure(bool /*bWait*/)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	// Set the probe travel speed
	HAL_Motion_SetSpeedXYZ(g_HSIExt.m_ProbeMeasPointTSpeed);

	// Set the retract distance
	double RetractDist = fabs(g_HSIExt.m_ProbeDCCRetractDist / g_HSIExt.m_AxisResolution[HAL_AXIS_X]);
  UNREFERENCED_PARAMETER(RetractDist);

	// Move to start position
	Status = HAL_Motion_SetPositionXYZ(HAL_MOTION_AXIS_X|HAL_MOTION_AXIS_Y|HAL_MOTION_AXIS_Z,
		g_HSIExt.m_ProbeMeasPointStart[0],
		g_HSIExt.m_ProbeMeasPointStart[1],
		g_HSIExt.m_ProbeMeasPointStart[2],
		HAL_MOTION_MOVE_WAIT, 0.0);

	if(Status == HAL_STATUS_NORMAL)
	{
		// Set the probe seek speed
		HAL_Motion_SetSpeedXYZ(g_HSIExt.m_ProbeMeasPointSSpeed);

		// Flag we are expecting a DCC probe hit
		g_HSIExt.m_ProbeHitExpected = true;

		// Seek move
		Status = HAL_Motion_SetPositionXYZ(HAL_MOTION_AXIS_X|HAL_MOTION_AXIS_Y|HAL_MOTION_AXIS_Z,
			g_HSIExt.m_ProbeMeasPointEnd[0],
			g_HSIExt.m_ProbeMeasPointEnd[1],
			g_HSIExt.m_ProbeMeasPointEnd[2],
			HAL_MOTION_MOVE_WAIT, 0.0);

		// Flag we are no longer expecting a DCC probe hit
		g_HSIExt.m_ProbeHitExpected = false;
	}

	// Reset to the manual retract distance
	RetractDist = fabs(g_HSIExt.m_ProbeManRetractDist / g_HSIExt.m_AxisResolution[HAL_AXIS_X]);
  UNREFERENCED_PARAMETER(RetractDist);

	// Reset the original speed
	HAL_Motion_SetSpeedXYZ(0.0);

	return Status;
}

HAL_STATUS HAL_TP_IsMeasuring(bool & /*bMeasuring*/)
{
	HAL_STATUS Status = HAL_STATUS_NOT_SUPPORTED;

	return Status;
}

HAL_STATUS HAL_TP_GetMeasureData(CHALMeasPoint &TPData)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	double Position[] = {0.0, 0.0, 0.0};
	double Vector[] = {0.0, 0.0, 0.0};

	// Return probed point
	TPData.m_X = Position[0];
	TPData.m_Y = Position[1];
	TPData.m_Z = Position[2];
	TPData.m_I = Vector[0];
	TPData.m_J = Vector[1];
	TPData.m_K = Vector[2];

	return Status;
}

HAL_STATUS HAL_TP_Shutdown()
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	if(IsBoardPresent())
	{
		// Unregister event callbacks
	}
	else
		Status = HAL_STATUS_NOT_SUPPORTED;

	return Status;
}