PhysicalDevice/Machine/MachineryECOM/Trunk/DIOS.Dev.Machinery.Motion.Driver/DIOS.Dev.Machinery.Driver.ECOM/CARM/CBCTExamHandler.cpp

#include "stdafx.h"
#include "CBCTExamHandler.h"
#include "ConfigurerMotion.h"
#include "IMotionModelManager.h"
#include "IMotionModel.h"
#include "ConfigurerPositionCode.h"
#include "MotionStages.h"
#include "CBCTMotionStageArgs.h"
#include "IPositionManager.h"
#include "..\\LogicDeviceHandSwitch.h"
#include "LogicDeviceManager.h"
#include "MechnicalMonitor.h"
#include "PacketDefine.h"
#include "FeedbackDefine.h"
#include "LogicDriverDPCProxy.h"
#include "CArmGeneralMotionStageArgs.h"
#include "LogicDeviceMechCarm.h"

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

CBCTExamHandler::CBCTExamHandler()
	:m_handSwitchState(HS_NONE),
	m_modelLoaded(FALSE),
	m_postionManager(nullptr),
	m_modelManager(nullptr),
	m_cbctPerformGate(CBCTLG_CHANNEL_RESET),
	m_mechCBCT(TOMO_TUBE_HEIGHT)
{
}


CBCTExamHandler::~CBCTExamHandler()
{
}

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

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

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

	ChangeMotionModelStage(CBCT_STAGE_CLEAR_PARAMS);

	if (!m_modelLoaded)
	{
		auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);
		if (model)
		{
			ResDataObject modelparms;
			if (ConfigurerMotion::GetModelConfigs(modelparms))
			{
				model->LoadModelParams(modelparms);
			}

			ResDataObject mechparms;
			if (ConfigurerMotion::GetMachineryConfigs(mechparms))
			{
				model->LoadMachineryParams(mechparms);
			}

			m_modelLoaded = TRUE;
		}
	}

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnSelectExamMode]->[Exit][{$}]", pExamKey);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CBCTExamHandler::OnSetTechParamsInfo(ResDataObject& pParam)
{
	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnSetTechParamsInfo]->[Enter][{$}]", pParam.encode());

	ResDataObject technicals;
	if (ParseTechnicalParams(pParam, technicals))
	{
		auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);
		if (model)
		{
			model->SetTechnicalParams(technicals);
		}

		ChangeMotionModelStage(CBCT_STAGE_CALCULATE_PARAMS);

		NotifyTechParamsToDriver(technicals);
	}

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnSetTechParamsInfo]->[Exit]");

	return RET_STATUS::RET_SUCCEED;
}

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

	auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);
	auto args = (CBCTMotionStageArgs*)model->GetStageArgs();

	if (motionEventName == MONITOR_EVENT_TUBEANGLE_MOVE_STOP)
	{
		args->SetTubeAngleMotionStatus(FALSE);
	}
	else if (motionEventName == MONITOR_EVENT_TUBEHEIGHT_MOVE_STOP)
	{
		args->SetTubeHeightMotionStatus(FALSE);
	}

	if (motionEventName == MONITOR_EVENT_TUBEANGLE_MOVE_STOP
		|| motionEventName == MONITOR_EVENT_TUBEHEIGHT_MOVE_STOP)
	{
		if (args->IsMotionStoped())
		{
			MechnicalMonitor::Instance()->EndMonitor();

			auto currentStage = model->GetStageName();
			if (CBCT_STAGE_MOVETO_START_POS == currentStage)
			{
				auto hswDevice = (LogicDeviceHandSwitch *)LogicDeviceManager::Instance()->Resove(LOGICDEVICE_MECH_HSW);

				UpdateCBCTTriggerGate(CBCTLG_CHANNEL_MOTION_READY);

				//OnHandSwitchGearSecondPressed();
			}
		}
	}

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnMotionEvent]->[Exit][{$}]", motionEventName.c_str());
}

