#include "StdAfx.h"
#include "CHAfmControls/AFMControl_Wegu.h"
#include "AFT_LIB/atf_lib.h"
#include <tchar.h>

#pragma comment(lib, "legacy_stdio_definitions.lib")

CAfmControl_Wegu::CAfmControl_Wegu(int nIndex) : CAfmControl(nIndex)
{
	m_dSpeed = 1.0;
	m_hMutex = CreateMutex(NULL, FALSE, _T("AFMMutex"));
}

CAfmControl_Wegu::~CAfmControl_Wegu(void)
{		
	CloseHandle(m_hMutex);
}

int	CAfmControl_Wegu::Connect(const CAfmControlInfo& controlInfo)
{
	WaitForSingleObject(m_hMutex, INFINITE);
	
	m_bConnected = FALSE;
	m_ControlInfo = controlInfo;

	CString strPort = _T("COM") + m_ControlInfo.GetConnectionPort();
	//strPort.Format(_T("COM%d"), connParam.nPortNum);

#ifdef UNICODE
	int Ret = 0;
	Ret = WideCharToMultiByte(CP_ACP, 0, strPort, (int)_tcslen(strPort), m_strPort, 15, NULL, NULL);
	m_strPort[Ret] = 0;
#endif
	strcpy(m_strPort, strPort);

	//CWinThread* thread = AfxBeginThread(MyThreadProc, m_strPort);	
	m_nBaudrate_start = 9600;		// upon powercycle sensor always starts with 9600
	int baudrate_run = 9600;
	int point_count = -1;
	
	// open port, return if failed
	int ret = atf_OpenConnection(m_strPort, m_nBaudrate_start);
	if (ret)
	{
		ReleaseMutex(m_hMutex);	
		return 0;
	}

	Sleep(100);

	if(m_bConnected  = atf_isSerialConnection())
	{
		atf_SetComIdx(m_nIndex);

		Sleep(100);

		atf_ReadMicrostep(&m_nMicroSteps);

		Sleep(100);

		atf_ReadStepPerMmConversion(&m_nFullsteps);

		Sleep(100);

		atf_ReadHomingZ_Parameters(m_pAFMHomingZParam);		
	}

	if (m_bConnected==0) 
	{
		ReleaseMutex(m_hMutex);	
		return 0;
	}

	m_nRecipeIndex = 0;
	m_strRecipeName = _T("Default");
	int nZoomIndex = m_nZoomIndex = -1;
	
	ret = atf_ReadObjNum(&nZoomIndex);
	if (ret==0)
	{
		m_nZoomIndex = nZoomIndex;
	}

	RecipeZoom(1);

	ReleaseMutex(m_hMutex);	

	//atf_WriteMotionLimits(100,100,1) //taek 210112 wsi limit setting


	return 1;
}

void CAfmControl_Wegu::Disconnect()
{
	int ret;
	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);		
	}
	atf_CloseConnection();
}

int	CAfmControl_Wegu::RecipeJogSpeed(double dSpeed)
{
	m_dSpeed = dSpeed;
	return 1;
}

int	CAfmControl_Wegu::RecipeJogCommand(int nCmd)
{
	int ret;

//	if(!atf_isSerialConnection()) return 0;
	if(!m_bConnected) return 0;

	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);
		if(ret) return 0;
	}

	int nPos = 0;
	switch(nCmd)
	{
	case 0:
		ret = 0;
		break;
	case AfmJogCmd_Down:
		nPos = int( ((double)m_dSpeed * -m_nMicroSteps * m_nFullsteps)/1000.0 );
		ret = atf_MoveZ(nPos);
		break;
	case AfmJogCmd_Up:
		nPos = int( ((double)-m_dSpeed * -m_nMicroSteps * m_nFullsteps)/1000.0 );
		ret = atf_MoveZ(nPos);
		break;
	}
	if(ret) return 0;

	return 1;	
}


int	CAfmControl_Wegu::RecipeTracking(int nTracking)
{
	return 1;
}

int	CAfmControl_Wegu::RecipeZoom(int nZoomIndex)
{
	int ret;

//	if(!atf_isSerialConnection()) return 0;
	if(!m_bConnected) return 0;

	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);
		if(ret) return 0;
	}

	ret = atf_WriteObjNum(nZoomIndex);
	if(ret) return 0;

	return 1;
}

int	CAfmControl_Wegu::RecipeChange(int nRecipeIndex, int nZoomIndex)
{
	return RecipeZoom(nZoomIndex);
}

int	CAfmControl_Wegu::RecipeChange(const CString& strRecipeName, int nZoomIndex)
{
	return RecipeZoom(nZoomIndex);
}

