#include <iostream>
#include <cstring>
#include <bitset>

const int EmptyPktSize = 7; //Number of data bytes in a packet with no data field
const int HEADERSIZE = 6; //Number of bytes taken up by the header alone
const int FORWARD = 1;
const int BACKWARD = 2;
const int RIGHT = 3;
const int LEFT = 4;
const int UP = 5;
const int DOWN = 6;
const int OPEN = 7;
const int CLOSE = 8;

enum CmdType { DRIVE, SLEEP, ARM, CLAW, ACK };

struct MotorBody {
	char Direction;
	// no 3 bytes of padding
	// unsigned int Duration : 8;
	char Duration;
};

class PktDef {
	struct Header {
		unsigned int PktCount;
		unsigned Sleep : 1;
		unsigned Status : 1;
		unsigned Drive : 1;
		unsigned Claw : 1;
		unsigned Arm : 1;
		unsigned Ack : 1;
		unsigned Padding : 2;
		unsigned char Length;
	} Head;
	char * Data;
	unsigned char CRC;

	char * RawBuffer;
public:
	PktDef() {
		this->Head.PktCount = 0;
		this->Head.Sleep = 0;
		this->Head.Status = 0;
		this->Head.Drive = 0;
		this->Head.Claw = 0;
		this->Head.Arm = 0;
		this->Head.Ack = 0;
		this->Head.Padding = 0;
		this->Head.Length = (unsigned char)0;
		this->Data = nullptr;
		this->CRC = (unsigned char)0;
		this->RawBuffer = nullptr;
	}

	PktDef(char * rawBuffer) {
		memcpy(&this->Head.PktCount, rawBuffer, 5);
		memcpy(&this->Head.Length, rawBuffer + 5, 1);
		int bodyLength = (int)this->Head.Length - EmptyPktSize;
	//	memcpy(&this->Data, rawBuffer + HEADERSIZE, bodyLength);
		SetBodyData(rawBuffer + HEADERSIZE, bodyLength);
		memcpy(&this->CRC, rawBuffer + HEADERSIZE + bodyLength, sizeof(this->CRC));
		this->RawBuffer = nullptr;
	}

	~PktDef() {
		delete[] Data;
		// deletion of RawBuffer is the responsibility of the caller of GenPacket()
	}

	void SetCmd(CmdType cmdType) {
		// A bit unnecessary to set all the flags each time, but you can never be too careful
		switch (cmdType) {
		case DRIVE:
			this->Head.Sleep = 0;
			this->Head.Drive = 1;
			this->Head.Claw = 0;
			this->Head.Arm = 0;
			break;
		case SLEEP:
			this->Head.Sleep = 1;
			this->Head.Drive = 0;
			this->Head.Claw = 0;
			this->Head.Arm = 0;
			break;
		case ARM:
			this->Head.Sleep = 0;
			this->Head.Drive = 0;
			this->Head.Claw = 0;
			this->Head.Arm = 1;
			break;
		case CLAW:
			this->Head.Sleep = 0;
			this->Head.Drive = 0;
			this->Head.Claw = 1;
			this->Head.Arm = 0;
			break;
		case ACK:
			this->Head.Ack = 1;
			break;
		}
	}

	void SetCmdTest(CmdType cmdType) {
		this->Head.Sleep = 0;
		this->Head.Drive = 0;
		this->Head.Claw = 0;
		this->Head.Arm = 0;
		this->Head.Ack = 0;

		switch (cmdType) {
		case DRIVE:
			this->Head.Drive = 1;
			break;
		case SLEEP:
			this->Head.Sleep = 1;
			break;
		case ARM:
			this->Head.Arm = 1;
			break;
		case CLAW:
			this->Head.Claw = 1;
			break;
		case ACK:
			this->Head.Ack = 1;
			break;
		}
	}

	void SetPktCount(int count) {
		this->Head.PktCount = count;
	}

	CmdType GetCmd() {
		CmdType output = SLEEP;
		if (this->Head.Sleep == 1) { output = SLEEP; }
		else if (this->Head.Drive == 1) { output = DRIVE; }
		else if (this->Head.Claw == 1) { output = CLAW; }
		else if (this->Head.Arm == 1) { output = ARM; }
		else if (this->Head.Ack == 1) { output = ACK; }
		return output;
	}

