#include "stdafx.h"
#include "ResetMotionModel.h"
#include "IMachineryManager.h"
#include "MotionStages.h"
#include "ResetMotionStageArgs.h"
#include "ITubeAngleController.h"
#include "ITubeHeightController.h"
#include "ISensorADController.h"
#include "ISensorEncoderController.h"
#include "ILandmarkController.h"
#include "IPositionManager.h"
#include "IOInterfaceMapper.h"
#include "ConfigurerMotion.h"
#include "TubeLineMotionSwitchController.h"
#include "ConfigurerWS.h"

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

ResetMotionModel::ResetMotionModel()
	:m_StageName(""),
	m_stageArgs(new ResetMotionStageArgs()),
	m_coordinates(nullptr),
	m_motorTubeAngle(nullptr),
	m_motorTubeHeight(nullptr),
	m_adDetectorHeight(nullptr),
	m_encoderTubeAngle(nullptr),
	m_encoderTubeHeight(nullptr),
	m_landmark(nullptr),
	m_resetRotatePeriod(250),
	m_resetHeightPeriod(250),
	m_tubeRotateLandmarkDirection(0),
	m_tubeHeightLandmarkDirection(0),
	m_tubeRotateResetAngle(0.0f),
	m_tubeHeightAxisPositiveDirection(1),
	m_tubeRotateAxisPositiveDirection(1),
	m_tubeHeightLowPositionofRest(0.0),
	m_motorTubeHorizontal(nullptr),
	m_adTubeHorizontal(nullptr),
	m_encoderTubeHorizontal(nullptr),
	m_tubeLineMotionSwitch(nullptr),
	m_tubeHorizontalLandmarkDirection(1),
	m_tubeHorizontalAxisPositiveDirection(1),
	m_resetHorizontalPeriod(500)
{
}

ResetMotionModel::~ResetMotionModel()
{
	if (m_stageArgs)
	{
		delete m_stageArgs;
		m_stageArgs = nullptr;
	}
}

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

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

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

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

	if (name == RESET_STAGE_CLEAE_PARAMS)
	{
		OnStageClearParams();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_FIND_LANDMARK)
	{
		OnStageTubeAngleFindLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_FIND_LANDMARK)
	{
		OnStageTubeHeightFindLandmark();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_MOVE_TO_LANDMARK)
	{
		OnStageTubeAngleMoveToLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_MOVE_TO_LANDMARK)
	{
		OnStageTubeHeightMoveToLandmark();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_STOP_AT_LANDMARK)
	{
		OnStageTubeAngleStopAtLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_STOP_AT_LANDMARK)
	{
		OnStageTubeHeightStopAtLandmark();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_ATTACH_ZAXIS)
	{
		OnStageTubeAngleAttachZAxis();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_ATTACH_ZAXIS)
	{
		OnStageTubeHeightAttachZAxis();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_DETACH_ZAXIS)
	{
		OnStageTubeAngleDetachZAxis();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_DETACH_ZAXIS)
	{
		OnStageTubeHeightDetachZAxis();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_FIND_Z_AXIS)
	{
		OnStageTubeAngleFindZAxis();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_FIND_Z_AXIS)
	{
		OnStageTubeHeightFindZAxis();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_STOP_AT_ZAXIS)
	{
		OnStageTubeAngleStopAtZAxis();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_STOP_AT_ZAXIS)
	{
		OnStageTubeHeightStopAtZxis();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_FINISH_RESET)
	{
		OnStageTubeAngleFinishReset();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_FINISH_RESET)
	{
		OnStageTubeHeightFinishReset();
	}
	else if (name == RESET_STAGE_STOP_MOVE)
	{
		OnStageStopMove();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_GO_OUT_OF_LOW_LANDMARK)
	{
		OnStageTubeAngleGoOutOfLowLandmark();
	}
	else if (name == RESET_STAGE_TUBE_ANGLE_GO_OUT_OF_HIGH_LANDMARK)
	{
		OnStageTubeAngleGoOutOfHighLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_GO_OUT_OF_LOW_LANDMARK)
	{
		OnStageTubeHeightGoOutOfLowLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HEIGHT_GO_OUT_OF_HIGH_LANDMARK)
	{
		OnStageTubeHeightGoOutOfHighLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_FIND_LANDMARK)
	{
		OnStageTubeHorizontalFindLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_MOVE_TO_LANDMARK)
	{
		OnStageTubeHorizontalMoveToLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_STOP_AT_LANDMARK)
	{
		OnStageTubeHorizontalStopAtLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_ATTACH_ZAXIS)
	{
		OnStageTubeHorizontalAttachZAxis();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_DETACH_ZAXIS)
	{
		OnStageTubeHorizontalDetachZAxis();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_FIND_Z_AXIS)
	{
		OnStageTubeHorizontalFindZAxis();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_STOP_AT_ZAXIS)
	{
		OnStageTubeHorizontalStopAtZxis();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_FINISH_RESET)
	{
		OnStageTubeHorizontalFinishReset();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_GO_OUT_OF_LEFT_LANDMARK)
	{
		OnStageTubeHorizontalGoOutOfLeftLandmark();
	}
	else if (name == RESET_STAGE_TUBE_HORIZONTAL_GO_OUT_OF_RIGHT_LANDMARK)
	{
		OnStageTubeHorizontalGoOutOfRightLandmark();
	}
	else if (name == RESET_STAGE_ADJUST_SID_MOVE)
	{
		OnMotionStageMove2SID();
	}

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

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

	m_motorTubeAngle = (ITubeAngleController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
	m_motorTubeHeight = (ITubeHeightController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));
	m_adDetectorHeight = (ISensorADController *)(machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_AD));
	m_encoderTubeAngle = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE_ENCODER));
	m_encoderTubeHeight = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_ENCODER));
	m_landmark = (ILandmarkController *)(machineryManager->Resove(CONTROLLER_LANDMARK));

	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_motorTubeHorizontal);
	assert(m_encoderTubeHorizontal);
	assert(m_tubeLineMotionSwitch);
}

