#include "stdafx.h"
#include "SelfTestHandler.h"
#include "IMotionModelManager.h"
#include "IMotionModel.h"
#include "SelfTestMotionStageArgs.h"
#include "MotionStages.h"
#include "MechnicalMonitor.h"

using namespace DIOS::Dev::Detail::MachineryECOM;

SelfTestHandler::SelfTestHandler() :m_modelManager(nullptr)
{
}


SelfTestHandler::~SelfTestHandler()
{
}

void SelfTestHandler::OnModelLoaded(IMotionModelManager *modelManager)
{
	m_modelManager = modelManager;
}

void SelfTestHandler::OnCoordinatesLoaded(IPositionManager *coordinates)
{

}

RET_STATUS SelfTestHandler::OnSelectExamMode(const char *pExamKey)
{
	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS SelfTestHandler::OnSetTechParamsInfo(ResDataObject &pParam)
{
	return RET_STATUS::RET_SUCCEED;
}

void SelfTestHandler::OnMotionEvent(const std::string &motionEventName)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_SELFTEST);
	if (!model)
	{
		return;
	}

	auto stageArg = model->GetStageArgs();

	if (motionEventName == MONITOR_EVENT_TUBEANGLE_MOVE_STOP)
	{
		stageArg->SetTubeAngleMotionStatus(FALSE);
		MechnicalMonitor::Instance()->EndMonitor();

		ChangeSelfTestNextStage(MO_TUBE_ANGLE);
	}
	else if (motionEventName == MONITOR_EVENT_TUBEHEIGHT_MOVE_STOP)
	{
		stageArg->SetTubeHeightMotionStatus(FALSE);
		MechnicalMonitor::Instance()->EndMonitor();

		ChangeSelfTestNextStage(MO_TUBE_HEIGHT);
	}
}

RET_STATUS SelfTestHandler::OnStopMech()
{
	auto model = m_modelManager->Resove(MOTION_MODEL_SELFTEST);
	model->ChangeStage(SELFTEST_STAGE_END_AUTO_SELFTEST);
	auto stageArgs = (SelfTestMotionStageArgs*)model->GetStageArgs();
	stageArgs->CurrentRepeat = stageArgs->RepeatTime;
	
	return RET_STATUS::RET_SUCCEED;
}

void SelfTestHandler::OnStartSelfTest(ResDataObject &pParam)
{
	if (pParam.GetFirstOf("DOF") < 0
		|| pParam.GetFirstOf("HighLimit") < 0
		|| pParam.GetFirstOf("LowLimit") < 0
		|| pParam.GetFirstOf("Period") < 0
		|| pParam.GetFirstOf("DutyCycle") < 0
		|| pParam.GetFirstOf("RepeatTime") < 0)
	{
		return;
	}

	if (m_modelManager == nullptr)
	{
		return;
	}

	auto model = m_modelManager->Resove(MOTION_MODEL_SELFTEST);
	auto stageArgs = (SelfTestMotionStageArgs*)model->GetStageArgs();

	stageArgs->DOF = atoi(pParam["DOF"]);
	stageArgs->HighLimit = (float)atof(pParam["HighLimit"]);
	stageArgs->LowLimit = (float)atof(pParam["LowLimit"]);
	stageArgs->Period = atoi(pParam["Period"]);
	stageArgs->DutyCycle = (float)atof(pParam["DutyCycle"]);
	stageArgs->RepeatTime = atoi(pParam["RepeatTime"]);
	stageArgs->CurrentRepeat = 0;

	if(gbusinessLog) gbusinessLog->Info("[SelfTestHandler][OnStartSelfTest]->[DOF:{$:d},H:{$:f3},L:{$:f3},P:{$:d},D:{$:f3},Rept:{$:d}]",
		stageArgs->DOF, stageArgs->HighLimit, stageArgs->LowLimit, stageArgs->Period, stageArgs->DutyCycle, stageArgs->RepeatTime);

	auto offfset = -1;
	if (stageArgs->DOF == 0)
	{
		offfset = MO_TUBE_ANGLE;
	}
	else if (stageArgs->DOF == 1)
	{
		offfset = MO_TUBE_HEIGHT;
	}

	if (offfset == -1)
	{
		return;
	}

	model->ChangeStage(SELFTEST_STAGE_START_AUTO_SELFTEST);
	MechnicalMonitor::Instance()->BeginMonitor(this, offfset);
	
}

void SelfTestHandler::ChangeSelfTestNextStage(int dof)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_SELFTEST);
	auto stageArgs = (SelfTestMotionStageArgs*)model->GetStageArgs();

	stageArgs->CurrentRepeat++;

	if (stageArgs->CurrentRepeat >= stageArgs->RepeatTime)
	{
		return;
	}

	if (dof == MO_TUBE_ANGLE)
	{
		if (stageArgs->CurrentRepeat % 2 != 0)
		{//High->Low
			model->ChangeStage(SELFTEST_STAGE_TUBE_ANGLE_ROTATETO_TARGET);
		}
		else
		{//Low->High
			model->ChangeStage(SELFTEST_STAGE_TUBE_ANGLE_ROTATE_BACK);
		}

		MechnicalMonitor::Instance()->BeginMonitor(this, dof);
	}
	else if (dof == MO_TUBE_HEIGHT)
	{
		if (stageArgs->CurrentRepeat % 2 != 0)
		{//High->Low
			model->ChangeStage(SELFTEST_STAGE_TUBE_HEIGHT_MOVETO_TARGET);
		}
		else
		{//Low->High
			model->ChangeStage(SELFTEST_STAGE_TUBE_HEIGHT_MOVE_BACK);
		}

		MechnicalMonitor::Instance()->BeginMonitor(this, dof);
	}

}

RET_STATUS SelfTestHandler::OnStartMove(DOF_MECH mech, int nOrientation)
{
	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS SelfTestHandler::OnStopMove(DOF_MECH mech)
{
	return RET_STATUS::RET_SUCCEED;
}