#include "stdafx.h"
#include "CarmImplPositionManager.h"
#include "Converter.h"
#include "IMachineryManager.h"
#include "ISensorEncoderController.h"
#include "PacketAnalizer.h"
#include "CArmComponentNameDef.h"
#include "ConfigurerMotion.h"
#include "DIOS.Dev.Machinery.Driver.ECOM.h"

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

CarmImplPositionManager::CarmImplPositionManager()
:m_isSystemReady(FALSE),
m_isSystemReadySwing(FALSE),
m_isSystemReadyCircular(FALSE),
m_notifyLogicDriver(nullptr),
m_machineryManager(nullptr),
m_encoderCircular(nullptr),
m_encoderSwing(nullptr)
{
}


CarmImplPositionManager::~CarmImplPositionManager()
{
}

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

	m_encoderCircular = (ISensorEncoderController*)(m_machineryManager->Resove(CONTROLLER_CARM_TUBE_CIRCULAR_ENCODER));
	m_encoderSwing = (ISensorEncoderController*)(m_machineryManager->Resove(CONTROLLER_CARM_TUBE_SWING_ENCODER));
}

float CarmImplPositionManager::GetLandmarkPosition(DOF_MECH dof, LANDMARK_TYPE landmarkType)
{
	if(gbusinessLog) gbusinessLog->Error("[GetLandmarkPosition] [DOF [{$:d}] Not implemented]", dof);

	return 0.0f;
}

float CarmImplPositionManager::GetCurrentAbsolutePhysical(DOF_MECH dof)
{
	if(gbusinessLog) gbusinessLog->Error("[GetCurrentAbsolutePhysical] [DOF [{$:d}] Not implemented]", dof);

	return 0.0f;
}

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

	switch (dof)
	{
	case CARM_TUBE_SWING:
	{
							float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
							DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeAngleMotor();
							if (pulseinonecircle == 0)
							{
								ret = 0.0f;
							}

							ret = (angleinonecircle / pulseinonecircle);
	}
		break;
	case CARM_TUBE_CIRCULAR:
	{
							   float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
							   DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeHeightMotor();
							   if (pulseinonecircle == 0)
							   {
								   ret = 0.0f;
							   }

							   ret = (angleinonecircle / pulseinonecircle);
	}
		break;
	default:
		if(gbusinessLog) gbusinessLog->Error("[GetPhysicalPerMotorPulse] [DOF [{$:d}] Not implemented]", dof);
		break;
	}

	return ret;
}

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

	switch (dof)
	{
	case CARM_TUBE_SWING:
	{
							auto sensorvalue = m_encoderSwing->GetCurrentEncoderValue();
							float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
							auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeAngleMotor();
							auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeAngleOrigin();
							ret = Converter::ConvertEncoder(sensorvalue, angleinonecircle, encodervalueinonecircle, absolutevalueAtorigin);

							if (gmotionLog) gmotionLog->Info("[CarmImplPositionManager][GetCurrentPhysical]->[CARM_TUBE_SWING][Sensor:{$:d} DIS:{$:f3} ENC:{$:f3} ORI:{$:f3} Ret:{$:f3}]",
								sensorvalue, angleinonecircle, encodervalueinonecircle, absolutevalueAtorigin, ret);
	}
		break;
	case CARM_TUBE_CIRCULAR:
	{
							   auto sensorvalue = m_encoderCircular->GetCurrentEncoderValue();
							   float distanceinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
							   auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeHeightMotor();
							   auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeHeightOrigin();
							   ret = Converter::ConvertEncoder(sensorvalue, distanceinonecircle, encodervalueinonecircle, absolutevalueAtorigin);

							   if (gmotionLog) gmotionLog->Info("[CarmImplPositionManager][GetCurrentPhysical]->[CIRCULAR][Sensor:{$:d} DIS:{$:f3} ENC:{$:f3} ORI:{$:f3} Ret:{$:f3}]",
								   sensorvalue, distanceinonecircle, encodervalueinonecircle, absolutevalueAtorigin, ret);
	}
		break;
	default:
		if(gbusinessLog) gbusinessLog->Error("[GetCurrentPhysical] [DOF [{$:d}] Not implemented]", dof);
		break;
	}

	return ret;
}

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

	switch (dof)
	{
	case CARM_TUBE_SWING:
		ret = m_encoderSwing->GetCurrentEncoderValue();
		break;
	case CARM_TUBE_CIRCULAR:
		ret = m_encoderCircular->GetCurrentEncoderValue();
		break;
	default:
		if(gbusinessLog) gbusinessLog->Error("[GetCurrentSensorValue] [DOF [{$:d}] Not implemented]", dof);
		break;
	}

	return ret;
}

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

	if (componentName == CONTROLLER_CARM_TUBE_SWING_ENCODER)
	{
		//auto sensorvalue = m_encoderSwing->GetCurrentEncoderValue();
		float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
		auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeAngleMotor();
		auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeAngleOrigin();
		ret = Converter::ConvertEncoder(sensorValue, angleinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
	}
	else if (componentName == CONTROLLER_CARM_TUBE_CIRCULAR_ENCODER)
	{
		//auto sensorvalue = m_encoderCircular->GetCurrentEncoderValue();
		float distanceinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
		auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeHeightMotor();
		auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeHeightOrigin();
		ret = Converter::ConvertEncoder(sensorValue, distanceinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
	}

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

	return ret;
}

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

	if (componentName == CONTROLLER_CARM_TUBE_SWING_ENCODER)
	{
		float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
		auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeAngleMotor();
		auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeAngleOrigin();
		ret = Converter::ConvertPhysicsToEncoder(physicsValue, angleinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
	}
	else if (componentName == CONTROLLER_CARM_TUBE_CIRCULAR_ENCODER)
	{
		float distanceinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
		auto encodervalueinonecircle = ConfigurerMotion::GetEncoderValueInOneCircleOfTubeHeightMotor();
		auto absolutevalueAtorigin = ConfigurerMotion::GetAbsoluteValueAtTubeHeightOrigin();
		ret = Converter::ConvertPhysicsToEncoder(physicsValue, distanceinonecircle, encodervalueinonecircle, absolutevalueAtorigin);
	}

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

	return ret;
}

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

	if (componentName == CONTROLLER_CARM_TUBE_SWING)
	{
		float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
		DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeAngleMotor();
		ret = Converter::ConvertMotorPulse(physicsOffset, angleinonecircle, pulseinonecircle);
	}
	else if (componentName == CONTROLLER_CARM_TUBE_CIRCULAR)
	{
		float heightinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
		DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeHeightMotor();
		ret = Converter::ConvertMotorPulse(physicsOffset, heightinonecircle, pulseinonecircle);
	}

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

	return ret;
}

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

	if (componentName == CONTROLLER_CARM_TUBE_SWING)
	{
		float angleinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeAngleMotor();
		DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeAngleMotor();
		ret = Converter::ConvertSpeed(speed, angleinonecircle, pulseinonecircle);
	}
	else if (componentName == CONTROLLER_CARM_TUBE_CIRCULAR)
	{
		float heightinonecircle = ConfigurerMotion::GetDistanceInOneCircleOfTubeHeightMotor();
		DWORD pulseinonecircle = ConfigurerMotion::GetPulseInOneCircleOfTubeHeightMotor();
		ret = Converter::ConvertSpeed(speed, heightinonecircle, pulseinonecircle);
	}

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

	return ret;
}

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

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

