#include "stdafx.h"
#include "ServoDriveJRui.h"

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

const unsigned char SERVO_CMD_WRITE_SINGLE = 0x6;

static unsigned int crc_check(const unsigned char* data, size_t length) 
{
	int j;
	unsigned int reg_crc = 0xFFFF;
	while (length--) 
	{
		reg_crc ^= *data++;
		for (j = 0; j<8; j++) 
		{
			if (reg_crc & 0x01) /*LSB(bit 0 ) = 1 */
			{ 
				reg_crc = (reg_crc >> 1) ^ 0xA001;
			}
			else 
			{
				reg_crc = (reg_crc >> 1);
			}
		}
	}
	return reg_crc;
}


ServoDriveJRui::ServoDriveJRui() :m_driveNumber(-1)
{
}


ServoDriveJRui::~ServoDriveJRui()
{
}

void ServoDriveJRui::Initialize(int driveNumber)
{
	m_driveNumber = driveNumber;
}

bool ServoDriveJRui::MakeServoOnEnablePacket(std::basic_string<unsigned char> &outbuffer)
{
	return false;
}

bool ServoDriveJRui::MakeServoOnPacket(BOOL servoon, std::basic_string<unsigned char> &outbuffer)
{
	//unsigned short context = servoon ? 0x1 : 0x0;
	//return MakeRequest(SERVO_CMD_WRITE_SINGLE, 0x023C, context, outbuffer);
	return false;
}

bool ServoDriveJRui::MakePulseOnCirclePacket(unsigned short pulsenumber, std::basic_string<unsigned char> &outbuffer)
{
	//return MakeRequest(SERVO_CMD_WRITE_SINGLE, 0x015C, pulsenumber, outbuffer);
	return false;
}

bool ServoDriveJRui::MakeClearWarningPacket(std::basic_string<unsigned char> &outbuffer)
{
	return MakeRequest(SERVO_CMD_WRITE_SINGLE, 0x03, 0, outbuffer);
}

bool ServoDriveJRui::MakeRequest(UCHAR Cmd, USHORT Addr, USHORT Context, std::basic_string<unsigned char> &outbuffer)
{
	if (m_driveNumber < 0)
	{
		return false;
	}

	outbuffer.clear();
	unsigned char target = (unsigned char)m_driveNumber;
	outbuffer.push_back(target);
	outbuffer.push_back(Cmd);

	SHORTDATA data;
	data.ShortPart = Addr;
	outbuffer.push_back(data.High);
	outbuffer.push_back(data.Low);

	data.ShortPart = Context;
	outbuffer.push_back(data.High);
	outbuffer.push_back(data.Low);

	USHORT Crc16 = crc_check(outbuffer.data(), outbuffer.length());

	unsigned char tmp[2] = { 0 };
	memcpy(&tmp[0], &Crc16, sizeof(USHORT));

	outbuffer.push_back(tmp[0]);
	outbuffer.push_back(tmp[1]);

	return true;
}