#include <stdlib.h>
#include <string.h>
#include "log.h"
#include "sg.h"


#define SG_FREE_MARKER   ((void*)(uintptr_t)0xDEADBEEFDEADBEEFULL)

struct sgitem {
    struct sgitem *next;
    uint32_t       len;    /* how much useful data we have (not incl offset) */
    uint32_t       offset; /* how much in front to ignore */
    void          *data;   /* null if data follows and need nto be freed */
};

struct sgbuf {
    uint32_t       len;
    struct sgitem *head;
    struct sgitem *tail;
};

#ifdef SG_DEBUG
    void __attribute__((noinline)) sgVerifyFail()
    {
        loge("VERIFY FAIL!!!!\n");
        abort();
    }

    void __attribute__((noinline)) sgVerify(sg s)
    {
        struct sgitem *c, *p = NULL;
        uint32_t sz = 0;

        if (!s) {
            loge("VERIFY: NULL!\n");
            sgVerifyFail();
        }

        if (s->head == SG_FREE_MARKER || s->tail == SG_FREE_MARKER) {
            loge("VERIFY: freed!\n");
            sgVerifyFail();
        }

        for(c = s->head; c; p = c, c = c->next)
            sz += c->len;

        if (s->len != sz)
            loge("VERIFY: %u != %u\n", sz, s->len);
        else if (p != s->tail)
            loge("TAIL: 0x%p last: 0x%p\n", s->tail, p);
        else
            return;

        sgVerifyFail();
    }
#else
    #define sgVerify(s)
#endif

/*
 * FUNCTION: sgitemNewWithCopy
 * USE:      Allocates a new sgitem with a copy of passed data
 * PARAMS:   data - the data
 *           len - length of said data
 * RETURN:   the sgitem or NULL
 * NOTES:
 */
static struct sgitem *sgitemNewWithCopy(const void* data, uint32_t len)
{
    struct sgitem *item = malloc(sizeof(struct sgitem) + len);
    if (!item)
        return NULL;

    memcpy(item + 1, data, len);
    item->next = NULL;
    item->len = len;
    item->offset = 0;
    item->data = NULL;

    return item;
}

/*
 * FUNCTION: sgitemNewWithAllocedBuf
 * USE:      Allocates a new sgitem with a given alloced buffer and takes ownership of it
 * PARAMS:   data - the data
 *           len - length of said data
 * RETURN:   the sgitem or NULL
 * NOTES:
 */
static struct sgitem *sgitemNewWithAllocedBuf(void* data, uint32_t len)
{
    struct sgitem *item = malloc(sizeof(struct sgitem));
    if (!item)
        return NULL;

    item->next = NULL;
    item->len = len;
    item->offset = 0;
    item->data = data;

    return item;
}

/*
 * FUNCTION: sgitemFree
 * USE:      Frees an sgitem
 * PARAMS:   item - the item
 * RETURN:   NONE
 * NOTES:
 */
static void sgitemFree(struct sgitem* item)
{
    if (item->data)
        free(item->data);
    free(item);
}

/*
 * FUNCTION: sgitemGetDataPtr
 * USE:      Gets the data pointer from an sgitem
 * PARAMS:   item - the item
 * RETURN:   item's data
 * NOTES:    returned pointer is the pointer to useful data, not necessarily safe to call free() on
 */
static uint8_t* sgitemGetDataPtr(struct sgitem* item)
{
    uint8_t *buf = (uint8_t*)((item->data) ? item->data : (item + 1));

    return buf + item->offset;
}

/*
 * FUNCTION: sgNew
 * USE:      Allocates a new empty sg
 * PARAMS:   NONE
 * RETURN:   the sg or NULL
 * NOTES:
 */
sg sgNew(void)
{
    return calloc(1, sizeof(struct sgbuf));
}

/*
 * FUNCTION: sgFree
 * USE:      Frees an sg
 * PARAMS:   s - the sg
 * RETURN:   NONE
 * NOTES:
 */
void sgFree(sg s)
{
    struct sgitem *item = s->head, *t;

    sgVerify(s);
    while (item) {
        t = item;
        item = item->next;
        sgitemFree(t);
    }
    s->head = SG_FREE_MARKER;
    s->tail = SG_FREE_MARKER;
    free(s);
}

/*
 * FUNCTION: sgNewWithCopyData
 * USE:      Allocates a new sg with a copy of passed data
 * PARAMS:   data - the data
 *           len - length of said data
 * RETURN:   the sg or NULL
 * NOTES:
 */
