#include <cstring>
#include <sys/mman.h>
#include "RingBuffer.h"

// Create a new ringbuffer to hold at least `sz' bytes of data. The
// actual buffer size is rounded up to the next power of two.

RingBuffer::RingBuffer(size_t size)
	: m_Size(size)
{
	size_t	power_of_two;

	for (power_of_two = 1; (1 << power_of_two) < m_Size; power_of_two++);

	m_Size 		= 1 << power_of_two;
	m_SizeMask	= m_Size;
	m_SizeMask	-= 1;
	m_Write		= 0;
	m_Read	 	= 0;
	m_Buffer	=  new uint8_t(m_Size);
	memset(m_Buffer, 0, m_Size);
	m_Locked 	= false;
}

// Free all data associated with the ringbuffer.

RingBuffer::~RingBuffer ()
{
	if (m_Locked) {
		munlock (m_Buffer, m_Size);
	}
	free (m_Buffer);
}

/* Lock the data block of `rb' using the system call 'mlock'.  */

bool RingBuffer::Lock (void)
{
	if (mlock (m_Buffer, m_Size)) {
		m_Locked = false;
	} else {
		m_Locked = true;
	}
	return m_Locked;
}

// Reset the read and write pointers to zero. This is not thread
// safe.

void RingBuffer::Reset()
{
  m_Read = 0;
  m_Write = 0;
}

/* Reset the read and write pointers to zero. This is not thread
   safe. */

/* Return the number of bytes available for reading.  This is the
   number of bytes in front of the read pointer and behind the write
   pointer.  */

size_t RingBuffer::ReadSpace(void)
{
	size_t	w;
	size_t	r;

	w = m_Write;
	r = m_Read;

	if (w > r) {
		return w - r;
	} else {
		return (w - r + m_Size) & (m_SizeMask);
	}
}

/* Return the number of bytes available for writing.  This is the
   number of bytes in front of the write pointer and behind the read
   pointer.  */

size_t RingBuffer::WriteSpace (void)
{
	size_t	w;
	size_t	r;

	w = m_Write;
	r = m_Read;

	if (w > r) {
		return ((r - w + m_Size) & m_SizeMask) - 1;
	} else if (w < r) {
		return (r - w) - 1;
	} else {
		return m_Size - 1;
	}
}

/* The copying data reader.  Copy at most `cnt' bytes from `rb' to
   `dest'.  Returns the actual number of bytes copied. */

size_t RingBuffer::Read(uint8_t *dest, size_t cnt)
{
	size_t free_cnt;
	size_t cnt2;
	size_t to_read;
	size_t n1, n2;

	if ((free_cnt = ReadSpace()) == 0) {
		return 0;
	}

	to_read = cnt > free_cnt ? free_cnt : cnt;

	cnt2 = m_Read + to_read;

	if (cnt2 > m_Size) {
		n1 = m_Size - m_Read;
		n2 = cnt2 & m_SizeMask;
	} else {
		n1 = to_read;
		n2 = 0;
	}

	memcpy (dest, &(m_Buffer[m_Read]), n1);
	m_Read = (m_Read + n1) & m_SizeMask;

	if (n2) {
		memcpy (dest + n1, &(m_Buffer[m_Read]), n2);
		m_Read = (m_Read + n2) & m_SizeMask;
	}

	return to_read;
}

// The copying data reader w/o read pointer advance.  Copy at most
// `cnt' bytes from `rb' to `dest'.  Returns the actual number of bytes
// copied.

size_t RingBuffer::Peek (uint8_t *dest, size_t cnt)
{
	size_t 	free_cnt;
	size_t 	cnt2;
	size_t 	to_read;
	size_t 	n1;
	size_t	n2;
	size_t 	tmp_read_ptr;

	tmp_read_ptr = m_Read;

	if ((free_cnt = ReadSpace ()) == 0) {
		return 0;
	}

	to_read = cnt > free_cnt ? free_cnt : cnt;

	cnt2 = tmp_read_ptr + to_read;

	if (cnt2 > m_Size) {
		n1 = m_Size - tmp_read_ptr;
		n2 = cnt2 & m_SizeMask;
	} else {
		n1 = to_read;
		n2 = 0;
	}

	memcpy (dest, &(m_Buffer[tmp_read_ptr]), n1);
	tmp_read_ptr = (tmp_read_ptr + n1) & m_SizeMask;

	if (n2) {
		memcpy (dest + n1, &(m_Buffer[tmp_read_ptr]), n2);
	}

	return to_read;
}

