#include "ServoCommand.h"
#include "stdio.h"
#include <string.h>
#include <bitset>
#include "CRC16.h"

MitsubishiCommand::MitsubishiCommand(unsigned char addr)
	:m_addr(addr)
{
}


MitsubishiCommand::~MitsubishiCommand()
{
}


int MitsubishiCommand::GetRequestSequence( unsigned char* buffer)
{
	buffer[0] = 0x01;
	buffer[1] = m_addr;
	buffer[2] = 0x30;
	buffer[3] = 0x32;
	buffer[4] = 0x02;
	buffer[5] = 0x39;
	buffer[6] = 0x31;
	buffer[7] = 0x03;
	unsigned short sum = 0;
	for (int i = 1; i < 8;++i) {
		sum += buffer[i];
	}
	sum = sum % 0x100;
	char sbuffer[10];
	sprintf_s(sbuffer, sizeof(sbuffer), "%02X", sum);
	buffer[8] = sbuffer[0];
	buffer[9] = sbuffer[1];
	return 10;
}

bool MitsubishiCommand::Verify(unsigned char* buffer, int len)
{
	if (len != 14)return false;
	if (buffer[0] != 0x02)return false;
	if (buffer[1] != m_addr )return false;
	if (buffer[2] != 'A')return false;
	if (buffer[len - 3] != 0x03)return false;
	unsigned short sum = 0;
	for (int i = 1; i < len-2; ++i) {
		sum += buffer[i];
	}
	sum = sum % 0x100;
	char sbuffer[10];
	sprintf_s(sbuffer, sizeof(sbuffer), "%02X", sum);
	if (buffer[len - 2] != sbuffer[0]) return false;
	if (buffer[len - 1] != sbuffer[1])return false;
	
	
	unsigned char tempb[8];
	for (int i = 0; i < 8;++i) {
		if (buffer[3 + i]>='0' && buffer[3 + i]<='9') {
			tempb[i] = buffer[3 + i] - '0';
		}
		else if (buffer[3 + i] >= 'A' && buffer[3 + i] <= 'Z') {
			tempb[i] = buffer[3 + i] - 'A'+10;
		}
	}
	MitsubishiValue sv;
	for (int i = 0; i < 4;i++) {
		sv.bvalue[3 - i] = tempb[i * 2] * 16 + tempb[i * 2 + 1];
	}
	m_value = sv.ivalue;
	return true;
}

SanyoCommand::SanyoCommand(unsigned char addr, std::string ctrl, std::string code, std::string group, std::string param)
	:m_addr(addr)
	,m_ctrl(ctrl)
	,m_code(code)
	,m_group(group)
	,m_param(param)
{

}

SanyoCommand::~SanyoCommand() {

}

int SanyoCommand::ConstReturnSize()
{
	return m_ResponseLength;
}

int SanyoCommand::GetRequestSequence(unsigned char* buffer)
{
	buffer[0] = 0x02;
	buffer[1] = m_addr;
	buffer[2] = m_ctrl[0];
	buffer[3] = m_ctrl[1];
	buffer[4] = m_code[0]; 
	buffer[5] = m_code[1];
	buffer[6] = m_group[0];
	buffer[7] = m_group[1];
	buffer[8] = m_param[0];
	buffer[9] = m_param[1];
	buffer[10] = 0x03;

	unsigned char rel = 0;
	for (int i = 1; i < 11; ++i) {
		rel = rel^buffer[i];
	}
	buffer[11] = rel;
	return 12;
}

SanyoReadEncoderPosCommand::SanyoReadEncoderPosCommand(unsigned char addr, std::string ctrl, std::string code,
	std::string group, std::string param)
	:SanyoCommand(addr,ctrl,code,group,param)
{
	m_ResponseLength = 24;
}

SanyoReadEncoderPosCommand::~SanyoReadEncoderPosCommand()
{

}

bool SanyoReadEncoderPosCommand::Verify(unsigned char* buffer, int len)
{
	if (len != m_ResponseLength)return false;
	if (buffer[0] != 0x02)return false;
	if (buffer[1] != m_addr)return false;
	if (buffer[2] != 0x38)return false;
	if (buffer[3] != 0x30)return false;
	if (buffer[4] != m_code[0] || buffer[5] != m_code[1])return false;
	if (buffer[22] != 0x03)return false;
	unsigned char rel = 0;
	for (int i = 1; i < m_ResponseLength-1; ++i) {
		rel = rel^buffer[i];
	}
	if (buffer[m_ResponseLength - 1] != rel)return false;

	unsigned char tempa[16];
	for (int i = 0; i < 16; ++i) {
		if (buffer[6 + i] >= '0' && buffer[6 + i] <= '9') {
			tempa[i] = buffer[6 + i] - '0';
		}
		else if (buffer[6 + i] >= 'A' && buffer[6 + i] <= 'Z') {
			tempa[i] = buffer[6 + i] - 'A' + 10;
		}
	}
	unsigned char tempb[8] = {
		tempa[0] * 16 + tempa[1] ,
		tempa[2] * 16 + tempa[3] ,
		tempa[4] * 16 + tempa[5] ,
		tempa[6] * 16 + tempa[7] ,
		tempa[8] * 16 + tempa[9] ,
		tempa[10] * 16 + tempa[11] ,
		tempa[12] * 16 + tempa[13] ,
		tempa[14] * 16 + tempa[15]
	};
	SanyoValue_Int64 value;
	for (int i = 0; i < 8; i++) {
		value.bvalue[7 - i] = tempb[i];
	}
	int64_t realValue = value.lvalue;
	std::bitset<64> bitValut(realValue);
	std::bitset<16> bitcyc;
	std::bitset<17> bitsin;
	for (int i = 0; i < 16;i++) {
		bitcyc[i] = bitValut[17 + i];
	}
	for (int i = 0; i < 17; i++) {
		bitsin[i] = bitValut[i];
	}
	m_cycles =(short) bitcyc.to_ulong();
	m_singal = (int)bitsin.to_ulong();
	m_value =(long) m_cycles * ABS_CYCLE + m_singal;
	return true;
}


