#include "p_serial_packet.h"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

ps_packet::ps_packet(uint8_t* data)
{}

ps_packet::ps_packet(uint8_t _src_addr, uint8_t _dest_addr,
					 uint8_t* _frame_data, uint8_t _frame_data_len)
{
	data[0] = 0x7E;
	src_addr = &data[1];
	dest_addr = &data[2];
	frame_data_len = &data[3];
	frame_data = &data[4];
	msg_len = 5 + _frame_data_len;
	checksum = nullptr;

	*src_addr = _src_addr;
	*dest_addr = _dest_addr;
	*frame_data_len = _frame_data_len;

	memset(frame_data, 0, MAX_FRAME_DATA_LEN);
	memcpy(frame_data, _frame_data, _frame_data_len);
}
ps_packet::~ps_packet()
{}

// takes members and prepares the buffer for transmission
bool ps_packet::encode()
{
	bool ret = true;
	do
	{

	} while (0);
	return ret;
}

bool ps_packet::decode()
{
	bool ret = true;
	do
	{

	} while (0);
	return ret;
}

bool ps_packet::validate()
{
	bool ret = true;
	do
	{

	} while (0);
	return ret;
}
void ps_packet::calc_checksum()
{}

bool ps_packet::stuff_bytes()
{
	int old_ind = 0;
	int new_ind = 0;
	uint8_t temp_buffer[MAX_FRAME_DATA_LEN];
	memcpy(frame_data, temp_buffer, MAX_FRAME_DATA_LEN);
	msg_len = *frame_data_len + 5;
	for (; old_ind < *frame_data_len; old_ind++)
	{
		if (temp_buffer[old_ind] == 0x7E || temp_buffer[old_ind] == 0x7D)
		{
			msg_len++;
			frame_data[new_ind++] = 0x7D;
			frame_data[new_ind] = temp_buffer[old_ind];
		}
		else
		{
			frame_data[new_ind] = temp_buffer[old_ind];
		}
	}
	if (msg_len > MAX_MESSAGE_LEN)
	{
		return false;
	}
	return true;
}

bool ps_packet::destuff_bytes()
{
	return false;
}

ps_packet ps_packet::from_xy(int32_t x, int32_t y)
{
	uint8_t frame_data[MAX_FRAME_DATA_LEN];
	memset(frame_data, 0, MAX_FRAME_DATA_LEN);
	// op command
	// 0x01 = set xy
	frame_data[0] = 0x01;		   // + 1
	memcpy(&frame_data[1], &x, 4); // + sizeof(x)
	memcpy(&frame_data[5], &y, 4); // + sizeof(y)

	return ps_packet(MASTER_ADDR, 0x02, frame_data, 1 + sizeof(x) + sizeof(y));
}