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

#include "stdafx.h"
#include "CalibrationHandler.h"
#include "IMotionModel.h"
#include "IMotionModelManager.h"
#include "CalibrationStageArgs.h"
#include "MotionStages.h"
#include "ConfigurerMotion.h"
#include "ConfigurerCalibration.h"
#include "MechnicalMonitor.h"
#include "RADMotionStageArgs.h"

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

static bool LeastSquare(const vector<float>& x, const vector<float>& y, float &a, float &b)//最小二乘法公式
{
	bool bRes = false;
	if (x.size() > 0 && x.size() == y.size())
	{
		float t1 = 0, t2 = 0, t3 = 0, t4 = 0;
		for (int i = 0; i < x.size(); ++i)
		{
			t1 += x[i] * x[i];
			t2 += x[i];
			t3 += x[i] * y[i];
			t4 += y[i];
		}
		a = (t3 * x.size() - t2 * t4) / (t1 * x.size() - t2 * t2);
		b = (t1 * t4 - t2 * t3) / (t1 * x.size() - t2 * t2);

		bRes = true;
	}

	if(gbusinessLog) gbusinessLog->Info("[CalibrationHandler][LeastSquare]->[{$:f9} , {$:f9}]", a, b);

	return bRes;
}

CalibrationHandler::CalibrationHandler() 
:m_modelLoaded(FALSE),
m_modelManager(nullptr)
{
}


CalibrationHandler::~CalibrationHandler()
{
}

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

void CalibrationHandler::OnCoordinatesLoaded(IPositionManager *coordinates)
{
}

RET_STATUS CalibrationHandler::OnSelectExamMode(const char *pExamKey)
{
	if (!m_modelLoaded)
	{
		auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
		if (model)
		{
			ResDataObject mechparms;
			if (ConfigurerMotion::GetMachineryConfigs(mechparms))
			{
				model->LoadMachineryParams(mechparms);
			}
		}

		m_modelLoaded = TRUE;
	}

	return RET_STATUS::RET_SUCCEED;
}

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

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

	auto stageArg = model->GetStageArgs();

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

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

		ChangeAutoCalibrationADToNextStage(MO_TUBE_HEIGHT);
	}
}