RET_STATUS CBCTExamHandler::OnStopMech()
{
	ChangeMotionModelStage(CBCT_STAGE_MOTION_ERROR_STOP);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CBCTExamHandler::OnFrameAcq()
{
	IExamHandler::OnFrameAcq();

	UpdateCBCTTriggerGate(CBCTLG_CHANNEL_EXPOSURE_READY);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CBCTExamHandler::OnFramePrep()
{
	IExamHandler::OnFramePrep();

	m_cbctPerformGate = CBCTLG_CHANNEL_RESET;

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CBCTExamHandler::OnFrameError()
{
	IExamHandler::OnFrameError();

	ChangeMotionModelStage(CBCT_STAGE_MOTION_ERROR_STOP);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CBCTExamHandler::OnFrameRecover()
{
	IExamHandler::OnFrameRecover();

	ChangeMotionModelStage(CBCT_STAGE_MOTION_ERROR_RECOVER);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CBCTExamHandler::OnSeqError()
{
	IExamHandler::OnSeqError();

	m_cbctPerformGate = CBCTLG_CHANNEL_RESET;

	ChangeMotionModelStage(CBCT_STAGE_MOTION_ERROR_STOP);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CBCTExamHandler::OnSetFrameRate(FLOAT frameRate)
{
	IExamHandler::OnSetFrameRate(frameRate);

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnSetFrameRate]->[{$:f6}]", frameRate);
	auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);
	auto args = (CBCTMotionStageArgs*)model->GetStageArgs();
	args->FPS = frameRate;
	ChangeMotionModelStage(CBCT_STAGE_MOTION_CHANGE_PFS);

	return RET_STATUS::RET_SUCCEED;
}

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

	m_handSwitchState = HS_GEAR_FIRST_PRESSED;

	//m_cbctPerformGate = CBCTLG_CHANNEL_RESET;

	ChangeMotionModelStage(CBCT_STAGE_MOVETO_START_POS);

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

void CBCTExamHandler::OnHandSwitchGearFirstReleased()
{
	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnHandSwitchGearFirstReleased]->[Enter]");

	m_handSwitchState = HS_GEAR_FIRST_RELEASED;

	ChangeMotionModelStage(CBCT_STAGE_STOP_EXPOSURE);

	ChangeMotionModelStage(CBCT_STAGE_MOTION_ERROR_STOP);

	//auto hswDevice = (LogicDeviceHandSwitch *)LogicDeviceManager::Instance()->Resove(LOGICDEVICE_MECH_HSW);
	//if (hswDevice)
	//{
	//	hswDevice->SwitchState(HSW_FIRST_UP);
	//}

	if (m_postionManager)
	{
		auto tubeswing = m_postionManager->GetCurrentPhysical(CARM_TUBE_SWING);
		auto tubecircular = m_postionManager->GetCurrentPhysical(CARM_TUBE_CIRCULAR);

		if (gmotionLog) gmotionLog->Info("[CBCTExamHandler][OnHandSwitchGearFirstReleased]->[Current position {$:f3} {$:f3}]", tubeswing, tubecircular);
	}

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnHandSwitchGearFirstReleased]->[Exit]");
}

void CBCTExamHandler::OnHandSwitchGearSecondPressed()
{
	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnHandSwitchGearSecondPressed]->[Enter]");

	try
	{
		auto lastHandSwitchState = m_handSwitchState;

		m_handSwitchState = HS_GEAR_SECOND_PRESSED;

		if (lastHandSwitchState != HS_GEAR_FIRST_PRESSED)
		{
			throw new exception("[lastHandSwitchState != HS_GEAR_FIRST_PRESSED]");
		}

		auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);
		if (!model)
		{
			throw new exception("[MOTION_MODLE_CBCT is null]");
		}

		UpdateCBCTTriggerGate(CBCTLG_CHANNEL_HANDSWITCH_READY);

		auto stageArgs = (CBCTMotionStageArgs*)model->GetStageArgs();
		if (!stageArgs || !stageArgs->IsMotionStoped())
		{
			throw new exception("[Motion Not Stoped]");
		}

		ChangeMotionModelStage(CBCT_STAGE_START_EXPOSURE);

	}
	catch (exception *e)
	{
		if(gbusinessLog) gbusinessLog->Warn("[CBCTExamHandler][OnHandSwitchGearSecondPressed]->[Exception][{$}]", e->what());
	}

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnHandSwitchGearSecondPressed]->[Exit]");
}

void CBCTExamHandler::OnHandSwitchGearSecondReleased()
{
	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnHandSwitchGearSecondReleased]->[Enter]");

	m_handSwitchState = HS_GEAR_SECOND_RELEASED;

	auto hswDevice = (LogicDeviceHandSwitch *)LogicDeviceManager::Instance()->Resove(LOGICDEVICE_MECH_HSW);
	if (hswDevice)
	{
		hswDevice->SwitchState(HSW_SECOND_UP);
	}

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnHandSwitchGearSecondReleased]->[Exit]");
}

void CBCTExamHandler::OnPWMOffset(PWMOffsetPacket *pwmoff)
{

}

void CBCTExamHandler::OnXrayOn()
{

}

void CBCTExamHandler::OnXrayOff()
{

}

void CBCTExamHandler::OnTubeAngleEncoder(EncoderPacket *tubeangleEncoder)
{

}