sg sgNewWithCopyData(const void* data, uint32_t len)
{
    struct sgitem *item;
    sg s;

    if (!len)
        return sgNew();

    item = sgitemNewWithCopy(data, len);
    s = sgNew();

    if (s && item) {
        s->len = len;
        s->head = item;
        s->tail = item;

        sgVerify(s);
        return s;
    } else if (s)
        sgFree(s);
    else if (item)
        free(item);

    return NULL;
}

/*
 * FUNCTION: sgNewWithAllocedData
 * USE:      Allocates a new sg with a passed data whose owenership we take
 * PARAMS:   data - the data
 *           len - length of said data
 * RETURN:   the sg or NULL
 * NOTES:
 */
sg sgNewWithAllocedData(void *data, uint32_t len)
{
    struct sgitem *item;
    sg s;

    if (!len)
        return sgNew();

    item = sgitemNewWithAllocedBuf(data, len);
    s = sgNew();

    if (s && item) {
        s->len = len;
        s->head = item;
        s->tail = item;

        sgVerify(s);
        return s;
    } else if (s)
        sgFree(s);
    else if (item)
        free(item);

    return NULL;
}

/*
 * FUNCTION: sgLength
 * USE:      Get sg's length
 * PARAMS:   s - the sg
 * RETURN:   the length
 * NOTES:
 */
uint32_t sgLength(sg s)
{
    sgVerify(s);
    return s->len;
}

/*
 * FUNCTION: sgConcat
 * USE:      Concatenate two sg buffers
 * PARAMS:   front - the one to go first
 *           back - the one to go second
 * RETURN:   NONE
 * NOTES:
 */
void sgConcat(sg front, sg back)
{
    sgVerify(front);
    sgVerify(back);

    if (!back->head) {
        sgFree(back);
        return;
    }

    if (front->tail)
        front->tail->next = back->head;
    else
        front->head = back->head;
    front->tail = back->tail;

    front->len += back->len;
    free(back);

    sgVerify(front);
}

/*
 * FUNCTION: sgTruncBack
 * USE:      Truncate from the back (delete rear few bytes)
 * PARAMS:   s - the sg
 *           bytes_to_delete - how many bytes to delete
 * RETURN:   NONE
 * NOTES:    if bytes_to_delete > len, all bytes will be deleted
 */
void sgTruncBack(sg s, uint32_t bytes_to_delete)
{
    struct sgitem *item = s->head, *t = NULL;
    uint32_t bytes_to_keep;

    sgVerify(s);
    if (bytes_to_delete >= s->len)
        bytes_to_keep = 0;
    else if (!bytes_to_delete)
        return;
    else
        bytes_to_keep = s->len - bytes_to_delete;

    /* skip all chunks we're wholly keeping */
    s->len = bytes_to_keep;
    while (item && bytes_to_keep >= item->len) {
        bytes_to_keep -= item->len;
        t = item;
        item = item->next;
    }

    /* split chunk if needed */
    if (bytes_to_keep)  {
        item->len = bytes_to_keep;
        t = item;
        item = item->next;
    }

    /* now item points to first item we want to delete, t is the previous item - adjust pointers appropriately */
    if (t)
        t->next = NULL;
    else
        s->head = NULL;
    s->tail = t;

    while (item) {
        t = item;
        item = item->next;
        sgitemFree(t);
    }
    sgVerify(s);
}

/*
 * FUNCTION: sgTruncFront
 * USE:      Truncate from the front (delete front few bytes)
 * PARAMS:   s - the sg
 *           bytes_to_delete - how many bytes to delete
 * RETURN:   NONE
 * NOTES:    if bytes_to_delete > len, all bytes will be deleted
 */
void sgTruncFront(sg s, uint32_t bytes_to_delete)
{
    struct sgitem *item = s->head, *t;

    sgVerify(s);
    if (bytes_to_delete >= s->len)
        bytes_to_delete = s->len;

    /* delete all the chunks we wholly don't need */
    s->len -= bytes_to_delete;
    while (item && bytes_to_delete >= item->len) {
        bytes_to_delete -= item->len;
        t = item;
        item = item->next;
        sgitemFree(t);
    }
    s->head = item;
    if (!item)
        s->tail = item;

    /* split chunk if needed */
    if (bytes_to_delete) {
        item->offset += bytes_to_delete;
        item->len -= bytes_to_delete;
        item = item->next;
    }
    sgVerify(s);
}

