#include "stdafx.h"
#include "ImplPositionManager.h"
#include "IMachineryManager.h"
#include "ISensorADController.h"
#include "ISensorEncoderController.h"
#include "ConfigurerMotion.h"
#include "PacketAnalizer.h"
#include "ConfigurerCalibration.h"
#include "DIOS.Dev.Machinery.Driver.ECOM.h"

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

ImplPositionManager::ImplPositionManager()
	:m_isSystemReady(FALSE),
	m_nTomoMotionLimitationStatus(0),
	m_notifyLogicDriver(nullptr),
	m_machineryManager(nullptr),
	m_motorTubeAngle(nullptr),
	m_motorTubeHeight(nullptr),
	m_adTubeHeight(nullptr),
	m_adTubeAngle(nullptr),
	m_adDetectorHeight(nullptr),
	m_encoderTubeAngle(nullptr),
	m_encoderTubeHeight(nullptr),
	m_adTubeHorizontal(nullptr),
	m_encoderTubeHorizontal(nullptr),
	m_adDetectorHorizontal(nullptr)
{
}


ImplPositionManager::~ImplPositionManager()
{
}

void ImplPositionManager::Initialize(IMachineryManager *machmanager)
{
	m_machineryManager = machmanager;

	assert(m_machineryManager);

	m_motorTubeAngle = (ITubeAngleController *)(m_machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
	m_motorTubeHeight = (ITubeHeightController *)(m_machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));
	m_adTubeHeight = (ISensorADController *)(m_machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_AD));
	m_adTubeAngle = (ISensorADController *)(m_machineryManager->Resove(CONTROLLER_TUBE_ANGLE_AD));
	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_adTubeHorizontal = (ISensorADController*)(m_machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL_AD));
	m_adDetectorHorizontal = (ISensorADController*)(m_machineryManager->Resove(CONTROLLER_DETECTOR_HORIZONTAL_AD));
	m_encoderTubeHorizontal = (ISensorEncoderController*)(m_machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL_ENCODER)),

	assert(m_motorTubeAngle);
	assert(m_motorTubeHeight);
	assert(m_adDetectorHeight);
	assert(m_encoderTubeAngle);
	assert(m_encoderTubeHeight);
	assert(m_adTubeHorizontal);
	assert(m_adDetectorHorizontal);
	assert(m_encoderTubeHorizontal);

	DigitalTwinRecordConvertParams();
}

float ImplPositionManager::GetLandmarkPosition(DOF_MECH dof, LANDMARK_TYPE landmarkType)
{
	float ret = 0.0f;

	switch (dof)
	{
		case TOMO_TUBE_ANGLE:
			ret = GetTubeAngleLandmarkPosition(landmarkType);
			break;
		case TOMO_TUBE_HEIGHT:
			ret = GetTubeHeightLandmarkPosition(landmarkType);
			break;
		case TOMO_TUBE_HORIZONTAL:
			ret = GetTubeHorizontalLandmarkPosition(landmarkType);
			break;
		default:
			if(gbusinessLog) gbusinessLog->Error("[GetLandmarkPosition] [DOF [{$:d}] Not implemented]", dof);
			break;
	}
	return ret;
}

float ImplPositionManager::GetCurrentAbsolutePhysical(DOF_MECH dof)
{
	float ret = 0.0f;

	switch (dof)
	{
	case TOMO_TUBE_ANGLE:
		ret = GetCurrentAbsoluteTubeAngle();
		break;
	case TOMO_TUBE_HEIGHT:
		ret = GetCurrentAbsoluteTubeHeight();
		break;
	case TOMO_TUBE_HORIZONTAL:
		ret = GetCurrentAbsoluteTubeHorizontalPos();
		break;
	default:
		if(gbusinessLog) gbusinessLog->Error("[GetCurrentAbsolutePhysical] [DOF [{$:d}] Not implemented]", dof);
		break;
	}

	return ret;
}

float ImplPositionManager::GetPhysicalPerMotorPulse(DOF_MECH dof)
{
	float ret = 0.0f;

	switch (dof)
	{
	case TOMO_TUBE_ANGLE:
		ret = GetTubeAnglePerPulse();
		break;
	default:
		if(gbusinessLog) gbusinessLog->Error("[GetPhysicalPerMotorPulse] [DOF [{$:d}] Not implemented]", dof);
		break;
	}

	return ret;
}

