PhysicalDevice/Machine/MachineryECOM/Trunk/DIOS.Dev.Machinery.Common/DIOS.Dev.Machinery.BaseModel/TomoMotionModel.cpp

#include "stdafx.h"
#include "TomoMotionModel.h"
#include "IMachineryManager.h"
#include "MotionStages.h"
#include "TomoMotionStageArgs.h"
#include "ITubeAngleController.h"
#include "ITubeHeightController.h"
#include "ISensorADController.h"
#include "ISensorEncoderController.h"
#include "IExposureController.h"
#include "FeedbackDefine.h"
#include "generatorMove.hpp"
#include "IPositionManager.h"
#include "DigitalTwinLogger.h"
#include "ConfigureExposureDelayTime.h"
#include "TubeLineMotionSwitchController.h"
#include "ConfigurerMotion.h"
#include "ConfigurerWS.h"
#include "ConfigurerTomoMotionLimitation.h"

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

TomoMotionModel::TomoMotionModel() 
:m_StageName(""), 
m_stageArgs(new TomoMotionStageArgs()),
m_coordinates(nullptr),
m_machineryManager(nullptr),
m_motorTubeAngle(nullptr),
m_motorTubeHeight(nullptr),
m_adDetectorHeight(nullptr),
m_encoderTubeAngle(nullptr),
m_encoderTubeHeight(nullptr),
m_motorTubeHorizontal(nullptr),
m_adTubeHorizontal(nullptr),
m_encoderTubeHorizontal(nullptr),
m_tubeLineMotionSwitch(nullptr),
m_exposure(nullptr),
m_warmAngle(0.0f),
m_anglePerSlice(0.0f),
m_angleSliceCount(0),
m_heightSliceCount(0),
m_heightStartPosition(0.0f),
m_angleStartPosition(0.0f),
m_fps(4.0f)
{
	m_heightSlices = new float[TOMO_HEIGHT_SLICE_MAX]{0};
	m_anglePeriods = new int[TOMO_HEIGHT_SLICE_MAX]{0};
	m_angleSteps = new int[TOMO_HEIGHT_SLICE_MAX]{0};
}


TomoMotionModel::~TomoMotionModel()
{
	if (m_stageArgs)
	{
		delete m_stageArgs;
		m_stageArgs = nullptr;
	}
	if (m_heightSlices)
	{
		delete[] m_heightSlices;
		m_heightSlices = nullptr;
	}
	if (m_anglePeriods)
	{
		delete[] m_anglePeriods;
		m_anglePeriods = nullptr;
	}
	if (m_angleSteps)
	{
		delete[] m_angleSteps;
		m_angleSteps = nullptr;
	}
}

void TomoMotionModel::ChangeStage(const std::string &stageName)
{
	m_StageName = stageName;
	OnMotionStage(m_StageName);
}

std::string TomoMotionModel::GetStageName()
{
	return m_StageName;
}

IMotionStageArgs *TomoMotionModel::GetStageArgs()
{
	return m_stageArgs;
}

void TomoMotionModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates)
{
	m_machineryManager = machineryManager;
	m_coordinates = coordinates;

	m_motorTubeAngle = (ITubeAngleController *)(m_machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
	m_motorTubeHeight = (ITubeHeightController *)(m_machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));
	m_adDetectorHeight = (ISensorADController *)(m_machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_AD));
	m_encoderTubeAngle = (ISensorEncoderController *)(m_machineryManager->Resove(CONTROLLER_TUBE_ANGLE_ENCODER));
	m_encoderTubeHeight = (ISensorEncoderController *)(m_machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_ENCODER));
	m_exposure = (IExposureController *)(m_machineryManager->Resove(CONTROLLER_EXPOSURE));
	m_motorTubeHorizontal = (ITubeHeightController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL));
	m_encoderTubeHorizontal = (ISensorEncoderController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL_ENCODER));
	m_tubeLineMotionSwitch = (IOutputController*)(machineryManager->Resove(CONTROLLER_TUBE_LINE_MOTION_SWTICH));

	assert(m_motorTubeAngle);
	assert(m_motorTubeHeight);
	assert(m_adDetectorHeight);
	assert(m_encoderTubeAngle);
	assert(m_encoderTubeHeight);
	assert(m_exposure);
	assert(m_motorTubeHorizontal);
	assert(m_encoderTubeHorizontal);
	assert(m_tubeLineMotionSwitch);
}

void TomoMotionModel::LoadMachineryParams(ResDataObject &params)
{
	m_MachineryParams = params;
}

void TomoMotionModel::LoadModelParams(ResDataObject &params)
{
	m_ModelParams = params;
}

