//////////////////////////////////////////////////////////////////////
//
// HAL_Pendant.cpp : interface for the Pendant related functions
//
//////////////////////////////////////////////////////////////////////

#include "StdAfx.h"

#include <math.h>

#include "HAL.h"
#include "HSI.h"

extern CHAL *g_pHSI;
extern CHALMicroVuExt g_HSIExt;

extern int GetLampNumberFromLocation(const enum HAL_ILLUMINATION_LAMP_LOCATION_TYPE SearchLocationType);


HAL_STATUS HAL_Pendant_IsSupported(UINT &Type)
{
	Type = HAL_PENDANT_SUPPORTS_JOYSTICK;
	HAL_SendDebug(_T("HAL_Pendant_IsSupported %X\n"), Type);
	return HAL_STATUS_NORMAL;
}

void SetupPendantSpeeds()
{
	if(g_HSIExt.m_ActiveProbe==APT_VISION)
	{
		// Now we've changed mag, update the pendant settings so our fine speed is good for current optics     
		double PixelX=-1.0,PixelY=-1.0;
		if(g_HSIExt.m_IsCalibrated && g_pHSI->Optics.GetMagnification(PixelX,PixelY)==HAL_STATUS_NORMAL)
		{
			// Optics calibrated so base XYZ fine speed on pixel/fov size
			int FOVSizeX, FOVSizeY;
			g_pHSI->FrameGrabber.GetImageSize(FOVSizeX, FOVSizeY);

//			g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_X][APT_VISION] = fabs((PixelX * FOVSizeX) / g_HSIExt.m_AxisVelocity[HAL_AXIS_X]) * g_HSIExt.m_Pendant_FineSpeedFactor;
//			g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_Y][APT_VISION] = fabs((PixelX * FOVSizeX) / g_HSIExt.m_AxisVelocity[HAL_AXIS_Y]) * g_HSIExt.m_Pendant_FineSpeedFactor;
//			g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_Z][APT_VISION] = fabs((PixelX * FOVSizeX/2) / g_HSIExt.m_AxisVelocity[HAL_AXIS_Z]) * g_HSIExt.m_Pendant_FineSpeedFactor;
		}
		else
		{
			// Optics not calibrated so use default XYZ fine speed
//			g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_X][APT_VISION] = g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_X][APT_CONTACT] * g_HSIExt.m_Pendant_FineSpeedFactor;
//			g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_Y][APT_VISION] = g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_Y][APT_CONTACT] * g_HSIExt.m_Pendant_FineSpeedFactor;
//			g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_Z][APT_VISION] = g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_Z][APT_CONTACT] * g_HSIExt.m_Pendant_FineSpeedFactor;
		}

		// Use default Q fine speed
//		g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_Q][APT_VISION] = g_HSIExt.m_Pendant_FineSpeed[HAL_AXIS_Q][APT_CONTACT];
	}
}

HAL_STATUS SetPendantControlMode(enum HAL_CTRL_MODE /*ControlMode*/)
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;
	return Status;
}

HAL_STATUS CheckPendantLightControl()
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;
	return Status;
}

HAL_STATUS HAL_Pendant_Startup( )
{
	HAL_STATUS Status = HAL_STATUS_NORMAL;
	return Status;
}

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

HAL_STATUS HAL_Pendant_Shutdown( )
{
	return HAL_STATUS_NORMAL;
}