#include "stdafx.h"
#include "CArmResetHandler.h"
#include "IPositionManager.h"
#include "IMotionModelManager.h"
#include "ConfigurerMotion.h"
#include "IMotionModel.h"
#include "MotionStages.h"
#include "ResetMotionStageArgs.h"
#include "MechnicalMonitor.h"
#include "LogicDeviceManager.h"
#include "LogicDeviceMechCarm.h"
#include "CArmComponentNameDef.h"
#include "CArmGeneralMotionStageArgs.h"

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

CArmResetHandler::CArmResetHandler() :
m_modelLoaded(FALSE),
m_useHandSwitchControlReset(FALSE),
m_isResetting(FALSE),
m_isResettingSwing(FALSE),
m_isResettingCircular(FALSE),
m_postionManager(nullptr),
m_modelManager(nullptr),
m_MechReset(TOMO_TUBE_HEIGHT)
{
}


CArmResetHandler::~CArmResetHandler()
{
}

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

void CArmResetHandler::OnCoordinatesLoaded(IPositionManager *coordinates)
{
	m_postionManager = coordinates;
}

RET_STATUS CArmResetHandler::OnReset(DOF_MECH mech)
{
	DOF_MECH mechtemp = mech;
	if (mech == MECH_ALL)
	{
		mechtemp = m_MechReset;

		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnReset]->[Set reset value {$:d}]", mechtemp);
	}

	if (mechtemp == TOMO_TUBE_ANGLE)
	{
		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnReset]->[Enter][CARM_TUBE_SWING]");

		m_postionManager->ResetSystem(CONTROLLER_CARM_TUBE_SWING);
		DoStartSwingReset();
	}
	else if (mechtemp == TOMO_TUBE_HEIGHT)
	{
		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnReset]->[Enter][CARM_TUBE_CIRCULAR]");

		m_postionManager->ResetSystem(CONTROLLER_CARM_TUBE_CIRCULAR);
		DoStartCircularReset();
	}

	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnReset]->[Leave]");

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CArmResetHandler::OnCenterAdjust(DOF_MECH mech)
{
	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CArmResetHandler::OnSelectExamMode(const char *pExamKey)
{
	if (!m_modelLoaded)
	{
		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnSelectExamMode]->[Enter][{$}]", pExamKey);

		auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
		if (model)
		{
			ResDataObject mechparms;
			if (ConfigurerMotion::GetMachineryConfigs(mechparms))
			{
				model->LoadMachineryParams(mechparms);
			}
		}

		m_useHandSwitchControlReset = ConfigurerMotion::GetUseHandSwitchControlReset();

		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][ResetParams]->[{$:d}]",m_useHandSwitchControlReset);

		m_modelLoaded = TRUE;

		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnSelectExamMode]->[Exit]");
	}

	return RET_STATUS::RET_SUCCEED;
}

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

RET_STATUS CArmResetHandler::OnStopMech()
{
	ChangeMotionModelStage(RESET_STAGE_STOP_MOVE);

	m_isResetting = FALSE;
	m_isResettingCircular = FALSE;
	m_isResettingSwing = FALSE;

	return RET_STATUS::RET_SUCCEED;
}

void CArmResetHandler::OnMotionEvent(const std::string &motionEventName)
{
	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnMotionEvent]->[Enter][{$}]", motionEventName.c_str());

	auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
	if (!model)
	{
		return;
	}

	auto stageArg = model->GetStageArgs();

	if (motionEventName == MONITOR_EVENT_TUBEANGLE_MOVE_STOP)
	{
		stageArg->SetTubeAngleMotionStatus(FALSE);

		OnMotionEventTubeSwingStop();
	}
	else if (motionEventName == MONITOR_EVENT_TUBEHEIGHT_MOVE_STOP)
	{
		stageArg->SetTubeHeightMotionStatus(FALSE);

		OnMotionEventTubeCircularStop();
	}

	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnMotionEvent]->[Exit]");
}

void CArmResetHandler::OnHandSwitchGearFirstPressed()
{
	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnHandSwitchGearFirstPressed]->[Enter]");

	//if (m_useHandSwitchControlReset)
	//{
	//	DoStartReset();
	//}
	auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
	if (!model)
	{
		return;
	}

	model->ChangeStage(CARM_RESET_START_EXPOSURE);

	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnHandSwitchGearFirstPressed]->[Exit]");
}

void CArmResetHandler::OnHandSwitchGearFirstReleased()
{
	auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
	if (!model)
	{
		return;
	}

	model->ChangeStage(CARM_RESET_STOP_EXPOSURE);

	//auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
	//if (model && model->GetStageName() == CARM_RESET_TUBE_CIRCULAR_FINISHRESET)
	//{
	//	m_isResetting = FALSE;
	//	m_isResettingSwing = FALSE;
	//	m_isResettingCircular = FALSE;

	//	m_postionManager->NotifySystemIsReady();

	//	auto logicDevice = (LogicDeviceMechCarm*)LogicDeviceManager::Instance()->Resove(LOGICDEVICE_MECH_CARM);
	//	if (logicDevice)
	//	{
	//		logicDevice->NotifyMachineryReadyState(true);
	//	}

	//	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler]->[NotifySystemIsReady]");
	//}

	//if (m_useHandSwitchControlReset)
	//{
	//	ChangeMotionModelStage(RESET_STAGE_STOP_MOVE);
	//}
}