void TomoMotionModel::SetTechnicalParams(ResDataObject &params)
{
	m_TechnicalParams = params;

	DigitalTwinLogger::Instance()->ReceiveExamMode("TOMO");
}

void TomoMotionModel::OnFeedbackMotionParams(ResDataObject &params)
{
	std::string key((const char *)params["key"]);
	if (key == TOMO_MODEL_FEEDBACK_KEY_LINEENCODER)
	{
		m_FeedbackTubeHeightEncoders.push_back((int)params[key.c_str()]);

		DigitalTwinLogger::Instance()->ReceiveTomoExposureHeightPosition((int)params[key.c_str()]);
	}
	else if (key == TOMO_MODEL_FEEDBACK_KEY_ANGLEENCODER)
	{
		m_FeedbackTubeAngleEncoders.push_back((int)params[key.c_str()]);

		DigitalTwinLogger::Instance()->ReceiveTomoExposureAnglePosition((int)params[key.c_str()]);
	}	
}

BOOL TomoMotionModel::GetMotionParams(ResDataObject &params)
{
	std::string key = (const char *)params["key"];
	if (key == "AngleResult")
	{
		params.clear();
		int count = 0;
		for (auto item : m_FeedbackTubeAngleEncoders)
		{
			float angle = m_coordinates->ConvertSensorValue(CONTROLLER_TUBE_ANGLE_ENCODER, item);
			char buffer[10];
			memset(buffer, 0, 10);
			sprintf_s(buffer, "%d", count);
			params.add(buffer, angle);
			++count;
		}
	}
	else if (key == "HeightResult")
	{
		params.clear();
		int count = 0;
		for (auto item : m_FeedbackTubeHeightEncoders)
		{
			float height = m_coordinates->ConvertSensorValue(CONTROLLER_TUBE_HEIGHT_ENCODER, item);
			char buffer[10];
			memset(buffer, 0, 10);
			sprintf_s(buffer, "%d", count);
			params.add(buffer, height * 100);
			++count;
		}
	}
	return TRUE;
}

void TomoMotionModel::OnMotionStage(const std::string &stageName)
{
	if (gmotionLog) gmotionLog->Info("[TomoMotionModel][OnMotionStage]->[Enter][{$}]", stageName.c_str());

	if (stageName == TOMO_STAGE_CALCULATE_PARAMS)
	{
		MotionStageCalculateParams();
	}
	else if (stageName == TOMO_STAGE_SET_TOMO_SLICE)
	{
		MotionStageSetTomoMotionSlice();
	}
	else if (stageName == TOMO_STAGE_MOVETO_END_POS)
	{
		MotionStageMovetoEndPos();
	}
	else if (stageName == TOMO_STAGE_CLEAR_PARAMS)
	{
		MotionStageClearParams();
	}
	else if (stageName == TOMO_STAGE_MOVETO_START_POS_PRE_CENTER_ADJUST)
	{
		MotionStageCenterAdjust();
	}
	else if (stageName == TOMO_STAGE_MOVETO_START_POS)
	{
		MotionStageMovetoStartPos();
	}
	else if (stageName == TOMO_STAGE_MOTION_ERROR_STOP)
	{
		MotionStageErrorStop();
	}
	else if (stageName == TOMO_STAGE_MOTION_ERROR_RECOVER)
	{
		MotionStageErrorRecover();
	}
	else if (stageName == TOMO_STAGE_MOTION_CHANGE_PFS)
	{
		m_fps = m_stageArgs->FPS;
	}
	else if (stageName == TOMO_STAGE_ADJUST_SID_MOVE)
	{
		OnMotionStageMove2SID(m_CurWS);
	}

	if (gmotionLog) gmotionLog->Info("[TomoMotionModel][OnMotionStage]->[Exit][{$}]", stageName.c_str());
}

