#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#include "circbuf.h"

// --- Circbuf data structure ----

/** Offset in void* buffer */
#define PV_OFFS(pvBuf, elem_size, index) ((uint8_t*)(pvBuf) + ((elem_size)*(index)))


/**
 * @brief Write data to a CircBuf slot
 * @param cb     : circbuf
 * @param index  : slot index
 * @param source : data source
 */
static inline void write_buffer(CircBuf *cb, circbuf_size_t index, const void *source)
{
    memcpy(PV_OFFS(cb->buf, cb->elem_size, index), source, cb->elem_size);
}


/**
 * @brief Copy data from a CircBuf slot to a buffer
 * @param cb     : circbuf
 * @param index  : slot index
 * @param dest   : destination buffer
 */
static inline void read_buffer(const CircBuf *cb, circbuf_size_t index, void *dest)
{
    memcpy(dest, PV_OFFS(cb->buf, cb->elem_size, index), cb->elem_size);
}

/** Get index of the front position (for write) */
static inline circbuf_size_t front_writepos(const CircBuf *cb) {
    return (cb->back + cb->num_used) % cb->cap;
}


void cbuf_init(CircBuf *cb, void *buf, circbuf_size_t capacity, circbuf_size_t elem_size)
{
    // allocate the buffer
    cb->buf = buf;

    // set capacity, clear state
    cb->elem_size = elem_size;
    cb->cap = capacity;

    cbuf_clear(cb);
}


/** Check if cbuf is full */
bool cbuf_full(const CircBuf *cb)
{
    if (cb == NULL) {
        return false;
    }

    return cb->num_used == cb->cap;
}


/** Check if cbuf is empty */
bool cbuf_empty(const CircBuf *cb)
{
    if (cb == NULL) {
        return true;
    }

    return cb->num_used == 0;
}


/** Get the max capacity of the buffer */
circbuf_size_t cbuf_capacity(const CircBuf *cb)
{
    return cb->cap;
}


/** Get the current number of items in the buffer */
circbuf_size_t cbuf_count(const CircBuf *cb)
{
    return cb->num_used;
}


/** Push one element to the front_writepos */
bool cbuf_push(CircBuf *cb, const void *source)
{
    if (cb == NULL || source == NULL || cb->num_used == cb->cap) {
        return false;
    }

    write_buffer(cb, front_writepos(cb), source);

    // increment
    cb->num_used++;

    return true;
}


/** Pop one element from the front_writepos */
bool cbuf_pop(CircBuf *cb, void *dest)
{
    if (cb == NULL || cb->num_used == 0) {
        return false;
    }

    cb->num_used--;

    circbuf_size_t f = front_writepos(cb);

    if (dest != NULL) {
        read_buffer(cb, f, dest);
    }

#ifdef CIRCBUF_ZERO_FREE_SLOTS
    memset(PV_OFFS(cb->buf, cb->elem_size, f), 0, cb->elem_size);
#endif

    return true;
}


/** Peek at the front_writepos element */
bool cbuf_peek(const CircBuf *cb, void *dest)
{
    if (cb == NULL || dest == NULL || cb->num_used == 0) {
        return false;
    }

    circbuf_size_t f = (cb->back + cb->num_used - 1) % cb->cap;
    read_buffer(cb, f, dest);
    return true;
}


void * cbuf_ptr(const CircBuf *cb)
{
    if (cb == NULL || cb->num_used == 0) {
        return NULL;
    }

    circbuf_size_t f = (cb->back + cb->num_used - 1) % cb->cap;
    return PV_OFFS(cb->buf, cb->elem_size, f);
}


/** Peek at the nth element (counted from back) */
bool cbuf_nth(const CircBuf *cb, circbuf_size_t num, void *dest)
{
    if (cb == NULL || dest == NULL || num > cb->num_used) {
        return false;
    }

    circbuf_size_t index = (cb->back + num) % cb->cap;
    read_buffer(cb, index, dest);
    return true;
}


void *cbuf_ptr_nth(const CircBuf *cb, circbuf_size_t num)
{
    if (cb == NULL || num > cb->num_used) {
        return NULL;
    }

    circbuf_size_t index = (cb->back + num) % cb->cap;
    return PV_OFFS(cb->buf, cb->elem_size, index);
}


/** Push one element to the back */
bool cbuf_push_back(CircBuf *cb, const void *source)
{
    if (cb == NULL || source == NULL || cb->num_used == cb->cap) {
        return false;
    }

    // move lr back
    if (cb->back == 0) {
        cb->back = cb->cap - 1; // wrap to the end
    } else {
        cb->back--;
    }
    cb->num_used++;

    write_buffer(cb, cb->back, source);

    return true;
}


/** Pop one element from the back */
bool cbuf_pop_back(CircBuf *cb, void *dest)
{
    if (cb == NULL || cb->num_used == 0) {
        return false;
    }

    if (dest != NULL) {
        read_buffer(cb, cb->back, dest);
    }

#ifdef CIRCBUF_ZERO_FREE_SLOTS
    memset(PV_OFFS(cb->buf, cb->elem_size, cb->back), 0, cb->elem_size);
#endif

    // increment
    cb->back++;
    if (cb->back == cb->cap) {
        cb->back = 0;
    }
    cb->num_used--;

    return true;
}


/** Pop one element from the back */
bool cbuf_peek_back(const CircBuf *cb, void *dest)
{
    if (cb == NULL || dest == NULL || cb->num_used == 0) {
        return false;
    }

    read_buffer(cb, cb->back, dest);
    return true;
}


void* cbuf_ptr_back(const CircBuf *cb)
{
    if (cb == NULL || cb->num_used == 0) {
        return NULL;
    }

    return PV_OFFS(cb->buf, cb->elem_size, cb->back);
}


/** Clear a cbuf */
void cbuf_clear(CircBuf *cb)
{
    if (cb == NULL) {
        return;
    }

    cb->num_used = 0;
    cb->back = 0;

#ifdef CIRCBUF_ZERO_FREE_SLOTS
    memset(PV_OFFS(cb->buf, cb->elem_size, 0), 0, cb->cap * cb->elem_size);
#endif
}