int  CarmImplPositionManager::IsTomoMotionLimitationStatus()
{
	return TRUE;
}

BOOL  CarmImplPositionManager::IsNeedCenterAdjust()
{
	return FALSE;
}

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

void  CarmImplPositionManager::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  CarmImplPositionManager::NotifySystemIsReady()
{
	m_isSystemReady = TRUE;

	if (m_notifyLogicDriver)
	{
		ResDataObject NotifyData;
		ResDataObject tmp;
		tmp.add("Systemready", (bool)m_isSystemReady);
		PacketAnalizer::MakeNotify(NotifyData, PACKET_CMD_UPDATE, NOTIFY_KEY_SYSTEM_READY.c_str(), (const char *)tmp["Systemready"]);

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

void  CarmImplPositionManager::NotifySystemIsTomoMotionLimitation(int nLimitaionStatus)
{

}

void CarmImplPositionManager::ResetSystem(const std::string &componentName)
{
	if (componentName == CONTROLLER_CARM_TUBE_SWING)
	{
		m_isSystemReadySwing = FALSE;
	}
	else if (componentName == CONTROLLER_CARM_TUBE_CIRCULAR)
	{
		m_isSystemReadyCircular = FALSE;
	}
}

BOOL CarmImplPositionManager::IsSystemReady(const std::string &componentName)
{
	BOOL bSysReady = FALSE;

	if (componentName == CONTROLLER_CARM_TUBE_SWING)
	{
		bSysReady = m_isSystemReadySwing;
	}
	else if (componentName == CONTROLLER_CARM_TUBE_CIRCULAR)
	{
		bSysReady = m_isSystemReadyCircular;
	}

	return bSysReady;
}

void CarmImplPositionManager::NotifySystemIsReady(const std::string &componentName)
{
	BOOL bSysReady = FALSE;

	if (componentName == CONTROLLER_CARM_TUBE_SWING)
	{
		m_isSystemReadySwing = TRUE;
		bSysReady = m_isSystemReadySwing;
	}
	else if (componentName == CONTROLLER_CARM_TUBE_CIRCULAR)
	{
		m_isSystemReadyCircular = TRUE;
		bSysReady = m_isSystemReadyCircular;
	}

	if (m_notifyLogicDriver)
	{
		ResDataObject NotifyData;
		ResDataObject tmp;
		tmp.add("Systemready", (bool)bSysReady);
		PacketAnalizer::MakeNotify(NotifyData, PACKET_CMD_UPDATE, NOTIFY_KEY_SYSTEM_READY.c_str(), (const char *)tmp["Systemready"]);

		m_notifyLogicDriver->FireNotify("Systemready", std::to_string(bSysReady));
	}
}