#include <string.h>

#include "malloc.h"
#include "macro.h"
#include "bufferq.h"

int sa_bufferq_init(sa_bufferq_t *q, unsigned nchannels, size_t sample_size) {
    sa_assert(q);
    sa_assert(sample_size > 0);
    sa_assert(nchannels > 0);

    memset(q, 0, sizeof(*q));
    q->sample_size = sample_size;
    q->nchannels = nchannels;

    if (!(q->items = sa_new0(sa_bufferq_item_t*, nchannels)))
        return SA_ERROR_OOM;

    if (!(q->last = sa_new0(sa_bufferq_item_t*, nchannels))) {
        sa_free(q->items);
        q->items = NULL;
        return SA_ERROR_OOM;
    }
    
    return SA_SUCCESS;
}

void sa_bufferq_done(sa_bufferq_t *q) {
    unsigned u;
    sa_assert(q);

    for (u = 0; u < q->nchannels; u++) {
        sa_bufferq_item_t *i;
        
        while ((i = q->items[u])) {
            SA_LLIST_REMOVE(sa_bufferq_item_t, bufferq, q->items[u], i);
            sa_free(i);
        }
    }

    sa_free(q->items);
    sa_free(q->last);
}

static sa_bufferq_item_t* bufferq_item_new(size_t size) {
    sa_bufferq_item_t *i;
    sa_assert(size > 0);
    
    if (!(i = sa_malloc(ALIGN(sizeof(sa_bufferq_item_t)) + size)))
        return i;

    SA_LLIST_ITEM_INIT(sa_bufferq_item_t, bufferq, i);

    return i;
}

int sa_bufferq_push(sa_bufferq_t *q, unsigned channel, const void *data, size_t nbytes, int64_t offset, sa_seek_t whence) {
    int64_t idx;
    sa_bufferq_item_t *i, *j;
    
    sa_assert(q);

    switch (whence) {
        case SA_SEEK_RELATIVE:
            idx = q->write_index + offset;
            break;
        case SA_SEEK_ABSOLUTE:
            idx = offset;
            break;
        case SA_SEEK_RELATIVE_END:
            idx = q->end_index + offset;
            break;
        case _SA_SEEK_MAX:
            sa_assert_not_reached();
    }

    if (q->read_index > idx) {
        int64_t l = q->read_index - idx;

        if (l > nbytes)
            l = nbytes;
        
        idx += l;
        data += l;
        nbytes -= l;
        
    }

    i = q->items[channel];
    
    while (i && nbytes > 0) {

        if (idx >= i->idx + i->size) {
            /* The new data belongs behind the current node */

            i = i->bufferq_next;

        } else if (idx < i->idx) {
            size_t l;
            /* The new data belongs before the current node (at least partially) */

            l = nbytes;

            if (l > i->idx - idx)
                l = i->idx - idx;
            
            if (!(j = bufferq_item_new(l)))
                return SA_ERROR_OOM;

            memcpy(SA_BUFFERQ_ITEM_DATA(j), data, l);
            j->idx = idx;
            j->size = l;

            SA_LLIST_INSERT_BEFORE(sa_bufferq_item_t, bufferq, q->items[channel], i, j);

            idx += l;
            data += l;
            nbytes -= l;
            
        } else {
            size_t l;

            /* The data belongs right in the current node somewhere */

            l = nbytes;

            if (l > i->idx + i->size - idx)
                l = i->idx + i->size - idx;

            memcpy((uint8_t*)  SA_BUFFERQ_ITEM_DATA(i) + (idx - i->idx), data, l);

            idx += l;
            data += l;
            nbytes -= l;
        }
    }

    if (nbytes > 0) {
        sa_assert(!i);

        if (!(j = bufferq_item_new(nbytes)))
            return SA_ERROR_OOM;

        memcpy(SA_BUFFERQ_ITEM_DATA(j), data, nbytes);
        j->idx = idx;
        j->size = nbytes;

        if (q->last[channel])
            SA_LLIST_INSERT_AFTER(sa_bufferq_item_t, bufferq, q->items[channel], q->last[channel], j);
        else
            SA_LLIST_PREPEND(sa_bufferq_item_t, bufferq, q->items[channel], j);
        
        q->last[channel] = j;
    }

    q->write_index = idx + nbytes;

    if (q->write_index > q->end_index)
        q->end_index = q->write_index;

    return SA_SUCCESS;
}

int sa_bufferq_get(sa_bufferq_t *q, void *i[], size_t *bytes) {
    int first = 1;
    unsigned u;
    sa_assert(q);

    *bytes = 0;
    
    for (u = 0; u < q->nchannels; u++) {

        if (q->items[u]) {

            if (q->items[u]->idx > q->read_index) {
                int64_t l;
                i[u] = NULL;

                l = q->items[u]->idx - q->read_index;

                if (first) {
                    *bytes = l;
                    first = 0;
                } else
                    *bytes = l < *bytes ? l : *bytes;
            } else {
                int64_t l;

                i[u] = (uint8_t*) SA_BUFFERQ_ITEM_DATA(q->items[u]) +  q->read_index - q->items[u]->idx;

                l = q->items[u]->size - (q->read_index - q->items[u]->idx);
                
                if (first) {
                    *bytes = l;
                    first = 0;
                } else
                    *bytes = l < *bytes ? l : *bytes;
            }
            
        } else
            i[u] = NULL;
    }

    return SA_SUCCESS;
}


int sa_bufferq_drop(sa_bufferq_t *q, int64_t bytes) {
    unsigned u;
    
    sa_assert(q);

    q->read_index += bytes;

    for (u = 0; u < q->nchannels; u++) {
        
        sa_bufferq_item_t *i;
        
        i = q->items[u];
        
        while (i && q->read_index >= i->idx + i->size) {
            sa_bufferq_item_t *n = i->bufferq_next;

            SA_LLIST_REMOVE(sa_bufferq_item_t, bufferq, q->items[u], i);

            sa_free(i);
            i = n;
        }

        if (!i)
            q->last[u] = NULL;
    }

    return SA_SUCCESS;
}