//////////////////////////////////////////////////////////////////////
//
// HAL_LaserProbe.cpp : interface for the Laser Probe related functions
//
//////////////////////////////////////////////////////////////////////

#include "StdAfx.h"

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

extern CHAL *g_pHSI;
extern CHALMicroVuExt g_HSIExt;

//////////////////////////////////////////////////////////////////////
HAL_STATUS HAL_LP_IsSupported(UINT &Type)
{
	switch(g_HSIExt.m_ActiveMachineType)
	{
	case HAL_MACHINE_TYPE_EXCEL:
	case HAL_MACHINE_TYPE_VECTRA:
	default:
		Type = HAL_LP_SUPPORTS_NONE;
		break;
	}

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

//////////////////////////////////////////////////////////////////////
HAL_STATUS HAL_LP_Startup()
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	UINT Supports = 0;
	HAL_LP_IsSupported(Supports);
	return Status;
}

HAL_STATUS HAL_LP_Enable(bool bEnable)
{
  UNREFERENCED_PARAMETER(bEnable);
	HAL_STATUS Status = HAL_STATUS_NORMAL;
	return Status;
}

//////////////////////////////////////////////////////////////////////
HAL_STATUS HAL_LP_IsEnabled(bool &bEnabled)
{
  UNREFERENCED_PARAMETER(bEnabled);
	HAL_STATUS Status = HAL_STATUS_NORMAL;
	return Status;
}

//////////////////////////////////////////////////////////////////////
void HAL_LP_SendQualityFeedback(int SignalQuality)
{
	UINT eventCallbackID=0;
	TCHAR EventData[HAL_MaxStringLength+1];
	CString Quality;
	Quality.Format(_T("%d"), SignalQuality);
	wcsncpy_s(EventData, _countof(EventData), Quality, HAL_MaxStringLength);  

	g_pHSI->Event(HAL_EVENT_FUNCTION, HAL_EVENT_LP_QUALITY, EventData, eventCallbackID);
}

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

	return Status;
}

//////////////////////////////////////////////////////////////////////
HAL_STATUS HAL_LP_SetActiveProbe(int nProbe)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	switch(nProbe)
	{
	case -1:
		Status = HAL_LP_Enable(false);
		break;

	default:
		if(g_HSIExt.m_OptimetInit)
		{
			g_HSIExt.m_ActiveProbe = APT_LASER;
			SetupPendantSpeeds();

			HAL_LP_LoadLens();
		}
		break;
	}

	return Status;
}

//////////////////////////////////////////////////////////////////////
void HAL_LP_LoadLens()
{
}

//////////////////////////////////////////////////////////////////////
HAL_STATUS HAL_LP_SetupMeasurePoint(CHALLPMeasurePoint *pParams)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	if(IsBoardPresent())
	{
		if(pParams)
		{
			// Probe position
			g_HSIExt.m_ProbeMeasPointStart[0] = pParams->m_MeasPoint.m_X;
			g_HSIExt.m_ProbeMeasPointStart[1] = pParams->m_MeasPoint.m_Y;
			g_HSIExt.m_ProbeMeasPointStart[2] = pParams->m_MeasPoint.m_Z;

			g_HSIExt.m_OptimetGain = pParams->m_Gain;
			g_HSIExt.m_OptimetFreq = pParams->m_Frequency;
			g_HSIExt.m_OptimetQualityThreshold = pParams->m_QualityThreshold;

			if(g_HSIExt.m_OptimetMag != pParams->m_AuxLens)
			{
				g_HSIExt.m_OptimetMag = pParams->m_AuxLens;
				HAL_LP_LoadLens();
			}
		}
	}
	else
		Status = HAL_STATUS_NOT_SUPPORTED;

	return Status;
}

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

	// Move to probe 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(g_HSIExt.m_OptimetInit && Status == HAL_STATUS_NORMAL)
	{
		// Make sure laser is enabled
		bool LaserEnabled;
		Status = HAL_LP_IsEnabled(LaserEnabled);
		if(!LaserEnabled)
			Status = HAL_LP_Enable(true);
  }
		// Measure single point
//		/*BOOL result = */FifoReadMeasurement(&g_HSIExt.m_OptimetMeasPt);

	return Status;
}

//////////////////////////////////////////////////////////////////////
HAL_STATUS HAL_LP_GetMeasureData(CHALMeasPoint &LPData)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	// Return probed point
//	LPData.m_X = g_HSIExt.m_LaserMeasPoint.m_data[HAL_AXIS_X];
//	LPData.m_Y = g_HSIExt.m_LaserMeasPoint.m_data[HAL_AXIS_Y];
//	LPData.m_Z = g_HSIExt.m_LaserMeasPoint.m_data[HAL_AXIS_Z];
	LPData.m_I = 0;
	LPData.m_J = 0;
	LPData.m_K = 1;

	return Status;
}

//////////////////////////////////////////////////////////////////////
HAL_STATUS HAL_LP_Shutdown()
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;

	if(g_HSIExt.m_OptimetInit)
	{
		if(IsBoardPresent() && *g_pHSI->Ctrl[Ctrl_LaserProbe])
			HAL_LP_Enable(false);

//		FifoTerminate();
	}
	return Status;
}