float ImplPositionManager::GetCurrentPhysical(DOF_MECH dof)
{
	float ret = 0.0f;

	switch (dof)
	{
	case TOMO_TUBE_ANGLE:
		ret = GetCurrentTubeAngle();
		break;
	case TOMO_TUBE_HEIGHT:
		ret = GetCurrentTubeHeight();
		break;
	case TOMO_TUBE_HORIZONTAL:
		ret = GetCurrentTubeHorizontalPos();
		break;
	case TOMO_DETECTOR_HEIGHT:
		ret = GetDetectorVerticalPos();
		break;
	case TOMO_DETECTOR_HORIZONTAL:
		ret = GetDetectorHorizontalPos();
		break;
	default:
		if(gbusinessLog) gbusinessLog->Error("[GetCurrentPhysical] [DOF [{$:d}] Not implemented]", dof);
		break;
	}

	return ret;
}

int ImplPositionManager::GetCurrentSensorValue(DOF_MECH dof)
{
	int ret = 0;

	switch (dof)
	{
	case TOMO_TUBE_ANGLE:
		ret = GetCurrentTubeAngleSensorValue();
		break;
	case TOMO_TUBE_HEIGHT:
		ret = GetCurrentEncoderSensorValue(m_encoderTubeHeight);
		break;
	case TOMO_TUBE_HORIZONTAL:
		//ret = GetCurrentEncoderSensorValue(m_encoderTubeHorizontal);
		ret = GetCurrentDetectorADSensorValue(m_adTubeHorizontal);
		break;
	case TOMO_DETECTOR_HEIGHT:
		ret = GetCurrentDetectorADSensorValue(m_adDetectorHeight);
		break;
	case TOMO_DETECTOR_HORIZONTAL:
		ret = GetCurrentDetectorADSensorValue(m_adDetectorHorizontal);
		break;
	default:
		if(gbusinessLog) gbusinessLog->Error("[GetCurrentSensorValue] [DOF [{$:d}] Not implemented]", dof);
		break;
	}

	return ret;
}

void ImplPositionManager::SetNotifyLogicDevice(MachineryECOMDriver* logicdriver)
{
	m_notifyLogicDriver = logicdriver;
}

float ImplPositionManager::ConvertSensorValue(const std::string &componentName, int sensorValue)
{
	float ret = 0.0f;

	if (componentName == CONTROLLER_TUBE_ANGLE_ENCODER)
	{
		ret = DoConvertTubeAngleEncoder(sensorValue);
	}
	else if (componentName == CONTROLLER_TUBE_HEIGHT_ENCODER)
	{
		ret = DoConvertTubeHeightEncoder(sensorValue);
	}
	else if (componentName == CONTROLLER_DETECTOR_HEIGHT_AD)
	{
		ret = DoConvertDetectorHeightAD(sensorValue);
	}
	else if (componentName == CONTROLLER_TUBE_ANGLE_AD)
	{
		ret = DoConvertTubeAngleAD(sensorValue);
	}
	else if (componentName == CONTROLLER_TUBE_HEIGHT_AD)
	{
		ret = DoConvertTubeHeightAD(sensorValue);
	}
	else if (componentName == CONTROLLER_DETECTOR_HORIZONTAL_AD)
	{
		ret = DoConvertDetectorHorizontalAD(sensorValue);
	}
	else if (componentName == CONTROLLER_TUBE_HORIZONTAL_AD)
	{
		ret = DoConvertTubeHorizontalAD(sensorValue);
	}
	else if (componentName == CONTROLLER_TUBE_HORIZONTAL_ENCODER)
	{
		ret = DoConvertTubeHorizontalEncoder(sensorValue);
	}

	if (gmotionLog) gmotionLog->Info("[ImplPositionManager][ConvertSensorValue]->[{$}][{$:d04}][{$:f6}]", componentName.c_str(), sensorValue, ret);

	return ret;
}