BOOL CAfmControl_Wegu::SetTracking(int nTracking)
{
	int ret;

//	if(!atf_isSerialConnection()) return 0;
	if(!m_bConnected) return 0;

	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);
		if(ret) return 0;
	}
	
	if(nTracking)
	{
		//210406 AF ¸®¹Ô
		atf_WriteMotionLimits(10000, -10000, 0);
		Sleep(50);
		ret = atf_AFTrack();
	}
	else
	{
		ret = atf_AfStop();
	}	

	if(ret) return FALSE;

	return TRUE;
}

BOOL CAfmControl_Wegu::SetZoomIndex(int nZoomIndex)
{
	int ret;

//	if(!atf_isSerialConnection()) return 0;
	if(!m_bConnected) return 0;

	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);
		if(ret) return 0;
	}

	ret = atf_WriteObjNum(nZoomIndex);

	if(ret) return FALSE;

	m_nZoomIndex = nZoomIndex;

	return TRUE;
}

BOOL CAfmControl_Wegu::SetRecipeIndex(int nRecipeIdnex, int nZoomIndex)
{
	int ret;
	if (nZoomIndex!=-1)
	{
		ret = SetZoomIndex(nZoomIndex);
	}
	return ret;
}

BOOL CAfmControl_Wegu::GetConnected() const
{	
	int ret;

//	if(!atf_isSerialConnection()) return 0;
	if(!m_bConnected) return 0;

	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);
		if(ret) return 0;
	}

	return atf_isSerialConnection();
}

BOOL CAfmControl_Wegu::SetRecipeName(const CString& strRecipeName, int nZoomIndex)
{
	int ret;

	if (nZoomIndex!=-1)
	{
		ret = SetZoomIndex(nZoomIndex);
	}
	
//	ReleaseMutex(m_hMutex);
	return ret;

}

BOOL CAfmControl_Wegu::GetTracking(int& nTracking) const
{
	nTracking = m_nTracking;	
	return TRUE;
}

BOOL CAfmControl_Wegu::GetZoomIndex(int& nZoomIndex) const
{
	nZoomIndex = m_nZoomIndex;	
	return TRUE;
}

BOOL CAfmControl_Wegu::GetRecipeIndex(int& nRecipeIdnex, int& nZoomIndex) const
{
	nRecipeIdnex = m_nRecipeIndex;
	nZoomIndex = m_nZoomIndex;	
	return TRUE;
}

BOOL CAfmControl_Wegu::GetRecipeName(CString& strRecipeName, int& nZoomIndex) const
{
	strRecipeName = strRecipeName;
	nZoomIndex = m_nZoomIndex;	
	return TRUE;
}

BOOL CAfmControl_Wegu::MoveToLimitPlus()
{
	int ret;

//	if(!atf_isSerialConnection()) return 0;
	if(!m_bConnected) return 0;

	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);
		if(ret) return 0;
	}

	//ret = atf_MoveZ(((double)300000 * m_nMicroSteps * m_nFullsteps)/1000);

	int arrHomeParam[8];

	memcpy(arrHomeParam, m_pAFMHomingZParam, sizeof(int) * 8);

	arrHomeParam[3] = arrHomeParam[7] = 0;

	ret = atf_RunHomingZ(arrHomeParam);

	if(ret) return FALSE;

	return TRUE;
}

BOOL CAfmControl_Wegu::MoveToLimitMinus()
{
	int ret;

//	if(!atf_isSerialConnection()) return 0;
	if(!m_bConnected) return 0;

	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);
		if(ret) return 0;
	}

	//ret = atf_MoveZ(((double)-300000 * m_nMicroSteps * m_nFullsteps)/1000);

	int arrHomeParam[8];

	memcpy(arrHomeParam, m_pAFMHomingZParam, sizeof(int) * 8);

	arrHomeParam[3] = arrHomeParam[7] = 0;

	arrHomeParam[0] *= -1;

	arrHomeParam[1] *= -1;

	arrHomeParam[2] *= -1;

	ret = atf_RunHomingZ(arrHomeParam);

	if(ret) return FALSE;

	return TRUE;
}

int CAfmControl_Wegu::MoveToHomePosition(int nHomePos)
{
	int ret;

	//	if(!atf_isSerialConnection()) return 0;
	if(!m_bConnected) return 0;

	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);
		if(ret) return 0;
	}

	//ret = atf_MoveZ(((double)-300000 * m_nMicroSteps * m_nFullsteps)/1000);

	int arrHomeParam[8];

	memcpy(arrHomeParam, m_pAFMHomingZParam, sizeof(int) * 8);

	//arrHomeParam[3] = arrHomeParam[7] = 0;

	//arrHomeParam[0] *= -1;

	//arrHomeParam[1] *= -1;

	//arrHomeParam[2] *= -1;

	ret = atf_RunHomingZ(arrHomeParam);

	if(ret) return FALSE;

	return TRUE;
}