void TomoMotionModel::DoMotionStageSetTomoMotionSlice(ISensorEncoderController* pEncoder, string encoderControllerName)
{
	int effectivePeriod = 50;

	//1.设置球管旋转方向
	m_motorTubeAngle->SetRotateOrientation(GetTomoRotateDirection());

	//2.设置高度编码器触发同步使能
	bool useHeightTriggerSync = (bool)atoi((const char*)m_MachineryParams["UseHeightTriggerSyncBox"]);
	bool useHeightTriggerAngleRotation = (bool)atoi((const char*)m_MachineryParams["UseHeightTriggerAngleRotation"]);
	if (useHeightTriggerSync)
	{
		auto exposureTriggerID = m_exposure->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
		pEncoder->ActiveExposureTrigger(exposureTriggerID);
	}

	if (useHeightTriggerAngleRotation)
	{
		auto rotateTriggerID = m_motorTubeAngle->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
		pEncoder->ActiveRotateTrigger(rotateTriggerID);
	}

	//3.依次设置曝光高度点位关联同步信号
	if (useHeightTriggerSync)
	{
		int count = m_heightSliceCount - 2;
		if (m_ModelParams.GetFirstOf("TomoNeedExtraTrigger") >= 0 && (int)atoi(m_ModelParams["TomoNeedExtraTrigger"]) != 0)
		{
			count = m_heightSliceCount - 1;
		}

		float speed = strtof((const char*)m_TechnicalParams["TomoMotionSpeed"], nullptr);
		float systemdelayTime = ConfigureExposureDelayTime::GetExposureDelayTimeInSeconds((const char*)m_TechnicalParams["PositionNumber"]);
		int direct = 1;
		if (m_heightSlices[0] < m_heightSlices[1])
		{
			direct = -1;
		}

		float detaHeight = speed * systemdelayTime * direct;

		for (int i = 2; i < count; ++i)
		{
			if (gbusinessLog) gbusinessLog->Info("[TomoMotionModel][SetExposureTrigger]->[{$:f3} {$:f3} {$:f3}]", m_heightSlices[i] + detaHeight, m_heightSlices[i], detaHeight);

			DWORD encoder = m_coordinates->ConvertPhysicsValue(encoderControllerName, m_heightSlices[i] + detaHeight);
			pEncoder->SetExposureTrigger(encoder);
		}
	}

	//4.设置起始曝光点位为触发旋转的点
	if (useHeightTriggerAngleRotation)
	{
		DWORD encoder = m_coordinates->ConvertPhysicsValue(encoderControllerName, m_heightSlices[2]);
		pEncoder->SetRotateTrigger(encoder);
	}

	//5.依次设置曝光高度点位关联的旋转周期和步长
	for (int i = 0; i < m_angleSliceCount; ++i)
	{
		int period = m_anglePeriods[i];
		int step = m_angleSteps[i];
		if (period <= effectivePeriod || period > 0xFFFF)
		{
			TPRINTA_ERROR("Error At Tomo Calculation.idx:%d,Period:%u,Effective:%u", period, effectivePeriod);
			continue;
		}
		m_motorTubeAngle->AppendPWM(step, period);
	}

	//6.设置曝光触发角度编码器值回传
	auto sensortubeHeightID = pEncoder->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
	m_exposure->ActiveExposureTubeHeightPositionAutoNotify(sensortubeHeightID);
	auto sensortubeAngleID = m_encoderTubeAngle->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
	m_exposure->ActiveExposureTubeAnglePositionAutoNotify(sensortubeAngleID);
}

