#include "stdafx.h"
#include "SensorEncoderCArmYiDongController.h"
#include "DiosBoardCtrlParams.h"
#include "ICommunicateEntity.h"

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

SensorEncoderCArmYiDongController::SensorEncoderCArmYiDongController() 
:m_communicate(nullptr)
{
}


SensorEncoderCArmYiDongController::~SensorEncoderCArmYiDongController()
{
}

std::string SensorEncoderCArmYiDongController::CLASSID()
{
	return "DA9E644D-CB4D-4E87-BB71-86E8FC4265BA";
}

void SensorEncoderCArmYiDongController::Initialize(const std::string &name)
{
	SetName(name);
}

void SensorEncoderCArmYiDongController::OnCommunicationEstablished(ICommunicateEntity *communicate)
{
	m_communicate = communicate;
}

int  SensorEncoderCArmYiDongController::GetCurrentEncoderValue()
{
	if (/*m_communicateInterfaceID < 0 || */!m_communicate)
	{
		return INVALID_ENCODER_VALUE;
	}

	int retry = 0;
	CAN_PARAM param;

	if (MakeSensorValueReadCanPacket(&param))
	{
		while (retry++ < 5)
		{
			if (m_communicate->CAN_Ctrl(OP_READ, (CAN_ID)m_functionIds[ID_CAN], CAN_ATTR_NONE, param))
			{
				return ParseSensorValueFromCanPacket(&param);
			}
		}
	}

	return INVALID_ENCODER_VALUE;
}

void SensorEncoderCArmYiDongController::LoadAxisInterfaceID(int id_axis_a, int id_axis_b, int id_axis_z)
{

}

void SensorEncoderCArmYiDongController::BindABAxis()
{

}

void SensorEncoderCArmYiDongController::BindZAxis()
{

}

void SensorEncoderCArmYiDongController::UnBindZAxis()
{

}

void SensorEncoderCArmYiDongController::ActiveAutoNotifyWhenZClear(bool active)
{

}

void SensorEncoderCArmYiDongController::ActiveExposureTrigger(int triggerID)
{
	SEC_PARAM param;
	param.associate_id = (unsigned char)triggerID;
	m_communicate->SEC_Ctrl(OP_SET, (SEC_ID)m_communicateInterfaceID, SEC_ATTR_ASSOCIATE_CH1_SET, param);
}

void SensorEncoderCArmYiDongController::ActiveRotateTrigger(int triggerID)
{

}

void SensorEncoderCArmYiDongController::SetExposureTrigger(int encoderValue)
{
	SEC_PARAM param;
	param.associate_value.associate_value_int = encoderValue;
	m_communicate->SEC_Ctrl(OP_SET, (SEC_ID)m_communicateInterfaceID, SEC_ATTR_ASSOCIATE_CH1_TABLE_SET, param);
}

void SensorEncoderCArmYiDongController::SetRotateTrigger(int encoderValue)
{

}

void SensorEncoderCArmYiDongController::ClearTrigger()
{
	SEC_PARAM param;
	m_communicate->SEC_Ctrl(OP_SET, (SEC_ID)m_communicateInterfaceID, SEC_ATTR_ASSOCIATE_CH1_TABLE_CLEAR, param);
	m_communicate->SEC_Ctrl(OP_SET, (SEC_ID)m_communicateInterfaceID, SEC_ATTR_ASSOCIATE_CH2_TABLE_CLEAR, param);
}

bool SensorEncoderCArmYiDongController::MakeSensorValueReadCanPacket(CAN_PARAM *canParm)
{
	canParm->can_dlc = 0x8;
	canParm->can_stdid = (unsigned short)m_canSequence;
	canParm->candata.push_back(0x40);
	canParm->candata.push_back(0x04);
	canParm->candata.push_back(0x60);
	canParm->candata.push_back(0x00);
	canParm->candata.push_back(0x00);
	canParm->candata.push_back(0x00);
	canParm->candata.push_back(0x00);
	canParm->candata.push_back(0x00);
	return true;
}

union _canSensorValue
{
	int encoder_value_int;
	struct _stSensorValue
	{
		unsigned char encoder_lowbyte_0;
		unsigned char encoder_lowbyte_1;
		unsigned char encoder_highbyte_0;
		unsigned char encoder_highbyte_1;
	}encoder_value_byte;

};

int  SensorEncoderCArmYiDongController::ParseSensorValueFromCanPacket(CAN_PARAM *canParm)
{
	int ret = 0;

	if (canParm->candata.size() == 8
		&& canParm->candata[1] == 0x04
		&& canParm->candata[2] == 0x60
		)
	{
		_canSensorValue value;
		value.encoder_value_int = 0;

		value.encoder_value_byte.encoder_lowbyte_0 = canParm->candata[4];
		value.encoder_value_byte.encoder_lowbyte_1 = canParm->candata[5];
		value.encoder_value_byte.encoder_highbyte_0 = canParm->candata[6];
		value.encoder_value_byte.encoder_highbyte_1 = canParm->candata[7];

		ret = value.encoder_value_int;
	}

	return ret;
}