RET_STATUS CalibrationHandler::OnStopMech()
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (model)
	{
		model->ChangeStage(CALIBRATION_STOP_TUBE_HEIGHT_AT_ZAXIS);
		model->ChangeStage(CALIBRATION_STOP_TUBE_ANGLE_AT_ZAXIS);
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnMoveTubeHeight(ResDataObject &ParamIn)
{
	int direction = 1;
	int steps = 0;
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	auto modelArgs = (CalibrationStageArgs *)model->GetStageArgs();

	if (ParamIn.GetFirstOf("Orientation") >= 0
		&& ParamIn.GetFirstOf("StepNumber") >= 0)
	{
		auto positive = ConfigurerMotion::GetTubeHeightAxisPositiveDirection() > 0 ? 1 : -1;
		auto ori = atoi(ParamIn["Orientation"]) > 0 ? positive : (-1 * positive);
		modelArgs->TubeHeightMoveDirection = ori;

		if (ParamIn.GetFirstOf("AsPhysical") >= 0)
		{
			modelArgs->TubeHeightMovePhysical = (float)atof(ParamIn["StepNumber"]);
			model->ChangeStage(CALIBRATION_MOVE_TUBE_HEIGHT_AS_PHYSICAL);
		}
		else
		{
			modelArgs->TubeHeightMoveStep = atoi(ParamIn["StepNumber"]);
			model->ChangeStage(CALIBRATION_MOVE_TUBE_HEIGHT);
		}	
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnRotateTubeAngle(ResDataObject &ParamIn)
{
	int direction = 1;
	int steps = 0;
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	auto modelArgs = (CalibrationStageArgs *)model->GetStageArgs();

	if (ParamIn.GetFirstOf("Orientation") >= 0
		&& ParamIn.GetFirstOf("StepNumber") >= 0)
	{
		auto positive = ConfigurerMotion::GetTubeRotateAxisPositiveDirection() > 0 ? 1 : -1;
		auto ori = atoi(ParamIn["Orientation"]) > 0 ? positive : (-1 * positive);
		modelArgs->TubeAngleRotateDirection = ori;

		if (ParamIn.GetFirstOf("AsPhysical") >= 0)
		{
			modelArgs->TubeAngleRotatePhysical = (float)atof(ParamIn["StepNumber"]);

			model->ChangeStage(CALIBRATION_ROTATE_TUBE_ANGLE_AS_PHYSICAL);
		}
		else
		{
			modelArgs->TubeAngleRotateStep = atoi(ParamIn["StepNumber"]);

			model->ChangeStage(CALIBRATION_ROTATE_TUBE_ANGLE);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetTubeHeightAD(ResDataObject &ParamOut)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_GET_TUBE_HEIGHT_AD);
	auto modelArgs = (CalibrationStageArgs *)model->GetStageArgs();
	ParamOut.add("ADTubeHeight", modelArgs->TubeHeightAD);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetTubeAngleAD(ResDataObject &ParamOut)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_GET_TUBE_ANGLE_AD);
	auto modelArgs = (CalibrationStageArgs *)model->GetStageArgs();
	ParamOut.add("ADTubeAngle", modelArgs->TubeAngleAD);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetDetectorHeightAD(ResDataObject &ParamOut)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_GET_DETECTOR_HEIGHT_AD);

	auto modelArgs = (CalibrationStageArgs *)model->GetStageArgs();
	ParamOut.add("ADDetectorHeight", modelArgs->DetectorHeightAD);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetTubeHeightEncoder(ResDataObject &ParamOut)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_GET_TUBE_HEIGHT_ENCODER);
	auto modelArgs = (CalibrationStageArgs *)model->GetStageArgs();
	ParamOut.add("EncoderTubeHeight", modelArgs->TubeHeightEncoder);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetTubeAngleEncoder(ResDataObject &ParamOut)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_GET_TUBE_ANGLE_ENCODER);
	auto modelArgs = (CalibrationStageArgs *)model->GetStageArgs();
	ParamOut.add("EncoderTubeAngle", modelArgs->TubeAngleEncoder);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetDetectorHeightEncoder(ResDataObject &ParamOut)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_GET_DETECTOR_HEIGHT_ENCODER);

	auto modelArgs = (CalibrationStageArgs *)model->GetStageArgs();
	ParamOut.add("EncoderDetectorHeight", modelArgs->DetectorHeightEncoder);

	return RET_STATUS::RET_SUCCEED;
}

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

	model->ChangeStage(CALIBRATION_ACTIVE_TUBE_ANGLE_ZAXIS_CLEAR);

	return RET_STATUS::RET_SUCCEED;
}

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

	model->ChangeStage(CALIBRATION_CANCEL_TUBE_ANGLE_ZAXIS_CLEAR);

	return RET_STATUS::RET_SUCCEED;
}

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

	model->ChangeStage(CALIBRATION_ACTIVE_TUBE_HEIGHT_ZAXIS_CLEAR);

	return RET_STATUS::RET_SUCCEED;
}

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

	model->ChangeStage(CALIBRATION_CANCEL_TUBE_HEIGHT_ZAXIS_CLEAR);

	return RET_STATUS::RET_SUCCEED;
}

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

	model->ChangeStage(CALIBRATION_ROTATE_TUBE_ANGLE_TO_ZAXIS);

	return RET_STATUS::RET_SUCCEED;
}

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

	model->ChangeStage(CALIBRATION_MVOE_TUBE_HEIGHT_TO_ZAXIS);

	return RET_STATUS::RET_SUCCEED;
}

void CalibrationHandler::OnTubeAngleEncoderZPos()
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (model)
	{
		model->ChangeStage(CALIBRATION_STOP_TUBE_ANGLE_AT_ZAXIS);
	}
}

void CalibrationHandler::OnTubeHeightEncoderZPos()
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (model)
	{
		model->ChangeStage(CALIBRATION_STOP_TUBE_HEIGHT_AT_ZAXIS);
	}
}