void TomoMotionModel::MotionStageSetTomoMotionSlice()
{
	if (m_CurWS == WS_WALL)
	{
		DoMotionStageSetTomoMotionSlice(m_encoderTubeHeight, CONTROLLER_TUBE_HEIGHT_ENCODER);
	}
	else if (m_CurWS == WS_TABLE)
	{
		DoMotionStageSetTomoMotionSlice(m_encoderTubeHorizontal, CONTROLLER_TUBE_HORIZONTAL_ENCODER);
	}
	//int effectivePeriod = 50;

	////1.设置球管旋转方向
	//m_motorTubeAngle->SetRotateOrientation(GetTomoRotateDirection());

	////2.设置高度编码器触发同步使能
	//bool useHeightTriggerSync = (bool)atoi((const char *)m_MachineryParams["UseHeightTriggerSyncBox"]);
	//bool useHeightTriggerAngleRotation = (bool)atoi((const char *)m_MachineryParams["UseHeightTriggerAngleRotation"]);
	//if (useHeightTriggerSync)
	//{
	//	auto exposureTriggerID = m_exposure->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
	//	m_encoderTubeHeight->ActiveExposureTrigger(exposureTriggerID);
	//}

	//if (useHeightTriggerAngleRotation)
	//{
	//	auto rotateTriggerID = m_motorTubeAngle->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
	//	m_encoderTubeHeight->ActiveRotateTrigger(rotateTriggerID);
	//}

	////3.依次设置曝光高度点位关联同步信号
	//if (useHeightTriggerSync)
	//{
	//	int count = m_heightSliceCount - 2;
	//	if (m_ModelParams.GetFirstOf("TomoNeedExtraTrigger") >= 0 && (int)atoi(m_ModelParams["TomoNeedExtraTrigger"]) != 0)
	//	{
	//		count = m_heightSliceCount - 1;
	//	}


	//	float speed = strtof((const char *)m_TechnicalParams["TomoMotionSpeed"], nullptr);
	//	float systemdelayTime = ConfigureExposureDelayTime::GetExposureDelayTimeInSeconds((const char *)m_TechnicalParams["PositionNumber"]);
	//	int direct = 1;
	//	if (m_heightSlices[0] < m_heightSlices[1])
	//	{
	//		direct = -1;
	//	}

	//	float detaHeight = speed * systemdelayTime * direct;

	//	for (int i = 2; i < count; ++i)
	//	{
	//		if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][SetExposureTrigger]->[{$:f3} {$:f3} {$:f3}]", m_heightSlices[i] + detaHeight, m_heightSlices[i], detaHeight);
	//		
	//		DWORD encoder = m_coordinates->ConvertPhysicsValue(CONTROLLER_TUBE_HEIGHT_ENCODER, m_heightSlices[i] + detaHeight);
	//		m_encoderTubeHeight->SetExposureTrigger(encoder);
	//	}
	//}

	////4.设置起始曝光点位为触发旋转的点
	//if (useHeightTriggerAngleRotation)
	//{
	//	DWORD encoder = m_coordinates->ConvertPhysicsValue(CONTROLLER_TUBE_HEIGHT_ENCODER, m_heightSlices[2]);
	//	m_encoderTubeHeight->SetRotateTrigger(encoder);
	//}

	////5.依次设置曝光高度点位关联的旋转周期和步长
	//for (int i = 0; i < m_angleSliceCount; ++i)
	//{
	//	int period = m_anglePeriods[i];
	//	int step = m_angleSteps[i];
	//	if (period <= effectivePeriod || period > 0xFFFF)
	//	{
	//		TPRINTA_ERROR("Error At Tomo Calculation.idx:%d,Period:%u,Effective:%u", period, effectivePeriod);
	//		continue;
	//	}
	//	m_motorTubeAngle->AppendPWM(step,period);
	//}

	////6.设置曝光触发角度编码器值回传
	//auto sensortubeHeightID = m_encoderTubeHeight->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
	//m_exposure->ActiveExposureTubeHeightPositionAutoNotify(sensortubeHeightID);
	//auto sensortubeAngleID = m_encoderTubeAngle->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
	//m_exposure->ActiveExposureTubeAnglePositionAutoNotify(sensortubeAngleID);
}

BOOL TomoMotionModel::GetTomoRotateDirection()
{
	auto direction = (int)atoi(m_ModelParams["TomoRotateDirection"]);
	return direction != 0;
}

void TomoMotionModel::MotionStageClearParams()
{
	m_FeedbackTubeHeightEncoders.clear();
	m_FeedbackTubeAngleEncoders.clear();
	m_motorTubeAngle->ClearSignal();
	m_encoderTubeHeight->ClearTrigger();

	if (m_encoderTubeHorizontal)
	{
		m_encoderTubeHorizontal->ClearTrigger();
	}
}