/*
 * FUNCTION: sgConcatFrontCopy
 * USE:      Append a copy of given data to the front of an sg
 * PARAMS:   s - the sg
 *           data - the data
 *           len - length of said data
 * RETURN:   success
 * NOTES:
 */
bool sgConcatFrontCopy(sg s, const void *data, uint32_t len)
{
    struct sgitem *item;

    sgVerify(s);
    if (!len)
        return true;

    item = sgitemNewWithCopy(data, len);

    if (!item)
        return false;

    item->next = s->head;
    s->head = item;
    if (!s->tail)
        s->tail = item;
    s->len += len;

    sgVerify(s);
    return true;
}

/*
 * FUNCTION: sgConcatFrontAlloced
 * USE:      Append a now-owned-by-us buffer to the front of an sg
 * PARAMS:   s - the sg
 *           data - the data
 *           len - length of said data
 * RETURN:   success
 * NOTES:
 */
bool sgConcatFrontAlloced(sg s, void *data, uint32_t len)
{
    struct sgitem *item;

    sgVerify(s);
    if (!len)
        return true;

    item = sgitemNewWithAllocedBuf(data, len);

    if (!item)
        return false;

    item->next = s->head;
    s->head = item;
    if (!s->tail)
        s->tail = item;
    s->len += len;

    sgVerify(s);
    return true;
}

/*
 * FUNCTION: sgConcatBackCopy
 * USE:      Append a copy of given data to the back of an sg
 * PARAMS:   s - the sg
 *           data - the data
 *           len - length of said data
 * RETURN:   success
 * NOTES:
 */
bool sgConcatBackCopy(sg s, const void *data, uint32_t len)
{
    struct sgitem *item;

    sgVerify(s);
    if (!len)
        return true;

    item = sgitemNewWithCopy(data, len);

    if (!item)
        return false;

    if (s->tail)
        s->tail->next = item;
    else
        s->head = item;
    s->tail = item;
    s->len += len;

    sgVerify(s);
    return true;
}

/*
 * FUNCTION: sgConcatBackAlloced
 * USE:      Append a now-owned-by-us buffer to the back of an sg
 * PARAMS:   s - the sg
 *           data - the data
 *           len - length of said data
 * RETURN:   success
 * NOTES:
 */
bool sgConcatBackAlloced(sg s, void *data, uint32_t len)
{
    struct sgitem *item;

    sgVerify(s);
    if (!len)
        return true;

    item = sgitemNewWithAllocedBuf(data, len);

    if (!item)
        return false;

    if (s->tail)
        s->tail->next = item;
    else
        s->head = item;
    s->tail = item;
    s->len += len;

    sgVerify(s);
    return true;
}

/*
 * FUNCTION: sgSplit
 * USE:      Split an sg into two
 * PARAMS:   s - the sg
 *           firstLen - how mnay bytes to leave with the first part
 * RETURN:   the sg containing the second part or NULL on error;
 * NOTES:
 */
sg sgSplit(sg s, uint32_t firstPart)
{
    struct sgitem *i = s->head, *p = NULL, *t;
    uint32_t leftToCut = firstPart;
    sg ret = sgNew();

    sgVerify(s);
    if (!ret)
        return NULL;


    /* first, check for stupid inputs (part 1: cut off all that we have) */
    if (!firstPart) {
        /*
         * Ah, the "cut off everything" case. We simply swap the empty sg we
         * just created with s, and s's internal structure into ret. Dirty?
         * Sure. Clever? Definitely!
         */

        sgSwap(s, ret);
        sgVerify(s);
        sgVerify(ret);
        return ret;
    }

    /* first, check for stupid inputs (part 2: cut off nothing) */
    if (firstPart >= s->len) {
        /* nothing to cut off */
        sgVerify(s);
        sgVerify(ret);
        return ret;
    }

    /* if we got here, we have actual splitting we need to do... */
    ret->len = s->len - firstPart;
    s->len = firstPart;
    while (i && i->len <= leftToCut) {
        leftToCut -= i->len;
        p = i;
        i = i->next;
    }

    if (!leftToCut) { /* exact boundary. nice! */
        ret->tail = s->tail;
        p->next = NULL;
        s->tail = p;

        ret->head = i;
    } else { /* this is the messiest case */
        t = sgitemNewWithCopy(sgitemGetDataPtr(i) + leftToCut, i->len - leftToCut);
        if (!t) {
            sgFree(ret);
            sgVerify(s);
            return NULL;
        }
        i->len = leftToCut;

        ret->tail = s->tail == i ? t : s->tail;
        ret->head = t;
        t->next = i->next;
        i->next = NULL;
        s->tail = i;
    }

    sgVerify(s);
    sgVerify(ret);
    return ret;
}