void CArmResetHandler::OnTubeAngleLowLimitPos()
{

}

void CArmResetHandler::OnTubeAngleHighLimitPos()
{

}

void CArmResetHandler::OnTubeHeightLowLimitPos()
{

}

void CArmResetHandler::OnTubeHeightHighLimitPos()
{

}

void CArmResetHandler::OnTubeAngleLeaveLowLimitPos()
{
	if (m_isResettingSwing)
	{
		ChangeMotionModelStage(CARM_RESET_TUBE_SWING_DETACH_Z_AXIS);
		ChangeMotionModelStage(CARM_RESET_TUBE_SWING_STOP_AT_LANDMARK);
		ChangeMotionModelStage(CARM_RESET_TUBE_SWING_FINISHRESET);
	}
}

void CArmResetHandler::OnTubeAngleLeaveHighLimitPos()
{
	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler]->[OnTubeAngleLeaveHighLimitPos][Enter][m_isResettingSwing = {$:d}, m_isResetting = {$:d}]", m_isResettingSwing, m_isResetting);
	if (m_isResettingSwing)
	{
		ChangeMotionModelStage(CARM_RESET_TUBE_SWING_DETACH_Z_AXIS);
		ChangeMotionModelStage(CARM_RESET_TUBE_SWING_STOP_AT_LANDMARK);
		ChangeMotionModelStage(CARM_RESET_TUBE_SWING_FINISHRESET);
	}
	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler]->[OnTubeAngleLeaveHighLimitPos][Leave]");
}

void CArmResetHandler::OnTubeHeightLeaveLowLimitPos()
{
	if (m_isResettingCircular)
	{
		ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_DETACH_Z_AXIS);
		ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_STOP_AT_LANDMARK);
		ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_FINISHRESET);
	}
}

void CArmResetHandler::OnTubeHeightLeaveHighLimitPos()
{
	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler]->[OnTubeHeightLeaveHighLimitPos][Enter][m_isResettingCircular = {$:d}, m_isResetting = {$:d}]", m_isResettingCircular, m_isResetting);
	if (m_isResettingCircular)
	{
		ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_DETACH_Z_AXIS);
		ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_STOP_AT_LANDMARK);
		ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_FINISHRESET);
	}
	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler]->[OnTubeHeightLeaveHighLimitPos][Leave]");
}

void CArmResetHandler::OnTubeAngleEncoderZPos()
{

}

void CArmResetHandler::OnTubeHeightEncoderZPos()
{

}

void CArmResetHandler::OnExit()
{

}

void CArmResetHandler::DoStartReset()
{
	if (m_isResetting)
	{
		return;
	}

	m_isResetting = TRUE;

	StartSwingAxisReset();
}

void CArmResetHandler::DoStartSwingReset()
{
	if (m_isResetting)
	{
		return;
	}

	m_isResetting = TRUE;
	m_isResettingSwing = TRUE;

	StartSwingAxisReset();
}

void CArmResetHandler::DoStartCircularReset()
{
	if (m_isResetting)
	{
		return;
	}

	m_isResetting = TRUE;
	m_isResettingCircular = TRUE;

	StartCircularAxisReset();
}

void CArmResetHandler::ChangeMotionModelStage(const std::string &stageName)
{
	if (m_isResetting)
	{
		auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
		if (model)
		{
			model->ChangeStage(stageName);
			if (stageName == CARM_RESET_TUBE_SWING_FINISHRESET)
			{
				auto stageArgs = model->GetStageArgs();
				stageArgs->SetTubeAngleMotionStatus(TRUE);
				MechnicalMonitor::Instance()->BeginMonitor(this, MO_TUBE_ANGLE);
			}
			else if (stageName == CARM_RESET_TUBE_CIRCULAR_FINISHRESET)
			{
				auto stageArgs = model->GetStageArgs();
				stageArgs->SetTubeHeightMotionStatus(TRUE);
				MechnicalMonitor::Instance()->BeginMonitor(this, MO_TUBE_HEIGHT);
			}
		}
	}
}

void CArmResetHandler::StartSwingAxisReset()
{
	if (m_isResettingSwing)
	{
		ChangeMotionModelStage(CARM_RESET_TUBE_SWING_ATTACH_Z_AXIS);
		ChangeMotionModelStage(CARM_RESET_TUBE_SWING_FIND_LANDMARK);

		auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
		auto stageArg = model->GetStageArgs();

		if (stageArg->IsTubeAngleAtResetLandmark)
		{
			stageArg->IsTubeAngleAtResetLandmark = FALSE;
			ChangeMotionModelStage(CARM_RESET_TUBE_SWING_DETACH_Z_AXIS);
			ChangeMotionModelStage(CARM_RESET_TUBE_SWING_FINISHRESET);
		}
	}
}

