#include "stdafx.h"
#include "SevenOcean\CMMIO_SERIAL.h"
#include "HSI.h"
#include "HSI_Motion.h"
#include "HSI_Function.h"
#include "math.h"

HSI_Function *g_pHSI_Function = nullptr;

HSI_Function::HSI_Function()
{
	for (size_t i = 0; i < 4; i++)
	{
		startPos[i] = 0;
		endPos[i] = 0;
	}
	travelSpeed = 0;
	seekSpeed = 0;
	retractDis = 0;
	retractManDis = 0;
	probeRunStatus = E_EF3_PROBE_DONE;
}
HSI_Function::~HSI_Function()
{

}

HSI_STATUS HSI_Function::HSI_TP_Enable(bool bProbeEnable)
{
	HSI_STATUS Status = HSI_STATUS_NORMAL;

	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_Startup()
{
	HSI_STATUS Status = HSI_STATUS_NORMAL;
	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_GetActiveProbe(int & /*nProbe*/)
{
	HSI_STATUS Status = HSI_STATUS_NORMAL;

	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_SetActiveProbe(int nProbe)
{
	HSI_STATUS Status = HSI_STATUS_NORMAL;


	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_SetManualTouchData(double dRetract)
{
	HSI_STATUS Status = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
			retractManDis = dRetract;

		// Metronics 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
		int RetractManDist = (int)(fabs(retractManDis / g_pHSI_Motion->m_Resolution[1]));
		g_pHSI_Motion->ProbeRetractManDist(RetractManDist);
	}
	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_SetTipOffset(double /*OffsetX*/, double /*OffsetY*/, double /*OffsetZ*/)
{
	HSI_STATUS Status = HSI_STATUS_NOT_SUPPORTED;

	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_GetTipOffset(double & /*OffsetX*/, double & /*OffsetY*/, double & /*OffsetZ*/)
{
	HSI_STATUS Status = HSI_STATUS_NOT_SUPPORTED;

	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_SetupMeasurePoint(double dBeginX, double dBeginY, double dBeginZ, double dEndX, double dEndY, double dEndZ, double dTraveSpeed, double dTouchSpeed, double dRetrackDis)
{
	HSI_STATUS Status = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
    // Probe seek start position
    startPos[0] = dBeginX;
    startPos[1] = dBeginY;
    startPos[2] = dBeginZ;

    // Probe seek end position
    endPos[0] = dEndX;
    endPos[1] = dEndY;
    endPos[2] = dEndZ;

    // Probe travel speed
    travelSpeed = dTraveSpeed;

    // Probe seek speed
    seekSpeed = dTouchSpeed;

    // Retract distance
    retractDis = dRetrackDis;
	}
	else
		Status = HSI_STATUS_NOT_SUPPORTED;
	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_DoMeasure(bool /*bWait*/)
{
	HSI_STATUS Status = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion && (probeRunStatus == E_EF3_PROBE_DONE))
	{
		probeRunStatus == E_EF3_PROBE_RUNNING;
		double nowSpeed = 0.0;
		g_pHSI_Motion->GetSpeedXyz(HSI_MOTION_AXIS_X,nowSpeed);
		if (1== g_pHSI_Motion->m_iSpeedType)
		{
			g_pHSI_Motion->SetSpeedXyz(travelSpeed);
		}
		else
		{
			g_pHSI_Motion->SetSpeedXyz(travelSpeed/(g_pHSI_Motion->m_Resolution[1]*50));
		}

		Status = g_pHSI_Motion->SetPositionXyz(HSI_MOTION_AXIS_X | HSI_MOTION_AXIS_Y | HSI_MOTION_AXIS_Z,
			startPos[0], startPos[1], startPos[2], HSI_MOTION_MOVE_WAIT, 0.0);
		if (Status == HSI_STATUS_NORMAL)
		{
			if (1 == g_pHSI_Motion->m_iSpeedType)
			{
				g_pHSI_Motion->SetSpeedXyz(seekSpeed);
			}
			else
			{
				g_pHSI_Motion->SetSpeedXyz(seekSpeed / (g_pHSI_Motion->m_Resolution[1] * 50));
			}

			if (endPos[0] - startPos[0] > g_pHSI_Motion->m_Resolution[1] * 3)
			{
				g_pHSI_Motion->m_axisDirX = 1;
			}
			else if (endPos[0] - startPos[0] < g_pHSI_Motion->m_Resolution[1] * (-3))
			{
				g_pHSI_Motion->m_axisDirX = 2;
			}
			else
				g_pHSI_Motion->m_axisDirX = 0;

			if (endPos[1] - startPos[1] > g_pHSI_Motion->m_Resolution[2] * 3)
			{
				g_pHSI_Motion->m_axisDirY = 1;
			}
			else if (endPos[1] - startPos[1] < g_pHSI_Motion->m_Resolution[2] * (-3))
			{
				g_pHSI_Motion->m_axisDirY = 2;
			}
			else
				g_pHSI_Motion->m_axisDirY = 0;

			if (endPos[2] - startPos[2] > g_pHSI_Motion->m_Resolution[3] * 3)
			{
				g_pHSI_Motion->m_axisDirZ = 1;
			}
			else if (endPos[2] - startPos[2] < g_pHSI_Motion->m_Resolution[3] * (-3))
			{
				g_pHSI_Motion->m_axisDirZ = 2;
			}
			else
				g_pHSI_Motion->m_axisDirZ = 0;

			// Set the retract distance
			g_pHSI_Motion->m_ProbeReturnPos = retractDis;
			g_pHSI_Motion->m_probeSeekSpeed = seekSpeed / (g_pHSI_Motion->m_Resolution[1] * 50);

			//g_pHSI_Motion->m_ProbeHitExpected = true;
			Status = g_pHSI_Motion->JogProbe(HSI_MOTION_AXIS_X, 0.1);
			//g_pHSI_Motion->m_ProbeHitExpected = false;
		}


		// Reset to the manual retract distance
		int RetractManDist = (int)(fabs(retractManDis / g_pHSI_Motion->m_Resolution[1]));

		//g_pHSI_Motion->ProbeRetractManDist(RetractManDist);

		g_pHSI_Motion->SetSpeedXyz(nowSpeed);

		probeRunStatus == E_EF3_PROBE_DONE;
	}
	else
	{
		Status = HSI_STATUS_NOT_SUPPORTED;
	}
	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_IsMeasuring(bool & /*bMeasuring*/)
{
	HSI_STATUS Status = HSI_STATUS_NOT_SUPPORTED;

	return Status;
}

//锁存位置和碰撞后的位置
HSI_STATUS HSI_Function::HSI_TP_GetMeasureData(double &dBeginX, double &dBeginY, double &dBeginZ, double &dEndX, double &dEndY, double &dEndZ)
{
	HSI_STATUS Status = HSI_STATUS_NORMAL;
	if (g_pHSI_Motion)
	{
    dBeginX = g_pHSI_Motion->m_LockPos[1];
    dBeginY = g_pHSI_Motion->m_LockPos[2];
    dBeginZ = g_pHSI_Motion->m_LockPos[3];
    dEndX = g_pHSI_Motion->m_ijk[0];
    dEndY = g_pHSI_Motion->m_ijk[1];
    dEndZ = g_pHSI_Motion->m_ijk[2];
	}
	return Status;
}

HSI_STATUS HSI_Function::HSI_TP_Shutdown()
{
	HSI_STATUS Status = HSI_STATUS_NORMAL;

	return Status;
}