#include "stdafx.h"
#include "ServoDriveBS.h"

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

const unsigned char RS485_COMMAND_WRITE_ADDRESS = 0x06;

const USHORT crctalbeabs[] = {
	0x0000, 0xCC01, 0xD801, 0x1400, 0xF001, 0x3C00, 0x2800, 0xE401,
	0xA001, 0x6C00, 0x7800, 0xB401, 0x5000, 0x9C01, 0x8801, 0x4400
};

static USHORT crc16tablefast(const unsigned char *ptr, size_t len)
{
	USHORT crc = 0xffff;
	USHORT i;
	UCHAR ch;

	for (i = 0; i < len; i++) {
		ch = *ptr++;
		crc = crctalbeabs[(ch ^ crc) & 15] ^ (crc >> 4);
		crc = crctalbeabs[((ch >> 4) ^ crc) & 15] ^ (crc >> 4);
	}

	return crc;
}


ServoDriveBS::ServoDriveBS() :m_driveNumbre(-1)
{
}


ServoDriveBS::~ServoDriveBS()
{
}

void ServoDriveBS::Initialize(int driveNumber)
{
	m_driveNumbre = driveNumber;
}

bool ServoDriveBS::MakeServoOnEnablePacket(std::basic_string<unsigned char> &outbuffer)
{
	return MakeRequest(RS485_COMMAND_WRITE_ADDRESS, 0x0005, 0x0001, outbuffer);
}

bool ServoDriveBS::MakeServoOnPacket(BOOL servoon, std::basic_string<unsigned char> &outbuffer)
{
	unsigned short servoOn = servoon ? 0x01 : 0x0;
	return MakeRequest(RS485_COMMAND_WRITE_ADDRESS, 0x0004, servoOn, outbuffer);
}

bool ServoDriveBS::MakePulseOnCirclePacket(unsigned short pulsenumber, std::basic_string<unsigned char> &outbuffer)
{
	return MakeRequest(RS485_COMMAND_WRITE_ADDRESS, 0x0155, pulsenumber, outbuffer);
}

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

	outbuffer.clear();
	unsigned char target = (unsigned char)m_driveNumbre;
	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 = crc16tablefast(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;
}