void ResetMotionModel::LoadMachineryParams(ResDataObject &params)
{
	m_resetRotatePeriod = (DWORD)ConfigurerMotion::GetTubeRotateResetPeriod();
	m_tubeRotateResetAngle = ConfigurerMotion::GetTubeRotateResetAngle();
	m_tubeRotateAxisPositiveDirection = ConfigurerMotion::GetTubeRotateAxisPositiveDirection();
	m_tubeRotateLandmarkDirection = ConfigurerMotion::GetTubeRotateLandmarkDirection();

	m_resetHeightPeriod = (DWORD)ConfigurerMotion::GetTubeHeightResetPeriod();
	m_tubeHeightLandmarkDirection = ConfigurerMotion::GetTubeHeightLandmarkDirection();
	m_tubeHeightAxisPositiveDirection = ConfigurerMotion::GetTubeHeightAxisPositiveDirection();

	m_resetHorizontalPeriod = (DWORD)ConfigurerMotion::GetTubeHorizontalResetPeriod();
	m_tubeHorizontalLandmarkDirection = ConfigurerMotion::GetTubeHorizontalLandmarkDirection();
	m_tubeHorizontalAxisPositiveDirection = ConfigurerMotion::GetTubeHorizontalAxisPositiveDirection();

	m_tubeHeightLowPositionofRest = ConfigurerMotion::GetTubeHeightLowPositionofReset();
}

void ResetMotionModel::LoadModelParams(ResDataObject &params)
{

}

void ResetMotionModel::SetTechnicalParams(ResDataObject &params)
{

}

void ResetMotionModel::OnFeedbackMotionParams(ResDataObject &params)
{

}

void ResetMotionModel::OnStageTubeAngleFindZAxis()
{
	int direction = ComputeTubeAngleZToLandmarkRotateDirection();
	m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
}

void ResetMotionModel::OnStageTubeHeightFindZAxis()
{
	int direction = ComputeTubeHeightZToLandmarkMoveDirection();
	MoveTubeHeight(direction, -1, m_resetHeightPeriod);
}

BOOL ResetMotionModel::GetMotionParams(ResDataObject &params)
{
	return TRUE;
}

void ResetMotionModel::OnStageClearParams()
{
	m_motorTubeAngle->ClearSignal();
	m_encoderTubeHeight->ClearTrigger();
}

void ResetMotionModel::OnStageTubeAngleFindLandmark()
{
	if (IsTubeAngleAtResetLandmark())
	{
		m_stageArgs->IsTubeAngleAtResetLandmark = TRUE;
		return;
	}

	int direction = JudgeTubeRotateLandmarkDirection();
	m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
}