void TomoMotionModel::MotionStageCalculateParams()
{
	float detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);

	float sid = strtof((const char*)m_ModelParams["TomoSID"], nullptr);
	if (m_CurWS == WS_WALL)
	{
		if (m_ModelParams.GetFirstOf("TomoSID_Wall") >= 0)
		{
			sid = strtof((const char*)m_ModelParams["TomoSID_Wall"], nullptr);
		}
	}
	else if (m_CurWS == WS_TABLE)
	{
		if (m_ModelParams.GetFirstOf("TomoSID_Table") >= 0)
		{
			sid = strtof((const char*)m_ModelParams["TomoSID_Table"], nullptr);
		}
	}

	float acc = strtof((const char*)m_ModelParams["TomoAcceleratingDistance"], nullptr);
	float lat = strtof((const char*)m_ModelParams["TomoAlignmentDistance"], nullptr);
	float sus = strtof((const char*)m_ModelParams["TomoResetDistance"], nullptr);
	float brk = strtof((const char*)m_ModelParams["TomoDeceleratingDistance"], nullptr);
	float anglePerMot = m_coordinates->GetPhysicalPerMotorPulse(TOMO_TUBE_ANGLE);

	float speed = strtof((const char *)m_TechnicalParams["TomoMotionSpeed"],nullptr);
	int nproject = (int)m_TechnicalParams["TomoProjectionNumber"];
	float scnAngle = strtof((const char *)m_TechnicalParams["TomoScanAngle"],nullptr);
	memset(m_heightSlices, 0, TOMO_HEIGHT_SLICE_MAX);
	memset(m_anglePeriods, 0, TOMO_HEIGHT_SLICE_MAX);
	memset(m_angleSteps, 0, TOMO_HEIGHT_SLICE_MAX);

	string strScanAngle = (const char*)m_TechnicalParams["TomoScanAngle"];
	int nstatus = IsTomoMotionDetectorPosCorrect(detectorHeight, strScanAngle);
	if (nstatus != 0)
	{
		if (gbusinessLog) gbusinessLog->Warn("[TomoMotionModel][MotionStageCalculateParams]->[Current detector position is out of tomo scan range] ]");
		m_coordinates->NotifySystemIsTomoMotionLimitation(nstatus);

		return;
	}
	else
	{
		if (gbusinessLog) gbusinessLog->Info("[TomoMotionModel][MotionStageCalculateParams]->[Current detector position is in tomo scan range] ]");
		m_coordinates->NotifySystemIsTomoMotionLimitation(nstatus);
	}

	if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][MotionStageCalculateParams]->[{$:f3} {$:f3} {$:f3} {$:f3} {$:f3} {$:f3} {$:f3} {$:d} {$:f3} ]",
		sid, acc, lat, sus, brk, anglePerMot, speed, nproject, scnAngle);

	auto err = tomoMotionGeometry(true, detectorHeight, acc, brk, lat, sus, sid, speed,
		nproject, scnAngle, anglePerMot, 1.0, m_warmAngle, m_anglePerSlice,
		m_heightSlices, m_angleSteps, m_anglePeriods, m_angleSliceCount, m_heightSliceCount);

	if (err < 0)
	{
		if (gmotionLog) gmotionLog->Error("[TomoMotionModel][tomoMotionGeometry]->[Error occurs.]");
	}

	m_heightStartPosition = m_heightSlices[0];
	m_angleStartPosition = -(scnAngle / 2.0f);

	FixHeightStartPosition(speed);

	//auto period = (DWORD)atoi((const char *)m_MachineryParams["TubeAnglePeriodP0"]);
	auto period = ConfigurerMotion::GetTubeRotateNormalPeriod();

	DigitalTwinLogger::Instance()->ReceiveTomoScanParam(nproject, scnAngle);
	DigitalTwinLogger::Instance()->ReceiveSID(sid);
	DigitalTwinLogger::Instance()->ReceiveTomoHeightSpeed(speed);
	DigitalTwinLogger::Instance()->ReceiveTomoNormalRotatePeriod(period);
	DigitalTwinLogger::Instance()->ReceiveTomoProjectParams(acc, lat, sus, brk);
	DigitalTwinLogger::Instance()->ReceiveTomoProjectSliceParams(m_heightSlices, m_heightSliceCount);
	DigitalTwinLogger::Instance()->ReceiveTomoSliceRotateSpeeds(m_anglePeriods, m_angleSliceCount);
	DigitalTwinLogger::Instance()->ReceiveTomoSliceRotateSteps(m_angleSteps, m_angleSliceCount);
}

void TomoMotionModel::DoMotionStageMovetoEndPosHeight()
{
	DigitalTwinLogger::Instance()->ReceiveTimePoint();

	auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE);
	auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT);
	auto detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);
	DigitalTwinLogger::Instance()->ReceiveTomoStartPosition(tubeAngleSensorValue, tubeHeightSensorValue);
	DigitalTwinLogger::Instance()->ReceiveDetectorHeight(detectorHeight);

	float endPosition = m_heightSlices[m_heightSliceCount - 1];
	float currentPosition = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);

	int direction = JudgeDirectionInTubeHeightAxis(currentPosition, endPosition);

	MoveTubeHeight(direction, abs(currentPosition - endPosition));
}

void TomoMotionModel::DoMotionStageMovetoEndPosHorizontal()
{
	DigitalTwinLogger::Instance()->ReceiveTimePoint();

	auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE);
	auto tubeHorizontalSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HORIZONTAL);
	auto detectorHorizontal = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HORIZONTAL);
	DigitalTwinLogger::Instance()->ReceiveTomoStartPosition(tubeAngleSensorValue, tubeHorizontalSensorValue);
	DigitalTwinLogger::Instance()->ReceiveDetectorHeight(detectorHorizontal);

	float endPosition = m_heightSlices[m_heightSliceCount - 1];
	//float currentPosition = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HORIZONTAL); 
	float currentPosition = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);

	int direction = JudgeDirectionInTubeHorizontalAxis(currentPosition, endPosition);

	MoveTubeHorizontal(direction, abs(currentPosition - endPosition));
}

void TomoMotionModel::MotionStageMovetoEndPos()
{
	if (m_CurWS == WS_WALL)
	{
		DoMotionStageMovetoEndPosHeight();
	}
	else if (m_CurWS == WS_TABLE)
	{
		DoMotionStageMovetoEndPosHorizontal();
	}

	//DigitalTwinLogger::Instance()->ReceiveTimePoint();

	//auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE);
	//auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT);
	//auto detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);
	//DigitalTwinLogger::Instance()->ReceiveTomoStartPosition(tubeAngleSensorValue, tubeHeightSensorValue);
	//DigitalTwinLogger::Instance()->ReceiveDetectorHeight(detectorHeight);


	//float endPosition = m_heightSlices[m_heightSliceCount - 1];
	//float currentPosition = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);

	//int direction = JudgeDirectionInTubeHeightAxis(currentPosition, endPosition);

	//MoveTubeHeight(direction, abs(currentPosition - endPosition));
}

