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

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

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

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

int	CAfmControl_Msg::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
	
	//CWinThread* thread = AfxBeginThread(MyThreadProc, m_strPort);	
	m_nBaudrate_start = 9600;		// upon powercycle sensor always starts with 9600
	int baudrate_run = 57600;
	baudrate_run = 115200;

	DWORD dTest = 115200;

	int point_count = -1;

	// open port, return if failed
	int ret = atf_OpenConnection(m_strPort, m_nBaudrate_start);
	if (ret)
	{
		atf_CloseConnection();
		ReleaseMutex(m_hMutex);	
		return 0;
	}
	else
	{
		atf_WriteBaud(115200);
		if(atf_ChangeCommBaudrate(115200))
		{
			atf_CloseConnection();
			ReleaseMutex(m_hMutex);    
			return 0;
		}

		 //atf_ChangeCommBaudrate(115200);
        /*int nBaudrate = 0;
        ret=atf_ReadBaud(nBaudrate);

        if (atf_ChangeCommBaudrate(dTest))        
        {            
        atf_CloseConnection();
        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(m_nZoomIndex);

	ReleaseMutex(m_hMutex);	

	return 1;
}

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

}

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

int	CAfmControl_Msg::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_Msg::RecipeTracking(int nTracking)
{
	return 1;
}

int	CAfmControl_Msg::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_Msg::RecipeChange(int nRecipeIndex, int nZoomIndex)
{
	return RecipeZoom(nZoomIndex);
}

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

BOOL CAfmControl_Msg::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)
	{
		ret = atf_AFTrack();
	}
	else
	{
		ret = atf_AfStop();
	}	

	if(ret) return FALSE;

	return TRUE;
}

BOOL CAfmControl_Msg::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_Msg::SetRecipeIndex(int nRecipeIdnex, int nZoomIndex)
{
	int ret;
	if (nZoomIndex!=-1)
	{
		ret = SetZoomIndex(nZoomIndex);
	}
	return ret;
}

BOOL CAfmControl_Msg::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_Msg::SetRecipeName(const CString& strRecipeName, int nZoomIndex)
{
	int ret;

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

}

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

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

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

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

BOOL CAfmControl_Msg::MoveToLimitPlus()
{
	return FALSE;
	
	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_Msg::MoveToLimitMinus()
{
	return FALSE; // 

	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);

	const int homing_distance = -10000;
	const int homing_back_stepsize = 1;

	const int limit_sw=(1<<16);
	const int moving=(1<<12);

	ret = atf_MoveZum(homing_distance);
	if (ret) return FALSE;

	int hw_status;

	clock_t ts=clock();

	do 
	{
		ret = atf_ReadHwStat(&hw_status);
		if (ret) return FALSE;
	}while(~hw_status & limit_sw && hw_status & moving || clock()-ts<CLK_TCK*10);

	do
	{
		ret = atf_MoveZum(homing_back_stepsize);
		if(ret) return FALSE;
		do
		{
			ret = atf_ReadHwStat(&hw_status);
			if(ret) return FALSE;
		} while (hw_status & moving);

	}while(hw_status&limit_sw);


	if(ret) return FALSE;

	return TRUE;
}

int CAfmControl_Msg::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 2;
	}

	DWORD dwTime = GetTickCount();
	int stepspermm=5000;

	int nLensNum = -1;
	ret = atf_ReadObjNum(&nLensNum);
	if(ret) return 3;

	const int lensnumber=nLensNum;

	float currentspeed;
	// Read current Speed 
	ret = atf_ReadSpeed(lensnumber, &currentspeed);
	if (ret) return 4;

	// write low speed
	ret = atf_WriteSpeed(lensnumber, currentspeed/(float)3.0);
	u_short currentstepspermm;

	ret = atf_ReadStepPerMmConversion( &currentstepspermm );
	if (ret) return 5;

	stepspermm = (int)currentstepspermm;

	const int homing_distance_steps = 10000*stepspermm/1000;
	const int homing_back_stepsize = -2*stepspermm/1000;

	// ★ homing target distance steps is Reference Position ( Setting parameter need! ) 
	const int homing_target_distance_steps = nHomePos * stepspermm/1000.;

	const int homing_timeout_s=10;

	const int limit_sw=(1<<18);
	const int moving=(1<<12);

	// 	const float homingspeed_mm_per_s=(float)50000.0/stepspermm;
	// 	ret = atf_WriteSpeed(lensnumber, homingspeed_mm_per_s );
	// 	if (ret) return FALSE;

	// 1. Move to Limit 
	ret = atf_MoveZ(homing_distance_steps);
	if (ret) return 6;

	int hw_status;

	clock_t t1=clock();

	//2. Read Limit Sensor?
	do 
	{
		ret = atf_ReadHwStat(&hw_status);
		if (ret) return 7;
	}while(~hw_status & limit_sw && clock()-t1 < CLK_TCK*homing_timeout_s);

	if(~hw_status & limit_sw) return 8;

	do
	{
		// 3. move out ( not Limit / small down)
		ret = atf_MoveZ(homing_back_stepsize);
		if(ret) return 9;
		do
		{
			// check Limit ( not Limit )
			ret = atf_ReadHwStat(&hw_status);
			if(ret) return 10;
		} while (hw_status & moving);

	}while(hw_status&limit_sw);

	// Current Position is Zero.
	ret = atf_WriteAbsZPos(0);

	if(ret) return 11;

	ret = atf_WriteSpeed(lensnumber, currentspeed);
	Sleep(50);

	// Target Position
	ret = atf_MoveZ(homing_target_distance_steps);

	dwTime = GetTickCount() - dwTime;

	// to read the position counter
	//atf_ReadAbsZPos

	if(ret) return 12;

	return TRUE;
}



BOOL CAfmControl_Msg::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_Msg::MoveToTargetPosition( double dPos )
{
	u_short currentstepspermm;

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

	const int nTarget_distance_steps = int(currentstepspermm/1000 * dPos);

	bRet = atf_MoveZ(nTarget_distance_steps);
	if(bRet) return FALSE;

	return TRUE;
}

int CAfmControl_Msg::JumpAF()
{
	int ret = atf_AfDuvJump();
	
	return ret;
}

void CAfmControl_Msg::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_Msg::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_Msg::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_Msg::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_Msg::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 = nPos/ (currentstepspermm/1000) ;

	return TRUE;
}

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