void ResetMotionModel::OnStageTubeHeightFindLandmark()
{
	if (IsTubeHeightAtResetLandmark())
	{
		m_stageArgs->IsTubeHeightAtResetLandmark = TRUE;
		return;
	}

	int direction = JudgeTubeHeightLandmarkDirection();
	MoveTubeHeight(direction, -1, m_resetHeightPeriod);
}

void ResetMotionModel::OnStageTubeAngleStopAtLandmark()
{
	m_motorTubeAngle->StopRotation();
}

void ResetMotionModel::OnStageTubeHeightStopAtLandmark()
{
	m_motorTubeHeight->StopMove();
}

void ResetMotionModel::OnStageTubeAngleGoOutOfLowLandmark()
{
	int direction = ComputeTubeAngleZToLandmarkRotateDirection() * (-1);
	m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
}

void ResetMotionModel::OnStageTubeAngleGoOutOfHighLandmark()
{
	int direction = ComputeTubeAngleZToLandmarkRotateDirection();
	m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
}

void ResetMotionModel::OnStageTubeHeightGoOutOfLowLandmark()
{
	int direction = ComputeTubeHeightZToLandmarkMoveDirection();
	MoveTubeHeight(direction, -1, m_resetHeightPeriod);
}

void ResetMotionModel::OnStageTubeHeightGoOutOfHighLandmark()
{
	int direction = ComputeTubeHeightZToLandmarkMoveDirection();
	MoveTubeHeight(direction, -1, m_resetHeightPeriod);
}

void ResetMotionModel::OnStageTubeAngleAttachZAxis()
{
	m_encoderTubeAngle->BindABAxis();
	m_encoderTubeAngle->BindZAxis();
	m_encoderTubeAngle->ActiveAutoNotifyWhenZClear(true);
}

void ResetMotionModel::OnStageTubeHeightAttachZAxis()
{
	m_encoderTubeHeight->BindABAxis();
	m_encoderTubeHeight->BindZAxis();
	m_encoderTubeHeight->ActiveAutoNotifyWhenZClear(true);
}

void ResetMotionModel::OnStageTubeAngleDetachZAxis()
{
	m_motorTubeAngle->ClearSignal();
	m_encoderTubeAngle->UnBindZAxis();
	m_encoderTubeAngle->ActiveAutoNotifyWhenZClear(false);
}

void ResetMotionModel::OnStageTubeHeightDetachZAxis()
{
	m_encoderTubeHeight->UnBindZAxis();
	m_encoderTubeHeight->ActiveAutoNotifyWhenZClear(false);
}

void ResetMotionModel::OnStageTubeAngleFinishReset()
{
	auto resetAngle = GetTubeRotateResetPosition();

	string ws = ConfigurerWS::GetDefaultWS();

	if (ws == WS_WALL)
	{
		MoveTubeAngle(resetAngle);
	}
	else if (ws == WS_TABLE)
	{
		MoveTubeAngle(-90.0);
	}
}

void ResetMotionModel::OnStageTubeHeightFinishReset()
{
	auto detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);
	if ((m_tubeHeightLowPositionofRest > 0.5 && m_tubeHeightLowPositionofRest < 1.8) && detectorHeight < m_tubeHeightLowPositionofRest)
	{
		if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHeightFinishReset]->[detector height is lower {$:f6} than lowest position of tube height config {$:f6}, using tube lowest height]", detectorHeight, m_tubeHeightLowPositionofRest);

		detectorHeight = m_tubeHeightLowPositionofRest;
	}
	auto tubeheight = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);
	int direction = JudgeHeightDirection(tubeheight, detectorHeight);

	auto absHeight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);

	if(gbusinessLog) gbusinessLog->Info("[ResetMotionModel][OnStageTubeHeightFinishReset]->[{$:f6} {$:f6} {$:f6}]", absHeight, detectorHeight, tubeheight);

	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(tubeheight - detectorHeight));
	MoveTubeHeight(direction, steps, m_resetHeightPeriod);
}

void ResetMotionModel::OnStageTubeAngleStopAtZAxis()
{
	m_motorTubeAngle->StopRotation();
}

void ResetMotionModel::OnStageTubeHeightStopAtZxis()
{
	m_motorTubeHeight->StopMove();
}