void TomoMotionModel::DoMotionStageMovetoStartPosHeight()
{
	DigitalTwinLogger::Instance()->ReceiveTimePoint();

	auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE);
	auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT);
	DigitalTwinLogger::Instance()->ReceiveMotionInitialPosition(tubeAngleSensorValue, tubeHeightSensorValue);

	MoveTubeHeightToTomoStartPosiotn();
	RotateTubeAngleToTomoStartPostion();
}

void TomoMotionModel::DoMotionStageMovetoStartPosHorizontal()
{
	DigitalTwinLogger::Instance()->ReceiveTimePoint();

	auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE);
	auto tubeHorizontalSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HORIZONTAL);
	DigitalTwinLogger::Instance()->ReceiveMotionInitialPosition(tubeAngleSensorValue, tubeHorizontalSensorValue);

	MoveTubeHorizontalToTomoStartPosiotn();
	RotateTubeAngleToTomoStartPostion();
}

void TomoMotionModel::MotionStageMovetoStartPos()
{
	if (m_CurWS == WS_WALL)
	{
		DoMotionStageMovetoStartPosHeight();
	}
	else if (m_CurWS == WS_TABLE)
	{
		DoMotionStageMovetoStartPosHorizontal();
	}
	/*DigitalTwinLogger::Instance()->ReceiveTimePoint();

	auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE);
	auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT);
	DigitalTwinLogger::Instance()->ReceiveMotionInitialPosition(tubeAngleSensorValue, tubeHeightSensorValue);

	MoveTubeHeightToTomoStartPosiotn();
	RotateTubeAngleToTomoStartPostion();*/
}

void TomoMotionModel::MoveTubeHeightToTomoStartPosiotn()
{
	float tubeHeight = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);

	int direction = JudgeDirectionInTubeHeightAxis(tubeHeight, m_heightStartPosition);

	float speed = 0.0f;
	if (m_MachineryParams.GetFirstOf("MotionSpeed") > 0)
	{
		speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]);
	}

	MoveTubeHeight(direction, abs(m_heightStartPosition - tubeHeight), speed);
}

void TomoMotionModel::MoveTubeHorizontalToTomoStartPosiotn()
{
	float tubeHorizontal = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HORIZONTAL);

	int direction = JudgeDirectionInTubeHorizontalAxis(tubeHorizontal, m_heightStartPosition);

	float speed = 0.0f;
	if (m_MachineryParams.GetFirstOf("MotionSpeed") > 0)
	{
		speed = (float)atof((const char*)m_MachineryParams["MotionSpeed"]);
	}

	MoveTubeHorizontal(direction, abs(m_heightStartPosition - tubeHorizontal), speed);
}

void TomoMotionModel::RotateTubeAngleToTomoStartPostion()
{
	float tubeAngle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE);

	int direction = JudgeDirectionInTubeAngleAxis(tubeAngle, m_angleStartPosition);

	if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][RotateTubeAngleToTomoStartPostion]->[{$:f6} {$:f6}]", m_angleStartPosition, tubeAngle);

	RotateTubeAngle(direction, abs(m_angleStartPosition - tubeAngle));
}

void TomoMotionModel::MotionStageErrorStop()
{
	StopMotion();
}

void TomoMotionModel::MotionStageErrorRecover()
{
	StopMotion();
}

void TomoMotionModel::StopMotion()
{
	m_motorTubeAngle->StopRotation();
	m_motorTubeHeight->StopMove();
	m_motorTubeHorizontal->StopMove();

	auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE);
	auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT);
	DigitalTwinLogger::Instance()->ReceiveMotionStopPosition(tubeAngleSensorValue, tubeHeightSensorValue);

	DigitalTwinLogger::Instance()->SaveMotionRecord();
}

void TomoMotionModel::MotionStageCenterAdjust()
{
	AlignTubeHeightToDetector();
	RotateTubeToHorizontal();
}

void TomoMotionModel::AlignTubeHeightToDetector()
{
	float detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);
	float tubeHeight = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);

	int direction = JudgeDirectionInTubeHeightAxis(tubeHeight, detectorHeight);

	MoveTubeHeight(direction, abs(detectorHeight - tubeHeight));
}