BOOL CAfmControl_Wegu::MoveToBasePosition(int nZoomIndex)
{
	int ret;

//	if(!atf_isSerialConnection()) return 0;
	if(!m_bConnected) return 0;

	if(atf_GetComIdx() != m_nIndex)
	{
		ret = atf_SetComIdx(m_nIndex);
		if(ret) return 0;
	}

	//ret = atf_MoveZ(((double)-100 * m_nMicroSteps * m_nFullsteps)/1000);

	//if(ret) return FALSE;

	//Sleep(100);

//	ret = atf_RunHomingZ(m_pAFMHomingZParam);
	
	int nPos = int( ((double)m_pAFMHomingZParam[3] * m_nMicroSteps * m_nFullsteps)/1000.0 );
	ret = atf_MoveZ(nPos);

	if(ret) return FALSE;

	return TRUE;
}

BOOL CAfmControl_Wegu::MoveToTargetPosition( double dPos )
{
	return TRUE;
}

BOOL CAfmControl_Wegu::SetSystemTime(SYSTEMTIME stCurrentTime)
{
	return TRUE;
}

void CAfmControl_Wegu::WorkThreadProcess(PVOID pParameter)
{
	bool m_bIsWorking = FALSE;
	while(!m_bIsWorking){
		WaitForSingleObject(m_hMutex, INFINITE);
		atf_CloseConnection();
		int ret = atf_OpenConnection(m_strPort, m_nBaudrate_start);
		if (ret)
		{
			ReleaseMutex(m_hMutex);
			return ;
		}

		atf_IsInHomePosition(&m_bIsWorking);
		ReleaseMutex(m_hMutex);
		Wait(100);
	}

	WaitForSingleObject(m_hMutex, INFINITE);
	atf_CloseConnection();
	int ret = atf_OpenConnection(m_strPort, m_nBaudrate_start);
	if (ret)
	{
		ReleaseMutex(m_hMutex);
		return ;
	}	
	atf_AFTrack();
	ReleaseMutex(m_hMutex);
}

void CAfmControl_Wegu::Wait(DWORD dwMillisecond)

{
	MSG msg;
	DWORD dwStart;
	dwStart = GetTickCount();

	while(GetTickCount() - dwStart < dwMillisecond)
	{

		while(PeekMessage(&msg, NULL, 0, 0, PM_REMOVE))
		{
			TranslateMessage(&msg);
			DispatchMessage(&msg);
		}
	}
}
BOOL CAfmControl_Wegu::GetInFocus(BOOL* pRtCheck)
{
	// bit 13 (MsInFocus)
	// 0 = sensor beyond the In-Focus Range parameter (not in focus)
	// 1 = sensor inside In-Focus Range parameter (In-focus position)

	if(pRtCheck == NULL) return FALSE;

	short shStatus;

	if(atf_ReadStatus(&shStatus) == 0)
	{
		 if((shStatus & (1 << 13)) != 0)
		 {
			 *pRtCheck = TRUE;
			 return TRUE;
		 }
		 else
		 {
			 *pRtCheck = TRUE;
		 }
			 
	}
	return FALSE;
}

int CAfmControl_Wegu::GetAFMHomePosition(bool &bHome) const
{
	int nRet = atf_IsInHomePosition(&bHome);
	return nRet;

// 	DWORD dwStart;
// 	dwStart = GetTickCount();
// 	int nRet;
// 	while(GetTickCount() - dwStart < 5000)
// 	{
// 		nRet = atf_IsInHomePosition(&bHome);
// 		if(bHome == TRUE)
// 		{
// 			return nRet;
// 		}
// 		Sleep(100);
// 	}

// 	Output Boolean *pbInHome 
// 	True = in home position 
// 	False = not in home 
// 	position Return 
// 	value int 
// 	0 (ErrOK) = operation successful 
// 	8 (ErrNoresources) = insufficient command buffer size 
// 	10 (ErrOperFailed) = sending command to the sensor failure, sensor data receiving error or sensor responded with NACK 
// 	11 (ErrTimeout) = erroneous or no response
}

BOOL CAfmControl_Wegu::GetCurPos(double& dPos)
{
	u_short currentstepspermm;

	BOOL bRet = atf_ReadStepPerMmConversion(&currentstepspermm);
	if (bRet) return FALSE;

	int nPos = 0;
	bRet = atf_ReadAbsZPos(&nPos);

	if (bRet)
		return FALSE;

	dPos = (double)nPos / ((double)currentstepspermm / (double)1000);

	return TRUE;
}