void CBCTExamHandler::OnTubeHeightEncoder(EncoderPacket *tubeheightEncoder)
{
	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnTubeHeightEncoder]->[Enter][{$:d08}]", tubeheightEncoder->encoder);

	auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);
	if (model)
	{
		ResDataObject encoders;
		encoders.add("key", CARM_MODLE_FEEDBACK_KEY_CIRCULARENCODER.c_str());
		encoders.add(CARM_MODLE_FEEDBACK_KEY_CIRCULARENCODER.c_str(), tubeheightEncoder->encoder);
		model->OnFeedbackMotionParams(encoders);
	}

	if(gbusinessLog) gbusinessLog->Info("[TomoExamHandler][OnTubeHeightEncoder]->[Exit]");
}

BOOL CBCTExamHandler::ParseTechnicalParams(ResDataObject &paramIn, ResDataObject &paramOut)
{
	try
	{
		if (paramIn.GetFirstOf("SID") < 0)
		{
			TPRINTA_ERROR("SID is not exist");

			return FALSE;
		}

		if (paramIn.GetFirstOf("PositionNumber") < 0)
		{
			TPRINTA_ERROR("PositionNumber is not exist");

			return FALSE;
		}

		std::string strCode = (const char *)paramIn["PositionNumber"];
		int nCode = atoi(strCode.c_str());

		ResDataObject PositionCodeParam;
		if (!ConfigurerPositionCode::GetPositionCodeConfigs(PositionCodeParam))
		{
			TPRINTA_ERROR("PositionCode is not exist");

			return FALSE;
		}

		paramOut.clear();
		std::string strKey = "PC" + strCode;
		if (PositionCodeParam.GetFirstOf(strKey.c_str()) >= 0)
		{
			//30000到40000之间是CBCT的position code范围
			if (nCode > 30000 && nCode < 40000)
			{
				ResDataObject pcset;
				pcset = PositionCodeParam[strKey.c_str()];

				if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][GetPositionCodeConfigs]->[{$}]", pcset.encode());

				paramOut.add("PositionNumber", nCode);
				paramOut.add("CBCTStartPos", pcset["CBCTStartPos"]);
				paramOut.add("CBCTScanAngle", pcset["CBCTScanAngle"]);
				paramOut.add("CBCTMotionSpeed", pcset["CBCTMotionSpeed"]);
				paramOut.add("CBCTProjectionNumber", (int)atoi((const char *)pcset["CBCTProjectionNumber"]));
				paramOut.add("CBCTProjectionDirection", (int)atoi((const char *)pcset["CBCTProjectionDirection"]));

				return TRUE;
			}
		}
	}
	catch (ResDataObjectExption &exp)
	{
		TPRINTA_ERROR(exp.what());
	}
	catch (...)
	{
		TPRINTA_ERROR("unknown exp happen");
	}

	return FALSE;
}

void CBCTExamHandler::NotifyTechParamsToDriver(ResDataObject &technics)
{
	ResDataObject notify;
	float sid = ConfigurerMotion::GetTomoSID();
	float tid = ConfigurerMotion::GetTomoTID();
	if (m_CurWS == WS_WALL)
	{
		sid = ConfigurerMotion::GetTomoSID_Wall();
		tid = ConfigurerMotion::GetTomoTID_Wall();
	}
	else if (m_CurWS == WS_TABLE)
	{
		sid = ConfigurerMotion::GetTomoSID_Table();
		tid = ConfigurerMotion::GetTomoTID_Table();
	}

	std::string geometry = ConfigurerMotion::GetTomoGeometry();
	float angle = 180.0;

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][NotifyTechParamsToDriver]->[{$:f3} {$:f3} {$:f3}]", sid, tid, angle);

	auto logicCarm = (LogicDeviceMechCarm*)LogicDeviceManager::Instance()->Resove(LOGICDEVICE_MECH_CARM);
	if (logicCarm)
	{
		logicCarm->NotifyMachineryTechParams(tid * 100, sid * 100, technics["CBCTProjectionNumber"], technics["CBCTScanAngle"], technics["CBCTProjectionDirection"], geometry);
	}

	//LogicDriverDPCProxy::Instance()->UpdateDriverResource(DRIVER_RESOURCE_TOMO, tid, sid, technics["CBCTProjectionNumber"], angle, technics["CBCTProjectionDirection"], geometry);

}