void TomoMotionModel::RotateTubeToHorizontal()
{
	float tubeAngle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE);

	int direction = JudgeDirectionInTubeAngleAxis(tubeAngle,0.0f);

	RotateTubeAngle(direction, tubeAngle);
}

int TomoMotionModel::JudgeDirectionInTubeHeightAxis(float current, float target)
{
	//auto positive = (int)atoi((const char *)m_MachineryParams["TubeHeightAxisPositiveDirection"]);
	auto positive = ConfigurerMotion::GetTubeHeightAxisPositiveDirection();
	auto direction = positive > 0 ? 1 : -1;

	if (current > target)
	{
		return 1 * direction;
	}

	if (current < target)
	{
		return -1 * direction;
	}

	return 0;
}

int TomoMotionModel::JudgeDirectionInTubeAngleAxis(float current, float target)
{
	//auto positive = (int)atoi((const char *)m_MachineryParams["TubeRotateAxisPositiveDirection"]);
	auto positive = ConfigurerMotion::GetTubeRotateAxisPositiveDirection();
	auto direction = positive > 0 ? 1 : -1;

	if (current > target)
	{
		return -1 * direction;
	}

	if (current < target)
	{
		return 1 * direction;
	}

	return 0;
}

void TomoMotionModel::MoveTubeHeight(int direction, float offset, float spd)
{
	auto tolerance = ConfigurerMotion::GetMotionToleranceLine();
	if (offset < tolerance)
	{
		if (gmotionLog) gmotionLog->Info("[TomoMotionModel][MoveTubeHeight]->[ Offset is {$:f6}, less than {$:f6}m, no motion]", offset, tolerance);

		return;
	} 

	float speed = strtof((const char *)m_TechnicalParams["TomoMotionSpeed"], nullptr);
	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, offset);
	auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_TUBE_HEIGHT, speed);
	if (period < 50 || (period - 50) < 30)
	{
		period = 50;
	}

	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(false);
	}

	m_motorTubeHeight->Move(direction, step, period);
}

void TomoMotionModel::RotateTubeAngle(int direction, float offset)
{
	//auto period = (DWORD)atoi((const char *)m_MachineryParams["TubeAnglePeriodP0"]);
	auto period = ConfigurerMotion::GetTubeRotateNormalPeriod();
	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, offset);
	m_motorTubeAngle->Rotate(direction, step, period);
}

void TomoMotionModel::UpdateDigitalTwinTomoSliceParams()
{
	DigitalTwinLogger::Instance()->ReceiveTomoProjectSliceParams(m_heightSlices, m_heightSliceCount);
}

void TomoMotionModel::FixHeightStartPosition(float speed)
{
	//由于曝光系统存在延迟，因此在设置曝光触发点位时需要考虑系统时延

	if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][FixStartPos]->[S][{$:f6} {$:f6} {$:f6}]", m_heightSlices[0], m_heightSlices[1], m_heightSlices[2]);

	float systemdelayTime = ConfigureExposureDelayTime::GetExposureDelayTimeInSeconds((const char *)m_TechnicalParams["PositionNumber"]);
	int direct = 1;
	if (m_heightSlices[0] < m_heightSlices[1])
	{
		direct = -1;
	}

	float detaHeight = speed * systemdelayTime * direct;
	if (((m_heightSlices[2] + detaHeight) - (m_heightSlices[1])) * direct > 0)
	{
		m_heightSlices[0] = m_heightSlices[0] + detaHeight;
		m_heightSlices[1] = m_heightSlices[1] + detaHeight;

		m_heightStartPosition = m_heightSlices[0];
	}

	if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][FixStartPos]->[E][{$:f6} {$:f6} {$:f6} [{$:f6}]]", m_heightSlices[0], m_heightSlices[1], m_heightSlices[2], detaHeight);
}

void TomoMotionModel::SwitchScanningComponents(int nSwitch)
{

}

void TomoMotionModel::SwitchWorkstation(string ws)
{
	if (gmotionLog) gmotionLog->Info("[TomoMotionModel][SwitchWorkstation]->[{$}]", ws.c_str());
	m_CurWS = ws;
}