int ImplPositionManager::ConvertPhysicsValue(const std::string &componentName, float physicsValue)
{
	DWORD ret = 0;

	if (componentName == CONTROLLER_TUBE_ANGLE_ENCODER)
	{
		ret = DoConvertTubeAngleToEncoder(physicsValue);
	}
	else if (componentName == CONTROLLER_TUBE_HEIGHT_ENCODER)
	{
		ret = DoConvertTubeHeightToEncoder(physicsValue);
	}
	else if (componentName == CONTROLLER_TUBE_HORIZONTAL_ENCODER)
	{
		ret = DoConvertTubeHorizontalToEncoder(physicsValue);
	}
	else if (componentName == CONTROLLER_DETECTOR_HEIGHT_AD)
	{
		ret = DoConvertDetectorToAD(physicsValue);
	}
	else if (componentName == CONTROLLER_DETECTOR_HORIZONTAL_AD)
	{
		ret = DoConvertDetectorHorizontalToAD(physicsValue);
	}

	if (gmotionLog) gmotionLog->Info("[ImplPositionManager][ConvertPhysicsValue]->[{$}][{$:f6}][{$:d04}]", componentName.c_str(), physicsValue, ret);

	return ret;
}

DWORD ImplPositionManager::ConvertMotorStepValue(const std::string &componentName, float physicsOffset)
{
	DWORD ret = 0;

	if (componentName == CONTROLLER_TUBE_ANGLE)
	{
		ret = DoConvertTubeAngleMotorPulse(physicsOffset);
	}
	else if (componentName == CONTROLLER_TUBE_HEIGHT)
	{
		ret = DoConvertTubeHeightMotorPulse(physicsOffset);
	}
	else if (componentName == CONTROLLER_TUBE_HORIZONTAL)
	{
		ret = DoConvertTubeHorizontalMotorPulse(physicsOffset);
	}

	if (gmotionLog) gmotionLog->Info("[ImplPositionManager][ConvertMotorStepValue]->[{$}][{$:f6}][{$:d04}]", componentName.c_str(), physicsOffset, ret);

	return ret;
}

DWORD ImplPositionManager::ConvertMotorSpeed(const std::string &componentName, float speed)
{
	DWORD ret = 0;

	if (componentName == CONTROLLER_TUBE_ANGLE)
	{
		ret = DoConvertTubeAngleMotorSpeed(speed);
	}
	else if (componentName == CONTROLLER_TUBE_HEIGHT)
	{
		ret = DoConvertTubeHeightMotorSpeed(speed);
	}
	else if (componentName == CONTROLLER_TUBE_HORIZONTAL)
	{
		ret = DoConvertTubeHorizontalMotorSpeed(speed);
	}

	if (gmotionLog) gmotionLog->Info("[ImplPositionManager][ConvertMotorSpeed]->[{$}][{$:f6}][{$:d04}]", componentName.c_str(), speed, ret);

	return ret;
}

void ImplPositionManager::ResetSystem()
{
	m_isSystemReady = FALSE;
}

BOOL ImplPositionManager::IsSystemReady()
{
	return m_isSystemReady;
}

int ImplPositionManager::IsTomoMotionLimitationStatus()
{
	return m_nTomoMotionLimitationStatus;
}

BOOL ImplPositionManager::IsNeedCenterAdjust()
{
	auto prisionAngle = 1.0f;		//(°)
	auto prisionHeight = 0.02f;		//(m)
	auto tubeAngle = GetCurrentTubeAngle();
	auto tubeHeight = GetCurrentTubeHeight();
	auto detectorHeight = GetDetectorVerticalPos();

	if (abs(tubeAngle) <= prisionAngle && abs(tubeHeight - detectorHeight) <= prisionHeight)
	{
		return FALSE;
	}

	return TRUE;
}

void ImplPositionManager::NotifyAlignStatus(BOOL isAligned)
{
	if (m_notifyLogicDriver)
	{
		ResDataObject NotifyData;
		ResDataObject tmp;
		tmp.add("Alignstatus", (bool)isAligned);
		PacketAnalizer::MakeNotify(NotifyData, PACKET_CMD_UPDATE, NOTIFY_KEY_ALIGNSTATUS.c_str(), (const char *)tmp["Alignstatus"]);

		m_notifyLogicDriver->FireNotify("Alignstatus", std::to_string(isAligned));
	}
}

void ImplPositionManager::NotifySystemIsReady()
{
	m_isSystemReady = TRUE;
	if (m_notifyLogicDriver)
	{
		ResDataObject NotifyData;
		ResDataObject tmp;
		tmp.add("MachineryReady", (bool)m_isSystemReady);
		PacketAnalizer::MakeNotify(NotifyData, PACKET_CMD_UPDATE, NOTIFY_KEY_SYSTEM_READY.c_str(), (const char *)tmp["MachineryReady"]);

		m_notifyLogicDriver->FireNotify("MachineryReady", std::to_string(m_isSystemReady));
	}
}