void ResetMotionModel::OnStageStopMove()
{
	m_motorTubeAngle->StopRotation();
	m_motorTubeHeight->StopMove();
}

void ResetMotionModel::OnStageTubeAngleMoveToLandmark()
{
	auto currentangle = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_ANGLE);
	auto targetangle = m_coordinates->GetLandmarkPosition(TOMO_TUBE_ANGLE,LANDMARK_LOW);
	int direction = JudgeRotateDirection(currentangle, targetangle);
	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, abs(currentangle - targetangle));
	m_motorTubeAngle->Rotate(direction, steps, m_resetRotatePeriod);
}

void ResetMotionModel::OnStageTubeHeightMoveToLandmark()
{
	auto currentheight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
	auto targetheight = m_coordinates->GetLandmarkPosition(TOMO_TUBE_HEIGHT, LANDMARK_LOW);
	
	if (m_tubeHeightLowPositionofRest > 0.5 && m_tubeHeightLowPositionofRest < 1.8 && targetheight < m_tubeHeightLowPositionofRest)
	{
		if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHeightMoveToLandmark]->[target height is lower {$:f6} than lowest position of tube height config {$:f6}, using tube lowest height]", targetheight, m_tubeHeightLowPositionofRest);

		targetheight = m_tubeHeightLowPositionofRest;
	}
	int direction = JudgeHeightDirection(currentheight, targetheight);
	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(currentheight - targetheight));

	if (gbusinessLog) gbusinessLog->Info("[ResetMotionModel][OnStageTubeHeightMoveToLandmark]->[landmark height is {$:f6}, current tube height is {$:f6}]", targetheight, currentheight);

	MoveTubeHeight(direction, steps, m_resetHeightPeriod);
}

int ResetMotionModel::JudgeTubeRotateLandmarkDirection()
{
	int direction = m_tubeRotateAxisPositiveDirection > 0 ? 1 : -1;

	return m_tubeRotateLandmarkDirection > 0 ? direction : (-1 * direction);
}

int ResetMotionModel::JudgeTubeHeightLandmarkDirection()
{
	int direction = m_tubeHeightAxisPositiveDirection > 0 ? 1 : -1;

	return m_tubeHeightLandmarkDirection > 0 ? (-1 * direction) : direction;
}

float ResetMotionModel::GetTubeRotateResetPosition()
{
	return m_tubeRotateResetAngle;
}

int ResetMotionModel::JudgeRotateDirection(float currentAngle, float targetAngle)
{
	int direction = m_tubeRotateAxisPositiveDirection > 0 ? 1 : -1;

	if (currentAngle > targetAngle)
	{
		return -1 * direction;
	}

	if (currentAngle < targetAngle)
	{
		return 1 * direction;
	}

	return 0;
}

int ResetMotionModel::JudgeHeightDirection(float currentHeight, float targetHeight)
{
	int direction = m_tubeHeightAxisPositiveDirection > 0 ? 1 : -1;

	if (currentHeight < targetHeight)
	{
		return -1 * direction;
	}

	if (currentHeight > targetHeight)
	{
		return 1 * direction;
	}

	return 0;
}

int ResetMotionModel::ComputeTubeAngleZToLandmarkRotateDirection()
{
	return -1 * JudgeTubeRotateLandmarkDirection();
}

int ResetMotionModel::ComputeTubeHeightZToLandmarkMoveDirection()
{
	return -1 * JudgeTubeHeightLandmarkDirection();
}

BOOL ResetMotionModel::IsTubeAngleAtResetLandmark()
{
	BOOL ret = FALSE;

	if (m_tubeRotateLandmarkDirection > 0)
	{
		auto highstatus = m_landmark->ReadTubeAngleLowLandmarkStatus();
		ret = (highstatus == 1);
		if (ret)
		{
			m_stageArgs->ActivedTubeAngleLandmark = 1;
		}
	}
	else
	{
		auto lowstatus = m_landmark->ReadTubeAngleHighLandmarkStatus();
		ret = (lowstatus == 1);
		if (ret)
		{
			m_stageArgs->ActivedTubeAngleLandmark = 0;
		}
	}

	return ret;
}