void CBCTExamHandler::ChangeMotionModelStage(const std::string &stageName)
{
	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][ChangeMotionModelStage]->[{$}]", stageName.c_str());

	auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);
	if (model)
	{
		model->ChangeStage(stageName);

		if (stageName == CBCT_STAGE_MOVETO_START_POS)
		{
			auto args = (CBCTMotionStageArgs*)model->GetStageArgs();
			args->SetTubeAngleMotionStatus(TRUE);
			args->SetTubeHeightMotionStatus(TRUE);
			MechnicalMonitor::Instance()->BeginMonitor(this);
		}
		//if (stageName == CBCT_STAGE_MOVETO_END_POS)
		//{
		//	auto args = (CBCTMotionStageArgs*)model->GetStageArgs();
		//	if (m_mechCBCT == TOMO_TUBE_HEIGHT)
		//	{
		//		args->SetTubeHeightMotionStatus(TRUE);
		//	}
		//	else if (m_mechCBCT == TOMO_TUBE_ANGLE)
		//	{
		//		args->SetTubeAngleMotionStatus(TRUE);
		//	}
		//	MechnicalMonitor::Instance()->BeginMonitor(this);
		//}
	}
}

void CBCTExamHandler::UpdateCBCTTriggerGate(CBCT_LOGIC_GATE_CHANNEL channel)
{
	m_cbctPerformGate |= channel;

	if(gbusinessLog) gbusinessLog->Info("[TomoExamHandler][UpdateTomoLogicGate]->[{$:X03} -> {$:X03}]", channel, m_cbctPerformGate);

	OnCBCTTriggerGateChanged(m_cbctPerformGate);
}

void CBCTExamHandler::OnCBCTTriggerGateChanged(int gateStatus)
{
	if (gateStatus == CBCTLG_CHANNEL_PERFORM_READY)
	{
		DoPerformCBCT();
		m_cbctPerformGate = CBCTLG_CHANNEL_RESET;
	}
}

void CBCTExamHandler::DoPerformCBCT()
{
	ChangeMotionModelStage(CBCT_STAGE_SET_CBCT_SLICE);

	ChangeMotionModelStage(CBCT_STAGE_MOVETO_END_POS);
}

RET_STATUS CBCTExamHandler::OnGetExposurePositions(ResDataObject &resultSwing, ResDataObject &resultCircular)
{
	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnGetExposurePositions]->[Enter]");

	auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);
	if (model)
	{
		ResDataObject res;
		res.add("key", "CircularResult");
		model->GetMotionParams(res);
		resultCircular = res;
	}

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnGetExposurePositions]->[Exit]");

	return RET_STATUS::RET_SUCCEED;
}


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

	if (!IsMovingEnable())
	{
		if(gbusinessLog) gbusinessLog->Warn("[CBCTExamHandler][OnStartMove]->[CBCT is working, single motion is not allowed]");

		return RET_STATUS::RET_FAILED;
	}

	if (m_bMachineMoving)
	{
		if(gbusinessLog) gbusinessLog->Warn("[CBCTExamHandler][OnStartMove]->[Machine is movinig, single motion is not allowed]");

		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("[CBCTExamHandler][OnStartMove]->[Enter][TOMO_TUBE_HEIGHT][Org: {$:d}]", modelArgs->TubeCircularMoveDirection);

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

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

		model->ChangeStage(CARM_GENERAL_MOVE_TUBE_SWING);
	}

	m_bMachineMoving = TRUE;

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

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CBCTExamHandler::OnStopMove(DOF_MECH mech)
{
	if (!IsMovingEnable())
	{
		if(gbusinessLog) gbusinessLog->Warn("[CBCTExamHandler][OnStopMove]->[CBCT is working, single motion is not allowed]");

		return RET_STATUS::RET_FAILED;
	}

	if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][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);

	m_bMachineMoving = FALSE;

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

	return RET_STATUS::RET_SUCCEED;
}

bool CBCTExamHandler::IsMovingEnable()
{
	auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);
	if (model)
	{
		string stageName;
		model->ChangeStage(stageName);

		if (stageName == CBCT_STAGE_MOVETO_START_POS || stageName == CBCT_STAGE_MOVETO_END_POS)
		{
			return false;
		}
	}

	return true;
}

RET_STATUS CBCTExamHandler::OnScanningSwitch(DOF_MECH mech)
{
	auto model = m_modelManager->Resove(MOTION_MODLE_CBCT);

	if (model)
	{
		if (mech == TOMO_TUBE_HEIGHT)
		{
			if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnScanningSwitch]->[Using circular for scanning]");

			model->SwitchScanningComponents(0);
		}
		else if (mech == TOMO_TUBE_ANGLE)
		{
			if(gbusinessLog) gbusinessLog->Info("[CBCTExamHandler][OnScanningSwitch]->[Using swing for scanning]");

			model->SwitchScanningComponents(1);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}