void ImplPositionManager::NotifySystemIsTomoMotionLimitation(int nLimitaionStatus)
{
	m_nTomoMotionLimitationStatus = nLimitaionStatus;
	if (m_notifyLogicDriver)
	{
		ResDataObject NotifyData;
		ResDataObject tmp;
		tmp.add("MachineryTomoMotionLimitation", m_nTomoMotionLimitationStatus);
		PacketAnalizer::MakeNotify(NotifyData, PACKET_CMD_UPDATE, NOTIFY_KEY_SYSTEM_READY.c_str(), (const char*)tmp["MachineryTomoMotionLimitation"]);

		m_notifyLogicDriver->FireNotify("MachineryTomoMotionLimitation", std::to_string(m_nTomoMotionLimitationStatus));
	}
}

float ImplPositionManager::GetTubeHeightLandmarkPosition(LANDMARK_TYPE landmarkType)
{
	if (LANDMARK_HIGH == landmarkType)
	{
		return ConfigurerMotion::GetTubeHeightHighLandmarkHeight();
	}
	else if (LANDMARK_LOW == landmarkType)
	{
		//int ad = ConfigurerMotion::GetTubeHeightLowLandmarkAD();
		//return ConvertSensorValue(m_adTubeHeight->Name(), ad);
		return ConfigurerMotion::GetTubeHeightLowLandmarkPos();
	}

	return 0.0f;
}

float ImplPositionManager::GetTubeAngleLandmarkPosition(LANDMARK_TYPE landmarkType)
{
	if (LANDMARK_HIGH == landmarkType)
	{
		return ConfigurerMotion::GetTubeAngleHighLandmarkAngle();
	}
	else if (LANDMARK_LOW == landmarkType)
	{
		return ConfigurerMotion::GetTubeAngleLowLandmarkAngle();
	}

	return 0.0f;
}

float ImplPositionManager::GetTubeAnglePerPulse()
{
	float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
	DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeAngleMotor();
	if (pulseinonecircle == 0)
	{
		return 0.0f;
	}

	return (angleinonecircle / pulseinonecircle);
}

float ImplPositionManager::GetCurrentTubeHeight()
{
	auto heightencoder = m_encoderTubeHeight->GetCurrentEncoderValue();
	return ConvertSensorValue(m_encoderTubeHeight->Name(), heightencoder);
}

float ImplPositionManager::GetCurrentAbsoluteTubeAngle()
{
	auto ad = m_adTubeAngle->GetCurrentADValue();
	return ConvertSensorValue(m_adTubeAngle->Name(), ad);
}

float ImplPositionManager::GetCurrentAbsoluteTubeHeight()
{
	auto ad = m_adTubeHeight->GetCurrentADValue();
	return ConvertSensorValue(m_adTubeHeight->Name(), ad);
}

float ImplPositionManager::GetCurrentTubeAngle()
{
	auto angleencoder = m_encoderTubeAngle->GetCurrentEncoderValue();
	return ConvertSensorValue(m_encoderTubeAngle->Name(), angleencoder);
}

float ImplPositionManager::GetDetectorVerticalPos()
{
	auto detectorad = m_adDetectorHeight->GetCurrentADValue();
	return ConvertSensorValue(m_adDetectorHeight->Name(), detectorad);
}

float ImplPositionManager::GetDetectorHorizontalPos()
{
	auto detectorad = m_adDetectorHorizontal->GetCurrentADValue();
	return ConvertSensorValue(m_adDetectorHorizontal->Name(), detectorad);
}

int ImplPositionManager::GetCurrentTubeAngleSensorValue()
{
	return m_encoderTubeAngle->GetCurrentEncoderValue();
}

int ImplPositionManager::GetCurrentEncoderSensorValue(ISensorEncoderController* pSensorEncoder)
{
	//return m_encoderTubeHeight->GetCurrentEncoderValue();
	if (pSensorEncoder)
	{
		return pSensorEncoder->GetCurrentEncoderValue();
	}

	return -1;
}