RET_STATUS CalibrationHandler::OnSwitchSvoStatus(ResDataObject &ParamIn)
{
	if (ParamIn.GetFirstOf("DOF") < 0 || ParamIn.GetFirstOf("SvoStatus") < 0)
	{
		return RET_STATUS::RET_FAILED;
	}

	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	auto stageArgs = (CalibrationStageArgs*)model->GetStageArgs();
	stageArgs->SvoStatus = atoi((const char*)ParamIn["SvoStatus"]);
	stageArgs->AutoCalADDof = atoi((const char *)ParamIn["DOF"]);

	model->ChangeStage(CALIBRATION_SWITCH_SVO_STATUS);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnAutoCalibrationAD(ResDataObject &ParamIn)
{
	if (ParamIn.GetFirstOf("DOF") < 0
		|| ParamIn.GetFirstOf("CurrentHeight") < 0
		|| ParamIn.GetFirstOf("HighLimit") < 0
		|| ParamIn.GetFirstOf("LowLimit") < 0)
	{
		return RET_STATUS::RET_FAILED;
	}

	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	auto stageArgs = (CalibrationStageArgs*)model->GetStageArgs();
	stageArgs->AutoCalADDof = atoi(ParamIn["DOF"]);
	stageArgs->AutoCalADCurrentPhysical = (float)atof(ParamIn["CurrentHeight"]);
	stageArgs->AutoCalADHighLimit = (float)atof(ParamIn["HighLimit"]);
	stageArgs->AutoCalADLowLimit = (float)atof(ParamIn["LowLimit"]);
	stageArgs->AutoCalADCurrentStep = 0;
	stageArgs->AutoCalADTotalStep = 12;

	if(gbusinessLog) gbusinessLog->Info("[CalibrationHandler][OnAutoCalibrationAD]->[{$:f6} {$:f6} {$:f6}]",
		stageArgs->AutoCalADCurrentPhysical,
		stageArgs->AutoCalADHighLimit,
		stageArgs->AutoCalADLowLimit);

	if (stageArgs->AutoCalADCurrentPhysical < 1e-2
		|| stageArgs->AutoCalADHighLimit < 1e-2
		|| stageArgs->AutoCalADLowLimit < 1e-2
		|| (stageArgs->AutoCalADHighLimit < stageArgs->AutoCalADLowLimit))
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_AUTO_CALIBRATION_AD_START);
	if (stageArgs->AutoCalADDof == 0)
	{
		MechnicalMonitor::Instance()->BeginMonitor(this, MO_TUBE_ANGLE);
	}
	else if (stageArgs->AutoCalADDof == 1)
	{
		MechnicalMonitor::Instance()->BeginMonitor(this, MO_TUBE_HEIGHT);
	}

	return RET_STATUS::RET_SUCCEED;
}

void CalibrationHandler::ChangeAutoCalibrationADToNextStage(int offset)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	auto stageArgs = (CalibrationStageArgs*)model->GetStageArgs();
	
	if (stageArgs->AutoCalADTotalStep == stageArgs->AutoCalADCurrentStep)
	{
		//自动AD校正完成
		model->ChangeStage(CALIBRATION_AUTO_CALIBRATION_AD_END);
		if (stageArgs->AutoCalADDof == 0)
		{
			float slope = 0.0f;
			float intercept = 0.0f;
			if (LeastSquare(stageArgs->AutoCalADADs, stageArgs->AutoCalADPhysicals, slope, intercept))
			{
				ConfigurerCalibration::UpdateTubeAngleADToAngleCurve(slope, intercept);
			}
		}
		else if (stageArgs->AutoCalADDof == 1)
		{
			float slope = 0.0f;
			float intercept = 0.0f;
			if (LeastSquare(stageArgs->AutoCalADADs, stageArgs->AutoCalADPhysicals, slope, intercept))
			{
				ConfigurerCalibration::UpdateTubeHeightADToHeightCurve(slope, intercept);
			}
		}
	}
	else
	{
		model->ChangeStage(CALIBRATION_AUTO_CALIBRATION_AD_NEXT);
		stageArgs->AutoCalADCurrentStep++;
		MechnicalMonitor::Instance()->BeginMonitor(this, offset);	
	}
}