// The copying data writer.  Copy at most `cnt' bytes to `rb' from
// `src'.  Returns the actual number of bytes copied.

size_t RingBuffer::Write(const uint8_t* src, size_t cnt)
{
	size_t	free_cnt;
	size_t	cnt2;
	size_t	to_write;
	size_t	n1;
	size_t	n2;

	if ((free_cnt = WriteSpace ()) == 0) {
		return 0;
	}

	to_write = cnt > free_cnt ? free_cnt : cnt;

	cnt2 = m_Write + to_write;

	if (cnt2 > m_Size) {
		n1 = m_Size - m_Write;
		n2 = cnt2 & m_SizeMask;
	} else {
		n1 = to_write;
		n2 = 0;
	}

	memcpy (&(m_Buffer[m_Write]), src, n1);
	m_Write = (m_Write + n1) & m_SizeMask;

	if (n2) {
		memcpy (&(m_Buffer[m_Write]), src + n1, n2);
		m_Write = (m_Write + n2) & m_SizeMask;
	}

	return to_write;
}

/* Advance the read pointer `cnt' places. */
void RingBuffer::ReadAdvance(size_t cnt)
{
	size_t tmp = (m_Read + cnt) & m_SizeMask;
	m_Read = tmp;
}

/* Advance the write pointer `cnt' places. */

void RingBuffer::WriteAdvance(size_t cnt)
{
	size_t tmp = (m_Write + cnt) & m_SizeMask;
	m_Write = tmp;
}

/* The non-copying data reader.  `vec' is an array of two places.  Set
   the values at `vec' to hold the current readable data at `rb'.  If
   the readable data is in one segment the second segment has zero
   length.  */

void RingBuffer::ReadVector (buffers_t& buffers)
{
	size_t	free_cnt;
	size_t	cnt2;
	size_t	w;
	size_t	r;

	w = m_Write;
	r = m_Read;

	if (w > r) {
		free_cnt = w - r;
	} else {
		free_cnt = (w - r + m_Size) & m_SizeMask;
	}

	cnt2 = r + free_cnt;

	if (cnt2 > m_Size) {

		// Two part vector: the rest of the buffer after the current write
		// ptr, plus some from the start of the buffer.

		buffers.first_segment_address	= &(m_Buffer[r]);
		buffers.first_segment_length 	= m_Size - r;
		buffers.second_segment_address	= m_Buffer;
		buffers.second_segment_length	= cnt2 & m_SizeMask;

	} else {
		buffers.first_segment_address	= &(m_Buffer[r]);
		buffers.first_segment_length	= free_cnt;
		buffers.second_segment_length 	= 0;
	}
}

void RingBuffer::WriteVector (buffers_t& buffers)
{
	size_t 	free_cnt;
	size_t 	cnt2;
	size_t 	w;
	size_t	r;

	w = m_Write;
	r = m_Read;

	if (w > r) {
		free_cnt = ((r - w + m_Size) & m_SizeMask) - 1;
	} else if (w < r) {
		free_cnt = (r - w) - 1;
	} else {
		free_cnt = m_Size - 1;
	}

	cnt2 = w + free_cnt;

	if (cnt2 > m_Size) {

		// Two part vector: the rest of the buffer after the current write
		//   ptr, plus some from the start of the buffer.

		buffers.first_segment_address	= &(m_Buffer[w]);
		buffers.first_segment_length 	= m_Size - w;
		buffers.second_segment_address 	= m_Buffer;
		buffers.second_segment_length	= cnt2 & m_SizeMask;
	} else {
		buffers.first_segment_address 	= &(m_Buffer[w]);
		buffers.first_segment_length 	= free_cnt;
		buffers.second_segment_length 	= 0;
	}
}