int ImplPositionManager::GetCurrentDetectorADSensorValue(ISensorADController* pSensorAD)
{
	//return m_adDetectorHeight->GetCurrentADValue();
	if (pSensorAD)
	{
		return pSensorAD->GetCurrentADValue();
	}

	return -1;
}

float ImplPositionManager::DoConvertTubeAngleEncoder(int sensorvalue)
{
	float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
	auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeAngleMotor();
	auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeAngleOrigin();
	return DoConvertEncoder(sensorvalue, angleinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
}

float ImplPositionManager::DoConvertTubeHeightEncoder(int sensorvalue)
{
	float distanceinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
	auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeHeightMotor();
	auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeHeightOrigin();
	return DoConvertEncoder(sensorvalue, distanceinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
}

int ImplPositionManager::DoConvertTubeAngleToEncoder(float physics)
{
	float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
	auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeAngleMotor();
	auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeAngleOrigin();
	return DoConvertPhysicsToEncoder(physics, angleinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
}

int ImplPositionManager::DoConvertTubeHeightToEncoder(float physics)
{
	float distanceinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
	auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeHeightMotor();
	auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeHeightOrigin();
	return DoConvertPhysicsToEncoder(physics, distanceinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
}

float ImplPositionManager::DoConvertEncoder(int encoder, float physicsinonecircle, float encoderinonecircle, float physicsAtOrigin)
{
	return physicsAtOrigin + (physicsinonecircle / encoderinonecircle) * (encoder);
}

int ImplPositionManager::DoConvertPhysicsToEncoder(float physics, float physicsinonecircle, float encoderinonecircle, float physicsAtOrigin)
{
	return (int)((physics - physicsAtOrigin) / (physicsinonecircle / encoderinonecircle));
}

DWORD ImplPositionManager::DoConvertTubeAngleMotorPulse(float physicsoffset)
{
	float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
	DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeAngleMotor();
	return DoConvertMotorPulse(physicsoffset, angleinonecircle, pulseinonecircle);
}

DWORD ImplPositionManager::DoConvertTubeHeightMotorPulse(float physicsoffset)
{
	float heightinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
	DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeHeightMotor();
	return DoConvertMotorPulse(physicsoffset, heightinonecircle, pulseinonecircle);
}

DWORD ImplPositionManager::DoConvertMotorPulse(float physicsoffset, float physicsinonecircle, DWORD pulseinonecircle)
{
	return (DWORD)(pulseinonecircle * (abs(physicsoffset) / physicsinonecircle));
}

DWORD ImplPositionManager::DoConvertTubeAngleMotorSpeed(float speed)
{
	float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
	DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeAngleMotor();
	return DoConvertSpeed(speed, angleinonecircle, pulseinonecircle);
}

DWORD ImplPositionManager::DoConvertTubeHeightMotorSpeed(float speed)
{
	float heightinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
	DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeHeightMotor();
	return DoConvertSpeed(speed, heightinonecircle, pulseinonecircle);
}

DWORD ImplPositionManager::DoConvertSpeed(float speed, float physicsinonecircle, DWORD pulseinonecircle)
{
	return (DWORD)(1000000 / ((speed / physicsinonecircle) * pulseinonecircle));
}

float ImplPositionManager::DoConvertDetectorHeightAD(int sensorvalue)
{
	auto slope = ConfigurerCalibration::GetDetectorADToHeightSlope();
	auto intercept = ConfigurerCalibration::GetDetectorADToHeightIntercept();

	return slope * sensorvalue + intercept;
}

DWORD ImplPositionManager::DoConvertDetectorToAD(float physics)
{
	auto slope = ConfigurerCalibration::GetDetectorADToHeightSlope();
	auto intercept = ConfigurerCalibration::GetDetectorADToHeightIntercept();
	if (abs(slope) < 1e-3)
	{
		return 0;
	}

	return (DWORD)((physics - intercept) / slope);
}

float ImplPositionManager::DoConvertTubeAngleAD(DWORD sensorvalue)
{
	auto slope = ConfigurerCalibration::GetTubeAngleADToAngleSlope();
	auto intercept = ConfigurerCalibration::GetTubeAngleADToAngleIntercept();

	return slope * sensorvalue + intercept;
}

float ImplPositionManager::DoConvertTubeHeightAD(DWORD sensorvalue)
{
	auto slope = ConfigurerCalibration::GetTubeHeightADToHeightSlope();
	auto intercept = ConfigurerCalibration::GetTubeHeightADToHeightIntercept();

	return slope * sensorvalue + intercept;
}

void ImplPositionManager::DigitalTwinRecordConvertParams()
{
	float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
	auto angleencodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeAngleMotor();
	auto angleabsolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeAngleOrigin();

	float distanceinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
	auto heightencodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeHeightMotor();
	auto heightabsolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeHeightOrigin();

	if (gdigitalTwinLog) gdigitalTwinLog->Info("[CP]->[({$:f6},{$:f6},{$:f6}),({$:f6},{$:f6},{$:f6})]",
		angleabsolutevalueAtorigin,
		angleinonecircle,
		angleencodervalueinonecircle,
		heightabsolutevalueAtorigin,
		distanceinonecircle,
		heightencodervalueinonecircle);
}

void ImplPositionManager::ResetSystem(const std::string& componentName)
{

}

BOOL ImplPositionManager::IsSystemReady(const std::string& componentName)
{
	return TRUE;
}

void ImplPositionManager::NotifySystemIsReady(const std::string& componentName)
{

}

DWORD ImplPositionManager::DoConvertTubeHorizontalMotorPulse(float physicsoffset)
{
	float horizontalinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHorizontalMotor();
	DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeHorizontalMotor();
	return DoConvertMotorPulse(physicsoffset, horizontalinonecircle, pulseinonecircle);
}

DWORD ImplPositionManager::DoConvertTubeHorizontalMotorSpeed(float speed)
{
	float horizontalinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHorizontalMotor();
	DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeHorizontalMotor();
	return DoConvertSpeed(speed, horizontalinonecircle, pulseinonecircle);
}

float ImplPositionManager::DoConvertTubeHorizontalEncoder(int sensorvalue)
{
	float distanceinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHorizontalMotor();
	auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeHorizontalMotor();
	auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeHorizontalOrigin();
	return DoConvertEncoder(sensorvalue, distanceinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
}

float ImplPositionManager::DoConvertTubeHorizontalAD(DWORD sensorvalue)
{
	auto slope = ConfigurerCalibration::GetTubeHorizontalADToPositionSlope();
	auto intercept = ConfigurerCalibration::GetTubeHorizontalADToPositionIntercept();

	return slope * sensorvalue + intercept;
}

int ImplPositionManager::DoConvertTubeHorizontalToEncoder(float physics)
{
	float distanceinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHorizontalMotor();
	auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeHorizontalMotor();
	auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeHorizontalOrigin();
	return DoConvertPhysicsToEncoder(physics, distanceinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
}

float ImplPositionManager::DoConvertDetectorHorizontalAD(DWORD sensorvalue)
{
	auto slope = ConfigurerCalibration::GetDetectorHorizontalADToPositionSlope();
	auto intercept = ConfigurerCalibration::GetDetectorHorizontalADToPositionIntercept();

	return slope * sensorvalue + intercept;
}

DWORD ImplPositionManager::DoConvertDetectorHorizontalToAD(float physics)
{
	auto slope = ConfigurerCalibration::GetDetectorHorizontalADToPositionSlope();
	auto intercept = ConfigurerCalibration::GetDetectorHorizontalADToPositionIntercept();
	if (abs(slope) < 1e-3)
	{
		return 0;
	}

	return (DWORD)((physics - intercept) / slope);
}

float ImplPositionManager::GetTubeHorizontalLandmarkPosition(LANDMARK_TYPE landmarkType)
{
	if (LANDMARK_HIGH == landmarkType)
	{
		return ConfigurerMotion::GetTubeHorizontalRightLandmark();
	}
	else if (LANDMARK_LOW == landmarkType)
	{
		return ConfigurerMotion::GetTubeHorizontalLowLandmarkPos();
	}

	return 0.0f;
}

float ImplPositionManager::GetCurrentAbsoluteTubeHorizontalPos()
{
	auto ad = m_adTubeHorizontal->GetCurrentADValue();
	return ConvertSensorValue(m_adTubeHorizontal->Name(), ad);
}

float ImplPositionManager::GetCurrentTubeHorizontalPos()
{
	auto encoder = m_encoderTubeHorizontal->GetCurrentEncoderValue();
	return ConvertSensorValue(m_encoderTubeHorizontal->Name(), encoder);
}