RET_STATUS CalibrationHandler::OnSaveTubeHeightADCalibrationResult(ResDataObject &ParamIn, ResDataObject &pParamOut)
{
	if(gbusinessLog) gbusinessLog->Info("[CalibrationHandler][OnSaveTubeHeightADCalibrationResult]->[{$}]", ParamIn.encode());

	std::vector<float> physicals;
	std::vector<float> ads;
	if (ParsePhysicsAndADMapParams(ParamIn, physicals, ads))
	{
		float slope = 0.0f;
		float intercept = 0.0f;
		if (LeastSquare(ads, physicals, slope, intercept))
		{
			ConfigurerCalibration::UpdateTubeHeightADToHeightCurve(slope, intercept);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnSaveTubeAngleADCalibrationResult(ResDataObject &ParamIn, ResDataObject &pParamOut)
{
	if(gbusinessLog) gbusinessLog->Info("[CalibrationHandler][OnSaveTubeAngleADCalibrationResult]->[{$}]", ParamIn.encode());

	std::vector<float> physicals;
	std::vector<float> ads;
	if (ParsePhysicsAndADMapParams(ParamIn, physicals, ads))
	{
		float slope = 0.0f;
		float intercept = 0.0f;
		if (LeastSquare(ads, physicals, slope, intercept))
		{
			ConfigurerCalibration::UpdateTubeAngleADToAngleCurve(slope, intercept);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnSaveDetectorADCalibrationResult(ResDataObject &ParamIn, ResDataObject &pParamOut)
{
	if(gbusinessLog) gbusinessLog->Info("[CalibrationHandler][OnSaveDetectorADCalibrationResult]->[{$}]", ParamIn.encode());

	std::vector<float> physicals;
	std::vector<float> ads;
	if (ParsePhysicsAndADMapParams(ParamIn, physicals, ads))
	{
		float slope = 0.0f;
		float intercept = 0.0f;
		if (LeastSquare(ads, physicals, slope, intercept))
		{
			ConfigurerCalibration::UpdateDetectorHeightADToHeightCurve(slope, intercept);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}

bool CalibrationHandler::ParsePhysicsAndADMapParams(ResDataObject &ParamIn, std::vector<float> &physical, std::vector<float> &ads)
{
	string strKeyCount = "PointCount";

	int nPointCount = 0;
	if (ParamIn.GetFirstOf(strKeyCount.c_str()) >= 0)
	{
		nPointCount = atoi((const char *)ParamIn[strKeyCount.c_str()]);
	}
	else
	{
		return false;
	}

	for (int n = 0; n < nPointCount; n++)
	{
		char tmp1[64] = { 0 };
		char tmp2[64] = { 0 };
		sprintf_s(tmp1, "Physical%d", n);
		sprintf_s(tmp2, "AD%d", n);
		if (ParamIn.GetFirstOf(tmp1) >= 0 && ParamIn.GetFirstOf(tmp2) >= 0)
		{
			physical.push_back((float)atof((const char *)ParamIn[tmp1]));
			ads.push_back(atoi((const char *)ParamIn[tmp2]) * 1.0f);
		}		
	}

	return true;
}

BOOL CalibrationHandler::IsMovingEnable()
{
	BOOL bRes = TRUE;

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

		bRes = FALSE;
	}

	return bRes;
}

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

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

	auto modelArgs = (RADMotionStageArgs*)model->GetStageArgs();

	if (mech == TOMO_TUBE_HEIGHT)
	{
		modelArgs->TubeHeightDirection = nOrientation;
		modelArgs->TubeHeightStep = -1;

		if (gbusinessLog) gbusinessLog->Info("[CalibrationHandler][OnStartMove]->[Enter][TOMO_TUBE_HEIGHT][Org: {$:d}]", modelArgs->TubeHeightDirection);

		model->ChangeStage(RAD_STAGE_MOVE_TUBE_HEIGHT);
	}
	else if (mech == TOMO_TUBE_HORIZONTAL)
	{
		modelArgs->TubeHorizontalMoveDirection = nOrientation;
		modelArgs->TubeHorizontalMoveStep = -1;

		if (gbusinessLog) gbusinessLog->Info("[CalibrationHandler][OnStartMove]->[Enter][TOMO_TUBE_HORIZONTAL][Org: {$:d}]", modelArgs->TubeHeightDirection);

		model->ChangeStage(RAD_STAGE_MOVE_TUBE_HORIZONTAL);
	}
	else if (mech == TOMO_TUBE_ANGLE)
	{
		modelArgs->TubeAngleDirection = nOrientation;
		modelArgs->TubeAngleStep = -1;

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

		model->ChangeStage(RAD_STAGE_MOVE_TUBE_ROTATION);
	}

	m_bMachineMoving = TRUE;

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

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnStopMove(DOF_MECH mech)
{
	if (gbusinessLog) gbusinessLog->Info("[CalibrationHandler][OnStopMove]->[Enter]");

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

	model->ChangeStage(RAD_STAGE_STOP_MOVE);

	m_bMachineMoving = FALSE;

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

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnMoveTubeHorizontal(ResDataObject& ParamIn)
{
	int direction = 1;
	int steps = 0;
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	auto modelArgs = (CalibrationStageArgs*)model->GetStageArgs();

	if (ParamIn.GetFirstOf("Orientation") >= 0
		&& ParamIn.GetFirstOf("StepNumber") >= 0)
	{
		auto positive = ConfigurerMotion::GetTubeHorizontalAxisPositiveDirection() > 0 ? 1 : -1;
		auto ori = atoi(ParamIn["Orientation"]) > 0 ? positive : (-1 * positive);
		modelArgs->TubeHorizontalMoveDirection = ori;

		if (ParamIn.GetFirstOf("AsPhysical") >= 0)
		{
			modelArgs->TubeHorizontalMovePhysical = (float)atof(ParamIn["StepNumber"]);
			model->ChangeStage(CALIBRATION_MOVE_TUBE_HORIZONTAL_AS_PHYSICAL);
		}
		else
		{
			modelArgs->TubeHorizontalMoveStep = atoi(ParamIn["StepNumber"]);
			model->ChangeStage(CALIBRATION_MOVE_TUBE_HORIZONTAL);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetTubeHorizontalAD(ResDataObject& ParamOut)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_GET_TUBE_HORIZONTAL_AD);
	auto modelArgs = (CalibrationStageArgs*)model->GetStageArgs();
	ParamOut.add("ADTubeHeight", modelArgs->TubeHorizontalAD);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetTubeHorizontalEncoder(ResDataObject& ParamOut)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_GET_TUBE_HORIZONTAL_ENCODER);
	auto modelArgs = (CalibrationStageArgs*)model->GetStageArgs();
	ParamOut.add("EncoderTubeHeight", modelArgs->TubeHorizontalEncoder);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetDetectorHorizontalAD(ResDataObject& ParamOut)
{
	auto model = m_modelManager->Resove(MOTION_MODEL_CALIBRATION);
	if (!model)
	{
		return RET_STATUS::RET_FAILED;
	}

	model->ChangeStage(CALIBRATION_GET_DETECTOR_HORIZONTAL_AD);

	auto modelArgs = (CalibrationStageArgs*)model->GetStageArgs();
	ParamOut.add("ADDetectorHeight", modelArgs->DetectorHorizontalAD);

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnSaveTubeHorizontalADCalibrationResult(ResDataObject& ParamIn, ResDataObject& pParamOut)
{
	if (gbusinessLog) gbusinessLog->Info("[CalibrationHandler][OnSaveTubeHorizontalADCalibrationResult]->[{$}]", ParamIn.encode());

	std::vector<float> physicals;
	std::vector<float> ads;
	if (ParsePhysicsAndADMapParams(ParamIn, physicals, ads))
	{
		float slope = 0.0f;
		float intercept = 0.0f;
		if (LeastSquare(ads, physicals, slope, intercept))
		{
			ConfigurerCalibration::UpdateTubeHorizontalADToPositionCurve(slope, intercept);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnSaveDetectorHorizontalADCalibrationResult(ResDataObject& ParamIn, ResDataObject& pParamOut)
{
	if (gbusinessLog) gbusinessLog->Info("[CalibrationHandler][OnSaveDetectorHorizontalADCalibrationResult]->[{$}]", ParamIn.encode());

	std::vector<float> physicals;
	std::vector<float> ads;
	if (ParsePhysicsAndADMapParams(ParamIn, physicals, ads))
	{
		float slope = 0.0f;
		float intercept = 0.0f;
		if (LeastSquare(ads, physicals, slope, intercept))
		{
			ConfigurerCalibration::UpdateDetectorHorizontalADToPositionCurve(slope, intercept);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnSaveResetLowPosition(ResDataObject& ParamIn, ResDataObject& pParamOut)
{
	string motionUnit;
	string lowpos;
	if (ParamIn.GetFirstOf("MotionUnit") >= 0)
	{
		motionUnit = (const char*)(ParamIn["MotionUnit"]);
	}

	if (ParamIn.GetFirstOf("LowPosition") >= 0)
	{
		lowpos = (const char*)(ParamIn["LowPosition"]);
	}

	if (motionUnit == "TubeAngle")
	{
		if (ParamIn.GetFirstOf("LowPosition") >= 0)
		{
			ConfigurerMotion::UpdateResetLowPositionTubleAngle(lowpos);
		}
	}
	else if (motionUnit == "TubeVertical")
	{
		if (ParamIn.GetFirstOf("LowPosition") >= 0)
		{
			ConfigurerMotion::UpdateResetLowPositionTubleHeight(lowpos);
		}
	}
	else if (motionUnit == "TubeHorizontal")
	{
		if (ParamIn.GetFirstOf("LowPosition") >= 0)
		{
			ConfigurerMotion::UpdateResetLowPositionTubleHorizontal(lowpos);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnSaveResetEncorderZeroPosition(ResDataObject& ParamIn, ResDataObject& pParamOut)
{
	string motionUnit;
	string encoderzeropos;
	if (ParamIn.GetFirstOf("MotionUnit") >= 0)
	{
		motionUnit = (const char*)ParamIn["MotionUnit"];
	}
	else
	{
		return RET_STATUS::RET_FAILED;
	}

	if (ParamIn.GetFirstOf("EncoderZeroPosition") >= 0)
	{
		encoderzeropos = (const char*)(ParamIn["EncoderZeroPosition"]);
	}

	if (motionUnit == "TubeAngle")
	{
		if (ParamIn.GetFirstOf("EncoderZeroPosition") >= 0)
		{
			ConfigurerMotion::UpdateResetEncoderZeroPositionTubeAngle(encoderzeropos);
		}
	}
	else if (motionUnit == "TubeVertical")
	{
		if (ParamIn.GetFirstOf("EncoderZeroPosition") >= 0)
		{
			ConfigurerMotion::UpdateResetEncoderZeroPositionTubeHeight(encoderzeropos);
		}
	}
	else if (motionUnit == "TubeHorizontal")
	{
		if (ParamIn.GetFirstOf("EncoderZeroPosition") >= 0)
		{
			ConfigurerMotion::UpdateResetEncoderZeroPositionTubeHorizontal(encoderzeropos);
		}
	}

	return RET_STATUS::RET_SUCCEED;
}

RET_STATUS CalibrationHandler::OnGetResetPosition(ResDataObject& ParamIn, ResDataObject& pParamOut)
{
	ResDataObject SubParamOutTubeHeight;
	SubParamOutTubeHeight.add("LowPosition", ConfigurerMotion::GetTubeHeightLowLandmarkPos());
	SubParamOutTubeHeight.add("EncoderZeroPosition", ConfigurerMotion::GetAbsoluteValueAtTubeHeightOrigin());
	pParamOut.add("TubeHeight", SubParamOutTubeHeight);

	ResDataObject SubParamOutTubeHorizontal;
	SubParamOutTubeHorizontal.add("LowPosition", ConfigurerMotion::GetTubeHorizontalLowLandmarkPos());
	SubParamOutTubeHorizontal.add("EncoderZeroPosition", ConfigurerMotion::GetAbsoluteValueAtTubeHorizontalOrigin());
	pParamOut.add("TubeHorizontal", SubParamOutTubeHorizontal);

	return RET_STATUS::RET_SUCCEED;
}