SanyoReadLoadTorqueCommand::SanyoReadLoadTorqueCommand(unsigned char addr, std::string ctrl, std::string code, 
	std::string group, std::string param)
	:SanyoCommand(addr, ctrl, code, group, param)
{
	m_ResponseLength = 12;
}

SanyoReadLoadTorqueCommand::~SanyoReadLoadTorqueCommand()
{

}

bool SanyoReadLoadTorqueCommand::Verify(unsigned char* buffer, int len)
{
	if (len != m_ResponseLength)return false;
	if (buffer[0] != 0x02)return false;
	if (buffer[1] != m_addr)return false;
	if (buffer[2] != 0x38)return false;
	if (buffer[3] != 0x30)return false;
	if (buffer[4] != m_code[0] || buffer[5] != m_code[1])return false;
	if (buffer[10] != 0x03)return false;
	unsigned char rel = 0;
	for (int i = 1; i < m_ResponseLength-1; ++i) {
		rel = rel^buffer[i];
	}
	if (buffer[m_ResponseLength-1] != rel)return false;

	unsigned char tempa[4];
	for (int i = 0; i < 4; ++i) {
		if (buffer[6 + i] >= '0' && buffer[6 + i] <= '9') {
			tempa[i] = buffer[6 + i] - '0';
		}
		else if (buffer[6 + i] >= 'A' && buffer[6 + i] <= 'Z') {
			tempa[i] = buffer[6 + i] - 'A' + 10;
		}
	}
	unsigned char tempb[2] = {
		tempa[0] * 16 + tempa[1] ,
		tempa[2] * 16 + tempa[3] 
	};
	SanyoValue_Short value;
	for (int i = 0; i < 2; i++) {
		value.bvalue[1 - i] = tempb[i];
	}
	m_Value= value.svalue;
	return true;
}

SanyoReadAlarmCommand::SanyoReadAlarmCommand(unsigned char addr, std::string ctrl, std::string code)
	:SanyoCommand(addr, ctrl, code, "  ", "  ")
{
	m_ResponseLength = 10;
}

SanyoReadAlarmCommand::~SanyoReadAlarmCommand()
{

}

bool SanyoReadAlarmCommand::Verify(unsigned char* buffer, int len)
{
	if (len != m_ResponseLength)return false;
	if (buffer[0] != 0x02)return false;
	if (buffer[1] != m_addr)return false;
	if (buffer[2] != 0x38)return false;
	if (buffer[3] != 0x30)return false;
	if (buffer[4] != m_code[0] || buffer[5] != m_code[1])return false;
	if (buffer[8] != 0x03)return false;
	unsigned char rel = 0;
	for (int i = 1; i < m_ResponseLength - 1; ++i) {
		rel = rel^buffer[i];
	}
	if (buffer[m_ResponseLength - 1] != rel)return false;

	unsigned char tempa[2];
	for (int i = 0; i < 2; ++i) {
		if (buffer[6 + i] >= '0' && buffer[6 + i] <= '9') {
			tempa[i] = buffer[6 + i] - '0';
		}
		else if (buffer[6 + i] >= 'A' && buffer[6 + i] <= 'Z') {
			tempa[i] = buffer[6 + i] - 'A' + 10;
		}
	}
	unsigned char tempb = tempa[0] * 16 + tempa[1];
	m_Value = tempb;
	return true;
}

int SanyoReadAlarmCommand::GetRequestSequence(unsigned char* buffer)
{
	buffer[0] = 0x02;
	buffer[1] = m_addr;
	buffer[2] = m_ctrl[0];
	buffer[3] = m_ctrl[1];
	buffer[4] = m_code[0];
	buffer[5] = m_code[1];
	buffer[6] = 0x03;
	unsigned char rel = 0;
	for (int i = 1; i < 7; ++i) {
		rel = rel^buffer[i];
	}
	buffer[7] = rel;
	return 8;
}

SiemensCommand::SiemensCommand(unsigned char addr)
	:m_addr(addr)
{
	
}

SiemensCommand::~SiemensCommand() 
{
}

int SiemensCommand::GetRequestSequence(unsigned char* buffer)
{
	buffer[0] = m_addr;
	buffer[1] = 0x03;
	buffer[2] = 0x01;
	buffer[3] = 0x5F;
	buffer[4] = 0x00;
	buffer[5] = 0x02;

	unsigned short crc = CRC16(buffer, 6);
	buffer[6] = (crc & 0xFF00) >> 8;
	buffer[7] = (crc & 0x00FF);

	return 8;
}

bool SiemensCommand::Verify(unsigned char* buffer, int len)
{
	if (len != 9) return false;
	if (buffer[0] != m_addr) return false;
	if (buffer[1] != 0x03) return false;
	if (buffer[2] != 0x04) return false;

	unsigned short crc = CRC16(buffer, 7);
	if (((((unsigned short)buffer[7]) << 8) + buffer[8]) != crc) return false;

	unsigned char tmp[4];
	tmp[0] = buffer[6];
	tmp[1] = buffer[5];
	tmp[2] = buffer[4];
	tmp[3] = buffer[3];
	m_value = *(long *)tmp;

	return true;
}