void TomoMotionModel::OnMotionStageMove2SID(string ws)
{
	if (ws == WS_WALL)
	{
		//float desPos = atof((const char*)m_MachineryParams["SID_Wall"]);
		float desPos = ConfigurerWS::GetDefaultSIDWall();
		//float tubeHorizontalPos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HORIZONTAL);
		float tubeHorizontalPos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
		int direction = JudgeDirectionInTubeHorizontalAxis(tubeHorizontalPos, desPos);

		if (gmotionLog) gmotionLog->Info("[TomoMotionModel][OnMotionStageMove2SID]->[{$}][Dafault SID:{$:f6}, Cur Pos:{$:f6}]", ws.c_str(), desPos, tubeHorizontalPos);
		MoveTubeHorizontal(direction, abs(tubeHorizontalPos - desPos));
	}
	else if (ws == WS_TABLE)
	{
		//float desPos = atof((const char*)m_MachineryParams["SID_Table"]);
		float desPos = ConfigurerWS::GetDefaultSIDTable();
		float tubeHeightPos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);
		int direction = JudgeDirectionInTubeHeightAxis(tubeHeightPos, desPos);

		if (gmotionLog) gmotionLog->Info("[TomoMotionModel][OnMotionStageMove2SID]->[{$}][Dafault SID:{$:f6}, Cur Pos:{$:f6}]", ws.c_str(), desPos, tubeHeightPos);
		MoveTubeHeight(direction, abs(tubeHeightPos - desPos));
	}
}

int TomoMotionModel::JudgeDirectionInTubeHorizontalAxis(float current, float target)
{
	//auto positive = (int)atoi((const char*)m_MachineryParams["TubeHorizontalAxisPositiveDirection"]);
	auto positive = ConfigurerMotion::GetTubeHorizontalAxisPositiveDirection();
	auto direction = positive > 0 ? 1 : -1;

	if (current > target)
	{
		return 1 * direction;
	}

	if (current < target)
	{
		return -1 * direction;
	}

	return 0;
}

void TomoMotionModel::MoveTubeHorizontal(int direction, float offset, float spd)
{
	auto tolerance = ConfigurerMotion::GetMotionToleranceLine();
	if (offset < tolerance)
	{
		if (gmotionLog) gmotionLog->Info("[TomoMotionModel][MoveTubeHorizontal]->[ Offset is {$:f6}, less than {$:f6}m, no motion]", offset, tolerance);

		return;
	}

	float speed = strtof((const char*)m_TechnicalParams["TomoMotionSpeed"], nullptr);
	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, offset);
	auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_TUBE_HORIZONTAL, speed);
	if (period < 50 || (period - 50) < 30)
	{
		period = 50;
	}

	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(true);
	}

	m_motorTubeHorizontal->Move(direction, step, period);
}

int TomoMotionModel::IsTomoMotionDetectorPosCorrect(float fDetectorCurPos, string scanangle)
{
	float fDetectorMinPos = 0.0;
	float fDetectorMaxPos = 0.0;

	if (gmotionLog) gmotionLog->Info("[TomoMotionModel][IsTomoMotionDetectorPosCorrect]->[ WS is {$}, scan angle is {$}, current detector position is {$:f6}]", m_CurWS, scanangle, fDetectorCurPos);
	if (m_CurWS == WS_WALL)
	{
		fDetectorMinPos = ConfigurerTomoMotionLimitation::GetDetectorMinPositionWall(scanangle);
		fDetectorMaxPos = ConfigurerTomoMotionLimitation::GetDetectorMaxPositionWall(scanangle);
	}
	else if (m_CurWS == WS_TABLE)
	{
		fDetectorMinPos = ConfigurerTomoMotionLimitation::GetDetectorMinPositionTable(scanangle);
		fDetectorMaxPos = ConfigurerTomoMotionLimitation::GetDetectorMaxPositionTable(scanangle);
	}
	
	int nRes = DET_HIGHT_NORMAL;
	if (fDetectorMaxPos > 0.1 && fDetectorMinPos > 0.1 && fDetectorMaxPos > fDetectorMinPos)
	{
		if (fDetectorCurPos > fDetectorMaxPos)
		{
			if (gmotionLog) gmotionLog->Warn("[TomoMotionModel][IsTomoMotionDetectorPosCorrect]->[Detector current pos is out of range, max={$:f6}m, current={$:f6}m, no motion]", fDetectorMaxPos, fDetectorCurPos);
			nRes = DET_HIGHT_TOOHIGH;
		}
		else if (fDetectorCurPos < fDetectorMinPos)
		{
			if (gmotionLog) gmotionLog->Warn("[TomoMotionModel][IsTomoMotionDetectorPosCorrect]->[Detector current pos is out of range, min={$:f6}m, current={$:f6}m, no motion]", fDetectorMinPos, fDetectorCurPos);
			nRes = DET_HIGHT_TOOLOW;
		}
	}
	else
	{
		if (gmotionLog) gmotionLog->Warn("[TomoMotionModel][IsTomoMotionDetectorPosCorrect]->[Detector limitation pos is not correct max={$:f6}m, min={$:f6}m, no motion]", fDetectorMaxPos, fDetectorMinPos);
		nRes = DET_HIGHT_TOOHIGH;
	}

	return nRes;
}