void CArmResetHandler::OnMotionEventTubeSwingStop()
{
	if (m_isResettingSwing)
	{
		auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
		auto stageArg = model->GetStageArgs();

		if (model->GetStageName() == CARM_RESET_TUBE_SWING_FINISHRESET)
		{
			m_isResetting = FALSE;
			m_isResettingSwing = FALSE;

			MechnicalMonitor::Instance()->EndMonitor();

			m_postionManager->NotifySystemIsReady(CONTROLLER_CARM_TUBE_SWING);
		}
	}
}

void CArmResetHandler::OnMotionEventTubeCircularStop()
{
	if (m_isResettingCircular)
	{
		auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
		auto stageArg = model->GetStageArgs();

		if (model->GetStageName() == CARM_RESET_TUBE_CIRCULAR_FINISHRESET)
		{
			m_isResetting = FALSE;
			m_isResettingCircular = FALSE;

			MechnicalMonitor::Instance()->EndMonitor();

			m_postionManager->NotifySystemIsReady(CONTROLLER_CARM_TUBE_CIRCULAR);
		}
	}
}

void CArmResetHandler::StartCircularAxisReset()
{
	if (m_isResettingCircular)
	{
		ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_ATTACH_Z_AXIS);
		ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_FIND_LANDMARK);

		auto model = m_modelManager->Resove(MOTION_MODLE_CARM_RESTE);
		auto stageArg = model->GetStageArgs();

		if (stageArg->IsTubeHeightAtResetLandmark)
		{
			stageArg->IsTubeHeightAtResetLandmark = FALSE;
			ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_DETACH_Z_AXIS);
			ChangeMotionModelStage(CARM_RESET_TUBE_CIRCULAR_FINISHRESET);
		}
	}
}

RET_STATUS CArmResetHandler::OnStartMove(DOF_MECH mech, int nOrientation)
{
	auto model = m_modelManager->Resove(MOTION_MODLE_CARM_GENERAL);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	if (!IsMovingEnable())
	{
		return RET_STATUS::RET_FAILED;
	}

	auto modelArgs = (CArmGeneralMotionStageArgs *)model->GetStageArgs();
	
	if (mech == TOMO_TUBE_HEIGHT)
	{
		modelArgs->TubeCircularMoveDirection = nOrientation;
		modelArgs->TubeCircularMoveStep = -1;

		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnStartMove]->[Enter][TOMO_TUBE_CIRCULAR][Org: {$:d}]", modelArgs->TubeSwingDirection);

		model->ChangeStage(CARM_GENERAL_MOVE_TUBE_CIRCULAR);
	}
	else if (mech == TOMO_TUBE_ANGLE)
	{
		modelArgs->TubeSwingDirection = nOrientation;
		modelArgs->TubeSwingStep = -1;

		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnStartMove]->[Enter][TOMO_TUBE_ANGLE][Org: {$:d}]", modelArgs->TubeSwingDirection);

		model->ChangeStage(CARM_GENERAL_MOVE_TUBE_SWING);
	}

	m_bMachineMoving = TRUE;

	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnStartMove]->[Leave]");

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CArmResetHandler::OnStopMove(DOF_MECH mech)
{
	m_bMachineMoving = FALSE;

	if (!IsMovingEnable())
	{
		return RET_STATUS::RET_FAILED;
	}

	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnStopMove]->[Enter]");

	auto model = m_modelManager->Resove(MOTION_MODLE_CARM_GENERAL);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CARM_GENERAL_STOP_MECH_MOVING);

	if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnStopMove]->[Leave]");

	return RET_STATUS::RET_SUCCEED;
}

BOOL CArmResetHandler::IsMovingEnable()
{
	BOOL bRes = TRUE;
	if (m_isResetting)
	{
		if(gbusinessLog) gbusinessLog->Warn("[CArmResetHandler][IsMovingEnable]->[Reset is working, single motion is not allowed]");

		bRes = FALSE;
	}

	if (m_bMachineMoving)
	{
		if(gbusinessLog) gbusinessLog->Warn("[CArmResetHandler][IsMovingEnable]->[Machine is moving, single motion is not allowed]");

		bRes = FALSE;
	}

	return bRes;
}

RET_STATUS CArmResetHandler::OnScanningSwitch(DOF_MECH mech)
{
	m_MechReset = mech;

	if (m_MechReset == TOMO_TUBE_HEIGHT)
	{
		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnScanningSwitch]->[Using circular for scanning]");
	}
	else if (m_MechReset == TOMO_TUBE_ANGLE)
	{
		if(gbusinessLog) gbusinessLog->Info("[CArmResetHandler][OnScanningSwitch]->[Using swing for scanning]");
	}

	return RET_STATUS::RET_SUCCEED;
}