	void SetBodyData(char * bodyData, int length) {
		this->Data = nullptr;

		if (length == 0)
			return;

		this->Head.Length = (unsigned char)(length + EmptyPktSize);
		this->Data = new char[length];
		memcpy(this->Data, bodyData, length);
	}

	bool GetAck() { return (this->Head.Ack == 1); }

	int GetLength() { return (int)this->Head.Length; }

	char * GetBodyData() { return this->Data; }

	int GetPktCount() { return this->Head.PktCount; }

	bool CheckCRC(char * data, int size) {
		unsigned int ones = 0;
		for (int i = 0; i < size - 1; i++)
			if (data[i] != 0)
				for (int j = 0; j < 8; j++)
					ones += (data[i] >> j) & 1;

		return ones == data[size - 1];
	}

	void CalcCRC() {
		char* data = (char*)this;
		unsigned int ones = 0;
	//	printPacketBits();
		for (int i = 0; i < (unsigned int)this->Head.Length - sizeof(CRC); i++)
			if (data[i] != 0)
				for (int j = 0; j < 8; j++)
					ones += (data[i] >> j) & 1;

		this->CRC = (unsigned char)ones;
	}

	char * GenPacket() {
		this->RawBuffer = new char[this->Head.Length];
		memcpy(this->RawBuffer, &this->Head, HEADERSIZE);
		memcpy(this->RawBuffer + 5, &this->Head.Length, 1);

		if ((int)this->Head.Length > EmptyPktSize)
			memcpy(this->RawBuffer + HEADERSIZE, &this->Data, (int)this->Head.Length - EmptyPktSize);

		memcpy(this->RawBuffer + HEADERSIZE + ((int)this->Head.Length - EmptyPktSize), &this->CRC, sizeof(CRC));
		return RawBuffer;
	}

	void printHeaderContents() {
		// Helper function for testing
		std::cout << "Header Contents" << std::endl;
		std::cout << "----------------------" << std::endl;
		std::cout << "PktCount : " << (int)this->Head.PktCount << std::endl;
		std::cout << "Sleep    : " << (int)this->Head.Sleep << std::endl;
		std::cout << "Status   : " << (int)this->Head.Status << std::endl;
		std::cout << "Drive    : " << (int)this->Head.Drive << std::endl;
		std::cout << "Claw     : " << (int)this->Head.Claw << std::endl;
		std::cout << "Arm      : " << (int)this->Head.Arm << std::endl;
		std::cout << "Ack      : " << (int)this->Head.Ack << std::endl;
		std::cout << "Padding  : " << (int)this->Head.Padding << std::endl;
		std::cout << "Length   : " << (int)this->Head.Length << std::endl;
	}

	void printPacketBits() {
		// Helper function for testing
		std::cout << "Packet Bits" << std::endl;
		std::cout << "----------------------" << std::endl;
		std::cout << "PktCount : " << std::bitset<8>(this->Head.PktCount) << std::endl;
		std::cout << "Sleep    : " << std::bitset<8>(this->Head.Sleep) << std::endl;
		std::cout << "Status   : " << std::bitset<8>(this->Head.Status) << std::endl;
		std::cout << "Drive    : " << std::bitset<8>(this->Head.Drive) << std::endl;
		std::cout << "Claw     : " << std::bitset<8>(this->Head.Claw) << std::endl;
		std::cout << "Arm      : " << std::bitset<8>(this->Head.Arm) << std::endl;
		std::cout << "Ack      : " << std::bitset<8>(this->Head.Ack) << std::endl;
		std::cout << "Padding  : " << std::bitset<8>(this->Head.Padding) << std::endl;
		std::cout << "Length   : " << std::bitset<8>(this->Head.Length) << std::endl;
		std::cout << "Data     : ";
		for (int i = 0; i < (unsigned int)Head.Length - EmptyPktSize; i++)
			std::cout << std::bitset<8>(this->Data[i]) << ' ';
		std::cout << std::endl;
	}
};