BOOL ResetMotionModel::IsTubeHeightAtResetLandmark()
{
	BOOL ret = FALSE;

	if (m_tubeHeightLandmarkDirection > 0)
	{
		auto highstatus = m_landmark->ReadTubeHeightHighLandmarkStatus();
		ret = (highstatus == 1);
		if (ret)
		{
			m_stageArgs->ActivedTubeHeightLandmark = 1;
		}
	}
	else
	{
		auto lowstatus = m_landmark->ReadTubeHeightLowLandmarkStatus();
		ret = (lowstatus == 1);
		if (ret)
		{
			m_stageArgs->ActivedTubeHeightLandmark = 0;
		}
	}

	return ret;
}

void ResetMotionModel::SwitchScanningComponents(int nSwitch)
{

}

void ResetMotionModel::SwitchWorkstation(string ws)
{
	m_CurWS = ws;
}

void ResetMotionModel::OnStageTubeHorizontalMoveToLandmark()
{
	auto tubehight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
	if (tubehight < m_tubeHeightLowPositionofRest)
	{
		if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHorizontalMoveToLandmark]->[tube height is lower {$:f6} than lowest position of tube height config {$:f6}, no moving]", tubehight, m_tubeHeightLowPositionofRest);

		return;
	}

	auto currenthorizontalpos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
	auto targetpos = m_coordinates->GetLandmarkPosition(TOMO_TUBE_HORIZONTAL, LANDMARK_LOW);

	int direction = JudgeHorizontalDirection(currenthorizontalpos, targetpos);
	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, abs(currenthorizontalpos - targetpos));
	MoveTubeHorizontal(direction, steps, m_resetHorizontalPeriod);
}

void ResetMotionModel::OnStageTubeHorizontalFindLandmark()
{
	if (IsTubeHorizontalAtResetLandmark())
	{
		m_stageArgs->IsTubeHorizontalAtResetLandmark = TRUE;
		return;
	}

	int direction = JudgeTubeHorizontalLandmarkDirection();
	MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
}

void ResetMotionModel::OnStageTubeHorizontalStopAtLandmark()
{
	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(true);
	}

	if (m_motorTubeHorizontal)
	{
		m_motorTubeHorizontal->StopMove();
	}
}

void ResetMotionModel::OnStageTubeHorizontalGoOutOfLeftLandmark()
{
	int direction = ComputeTubeHorizontalZToLandmarkMoveDirection();
	MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
}

void ResetMotionModel::OnStageTubeHorizontalGoOutOfRightLandmark()
{
	int direction = ComputeTubeHorizontalZToLandmarkMoveDirection();
	MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
}

void ResetMotionModel::OnStageTubeHorizontalAttachZAxis()
{
	if (m_encoderTubeHorizontal)
	{
		m_encoderTubeHorizontal->BindABAxis();
		m_encoderTubeHorizontal->BindZAxis();
		m_encoderTubeHorizontal->ActiveAutoNotifyWhenZClear(true);
	}
}

void ResetMotionModel::OnStageTubeHorizontalDetachZAxis()
{
	if (m_encoderTubeHorizontal)
	{
		m_encoderTubeHorizontal->UnBindZAxis();
		m_encoderTubeHorizontal->ActiveAutoNotifyWhenZClear(false);
	}
}

void ResetMotionModel::OnStageTubeHorizontalFindZAxis()
{
	int direction = ComputeTubeHorizontalZToLandmarkMoveDirection();
	MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
}

void ResetMotionModel::OnStageTubeHorizontalStopAtZxis()
{
	if (m_motorTubeHorizontal)
	{
		m_motorTubeHorizontal->StopMove();
	}
}

void ResetMotionModel::OnStageTubeHorizontalFinishReset()
{
	auto tubehight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
	if (tubehight < m_tubeHeightLowPositionofRest)
	{
		if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHorizontalMoveToLandmark]->[tube height is lower {$:f6} than lowest position of tube height config {$:f6}, no moving]", tubehight, m_tubeHeightLowPositionofRest);

		return;
	}

	auto detectorhorizontal = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HORIZONTAL);

	//auto tubehorizontalpos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HORIZONTAL);
	auto tubehorizontalpos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);

	auto abshorizontalpos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);

	int direction = JudgeHorizontalDirection(abshorizontalpos, detectorhorizontal);

	if (gbusinessLog) gbusinessLog->Info("[ResetMotionModel][OnStageTubeHorizontalFinishReset]->[{$:f6} {$:f6} {$:f6}]", abshorizontalpos, detectorhorizontal, tubehorizontalpos);

	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, abs(abshorizontalpos - detectorhorizontal));

	MoveTubeHorizontal(direction, steps, m_resetHorizontalPeriod);
}

