#include "CComm.h"

#include <cstdint>
#include <cstring>

#include <pthread.h>

CComm::CComm()
	: thread_(), mutex_(), buffer_()
{
	pthread_mutex_init(&mutex_, NULL);
	buffer_.mask = sizeof(buffer_.data) - 1;
}

CComm::~CComm()
{
	pthread_mutex_destroy(&mutex_);
}

size_t CComm::GetReceiveData(uint8_t *buf)
{
	return ReadBuffer(buf);
}

size_t CComm::GetReceiveDataSize()
{
	return (buffer_.write_pos - buffer_.read_pos) & buffer_.mask;
}

void CComm::ClearReceiveBuffer()
{
	buffer_.read_pos = 0;
	buffer_.write_pos = 0;
}

void CComm::WriteBuffer(uint8_t *data, size_t size)
{
	pthread_mutex_lock(&mutex_);

	uint8_t *dst, *src;
	size_t write_size;

	if ((buffer_.write_pos + size) <= sizeof(buffer_.data))
	{
		dst = &buffer_.data[buffer_.write_pos];
		src = data;
		memcpy(dst, src, size);
	}
	else
	{
		dst = &buffer_.data[buffer_.write_pos];
		src = data;
		write_size = sizeof(buffer_.data) - buffer_.write_pos;
		memcpy(dst, src, write_size);

		dst = &buffer_.data[0];
		src = data + write_size;
		write_size = size - write_size;
		memcpy(dst, src, write_size);
	}
	buffer_.write_pos += size;
	buffer_.write_pos &= buffer_.mask;

	pthread_mutex_unlock(&mutex_);
}

size_t CComm::ReadBuffer(uint8_t *buf)
{
	pthread_mutex_lock(&mutex_);

	uint8_t *dst, *src;
	size_t read_pos, size, len;

	read_pos = buffer_.read_pos;
	size = (buffer_.write_pos - read_pos) & buffer_.mask;

	if (size != 0)
	{
		if ((read_pos + size) <= sizeof(buffer_.data))
		{
			dst = buf;
			src = &buffer_.data[read_pos];
			len = size;
			memcpy(dst, src, len);
		}
		else
		{
			dst = buf;
			src = &buffer_.data[read_pos];
			len = sizeof(buffer_.data) - read_pos;
			memcpy(dst, src, len);

			dst = buf + len;
			src = &buffer_.data[0];
			len = size - len;
			memcpy(dst, src, len);
		}

		buffer_.read_pos += size;
		buffer_.read_pos &= buffer_.mask;
	}

	pthread_mutex_unlock(&mutex_);

	return size;
}