/*
 * FUNCTION: sgSerialize
 * USE:      Serialize a piece of the sg into a buffer
 * PARAMS:   s - the sg
 *           data - the data
 *           len - length of said data
 * RETURN:   number of bytes produced
 * NOTES:
 */
uint32_t sgSerialize(sg s, uint32_t offset, uint32_t len, void* dstBuf)
{
    uint8_t *dst = (uint8_t*)dstBuf;
    struct sgitem *item = s->head;
    uint32_t origlen = len;

    sgVerify(s);
    /* skip all complete items we do not care about */
    while (item && item->len <= offset) {
        offset -= item->len;
        item = item->next;
    }

    while (item && len) {
        uint8_t *data = sgitemGetDataPtr(item) + offset;
        uint32_t avail = item->len - offset;
        uint32_t want = len;

        offset = 0;
        if (want > avail)
            want = avail;

        memcpy(dst, data, want);
        dst += want;
        len -= want;
        item = item->next;
    }

    return origlen - len;
}

/*
 * FUNCTION: sgSwap
 * USE:      Swap the data in a a and b
 * PARAMS:   a - an sg
 *           b - an sg
 * RETURN:   NONE
 * NOTES:
 */
void sgSwap(sg a, sg b)
{
    struct sgbuf t;

    sgVerify(a);
    sgVerify(b);

    memcpy(&t, a, sizeof(struct sgbuf));
    memcpy(a, b, sizeof(struct sgbuf));
    memcpy(b, &t, sizeof(struct sgbuf));

    sgVerify(a);
    sgVerify(b);
}

/*
 * FUNCTION: sgIterStart
 * USE:      Start an iteration over the pieces of an sg
 * PARAMS:   s - the sg
 * RETURN:   an iterator pointer or NULL if no chunks to iterate over
 * NOTES:
 */
void* sgIterStart(sg s)
{
    return s->head;
}

/*
 * FUNCTION: sgIterCurLen
 * USE:      Get "current" length in an iteration ofer an sg
 * PARAMS:   iter - the iterator (secretly an sgitem*)
 * RETURN:   current length
 * NOTES:
 */
uint32_t sgIterCurLen(void *iter)
{
    struct sgitem *item = (struct sgitem*)iter;

    return item->len;
}

/*
 * FUNCTION: sgIterCurData
 * USE:      Get "current" data in an iteration ofer an sg
 * PARAMS:   iter - the iterator (secretly an sgitem*)
 * RETURN:   current data pointer
 * NOTES:
 */
const void* sgIterCurData(void *iter)
{
    struct sgitem *item = (struct sgitem*)iter;

    return sgitemGetDataPtr(item);
}

/*
 * FUNCTION: sgIterAdvance
 * USE:      Move an sg iterator forward one chunk
 * PARAMS:   iter - the iterator (secretly an sgitem*)
 * RETURN:   new iterator value or NULL if no more left to iterate over
 * NOTES:
 */
void* sgIterAdvance(void *iter)
{
    struct sgitem *item = (struct sgitem*)iter;

    return item->next;
}

/*
 * FUNCTION: sgSerializeCutFront
 * USE:      Serialize first ln bytes of sg into a buffer, then cut them of
 * PARAMS:   s - the SG
 *           buf - destination
 *           len - num bytes to cut
 * RETURN:   true if all went well
 * NOTES:
 */
bool sgSerializeCutFront(sg s, void *buf, uint32_t len)
{
    sgVerify(s);
    if (sgLength(s) < len)
        return false;
    sgSerialize(s, 0, len, buf);
    sgTruncFront(s, len);
    return true;
}

/*
 * FUNCTION: sgDup
 * USE:      Duplicate the data into another SG. current SG is untouched
 * PARAMS:   from - the SG to duplicate
 * RETURN:   new SG or NULL
 * NOTES:
 */
sg sgDup(sg from)
{
    uint32_t len;
    void *retData;
    sg ret;

    sgVerify(from);
    len = sgLength(from);
    retData = malloc(len);
    if (retData) {
        sgSerialize(from, 0, len, retData);
        ret = sgNewWithAllocedData(retData, len);
        if (ret)
            return ret;
        free(retData);
    }
    return NULL;
}