int ResetMotionModel::JudgeTubeHorizontalLandmarkDirection()
{
	int direction = m_tubeHorizontalAxisPositiveDirection > 0 ? 1 : -1;

	return m_tubeHorizontalLandmarkDirection > 0 ? (-1 * direction) : direction;
}
int ResetMotionModel::JudgeHorizontalDirection(float currentHorizontalPos, float targetHorizontalPos)
{
	int direction = m_tubeHorizontalAxisPositiveDirection > 0 ? 1 : -1;

	if (currentHorizontalPos < targetHorizontalPos)
	{
		return -1 * direction;
	}

	if (currentHorizontalPos > targetHorizontalPos)
	{
		return 1 * direction;
	}

	return 0;
}
int ResetMotionModel::ComputeTubeHorizontalZToLandmarkMoveDirection()
{
	return -1 * JudgeTubeHorizontalLandmarkDirection();
}

BOOL ResetMotionModel::IsTubeHorizontalAtResetLandmark()
{
	BOOL ret = FALSE;

	if (m_tubeHorizontalLandmarkDirection > 0)
	{
		auto rightstatus = m_landmark->ReadTubeHorizontalRightLandmarkStatus();
		ret = (rightstatus == 1);
		if (ret)
		{
			m_stageArgs->ActivedTubeHorizontalLandmark = 1;
		}
	}
	else
	{
		auto leftstatus = m_landmark->ReadTubeHorizontalLeftLandmarkStatus();
		ret = (leftstatus == 1);
		if (ret)
		{
			m_stageArgs->ActivedTubeHorizontalLandmark = 0;
		}
	}

	return ret;
}

void ResetMotionModel::MoveTubeHorizontal(int direction, int steps, int pwmperiod)
{
	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(true);
	}

	if (m_motorTubeHorizontal)
	{
		m_motorTubeHorizontal->Move(direction, steps, pwmperiod);
	}
}

void ResetMotionModel::MoveTubeHeight(int direction, int steps, int pwmperiod)
{
	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(false);
	}

	if (m_motorTubeHeight)
	{
		m_motorTubeHeight->Move(direction, steps, pwmperiod);
	}
}

void ResetMotionModel::MoveTubeAngle(float despos)
{
	auto tubeangle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE);
	int direction = JudgeRotateDirection(tubeangle, despos);

	auto absAngle = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_ANGLE);

	if (gbusinessLog) gbusinessLog->Info("[ResetMotionModel][MoveTubeAngle]->[{$:f6} {$:f6} {$:f6}]", absAngle, despos, tubeangle);

	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, abs(tubeangle - despos));
	m_motorTubeAngle->Rotate(direction, steps, m_resetRotatePeriod);
}

void ResetMotionModel::OnMotionStageMove2SID()
{
	string ws = ConfigurerWS::GetDefaultWS();

	if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStageMove2SID]->[{$}]", ws.c_str());

	if (ws == WS_WALL)
	{
		float desPos = ConfigurerWS::GetDefaultSIDWall();
		float tubeHorizontalPos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
		int direction = JudgeHorizontalDirection(tubeHorizontalPos, desPos);

		if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStageMove2SID]->[MoveTubeHorizontal Des pos {$:f6}, Cur pos {$:f6}]", desPos, tubeHorizontalPos);
		
		auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, abs(tubeHorizontalPos - desPos));
		MoveTubeHorizontal(direction, steps, m_resetHorizontalPeriod);
	}
	else if (ws == WS_TABLE)
	{
		float desPos = ConfigurerWS::GetDefaultSIDTable();
		float tubeHeightPos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);
		int direction = JudgeHeightDirection(tubeHeightPos, desPos);

		if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStageMove2SID]->[MoveTubeHeight Des pos {$:f6}, Cur pos {$:f6}]", desPos, tubeHeightPos);

		auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(tubeHeightPos - desPos));
		MoveTubeHeight(direction, steps, m_resetHeightPeriod);

		//MoveTubeAngle(-90.0);
	}
}