/***
  This file is part of libsydney.

  Copyright 2007-2008 Lennart Poettering

  libsydney is free software; you can redistribute it and/or modify it
  under the terms of the GNU Lesser General Public License as
  published by the Free Software Foundation, either version 2.1 of the
  License, or (at your option) any later version.

  libsydney is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with libsydney. If not, see
  <http://www.gnu.org/licenses/>.
***/

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <math.h>
#include <string.h>

#include <speex/speex_resampler.h>

#include "converter.h"
#include "sydney.h"
#include "macro.h"
#include "common.h"
#include "format.h"
#include "volscale.h"
#include "bswap.h"
#include "zero.h"
#include "add.h"
#include "bbuffer.h"
#include "continued-fraction.h"
#include "malloc.h"
#include "resample.h"

/* Steps: byteswap -> convert -> volscale -> remap -> resample -> convert -> byteswap -> interleave|serialize */

/* Sample formats we know to process natively */
static int native_pcm_format_process(sa_pcm_format_t f) {
    return
        f == SA_PCM_FORMAT_U8 ||
        f == SA_PCM_FORMAT_S16_NE ||
        f == SA_PCM_FORMAT_S32_NE ||
        f == SA_PCM_FORMAT_FLOAT32_NE;
}

/* Sample formats we know to resample natively  */
static int native_pcm_format_resample(sa_pcm_format_t f) {
    return
        f == SA_PCM_FORMAT_S16_NE ||
        f == SA_PCM_FORMAT_FLOAT32_NE;
}

static sa_pcm_format_t byteswap_fix(sa_pcm_format_t f) {

    switch (f) {
        case SA_PCM_FORMAT_U8:
        case SA_PCM_FORMAT_ULAW:
        case SA_PCM_FORMAT_ALAW:
            return f;

        case SA_PCM_FORMAT_S16_NE:
        case SA_PCM_FORMAT_S16_RE:
            return SA_PCM_FORMAT_S16_NE;

        case SA_PCM_FORMAT_S24_NE:
        case SA_PCM_FORMAT_S24_RE:
            return SA_PCM_FORMAT_S24_NE;

        case SA_PCM_FORMAT_S32_NE:
        case SA_PCM_FORMAT_S32_RE:
            return SA_PCM_FORMAT_S32_NE;

        case SA_PCM_FORMAT_FLOAT32_NE:
        case SA_PCM_FORMAT_FLOAT32_RE:
            return SA_PCM_FORMAT_FLOAT32_NE;

        case _SA_PCM_FORMAT_MAX:
            ;
    }

    sa_assert_not_reached();
}

static sa_pcm_format_t get_work_format(sa_pcm_format_t a, sa_pcm_format_t b) {

    switch (a) {
        case SA_PCM_FORMAT_U8:
        case SA_PCM_FORMAT_ULAW:
        case SA_PCM_FORMAT_ALAW:

            switch (b) {
                case SA_PCM_FORMAT_U8:
                    return SA_PCM_FORMAT_U8;

                case SA_PCM_FORMAT_ULAW:
                case SA_PCM_FORMAT_ALAW:
                case SA_PCM_FORMAT_S16_LE:
                case SA_PCM_FORMAT_S16_BE:
                    return SA_PCM_FORMAT_S16_NE;

                case SA_PCM_FORMAT_S24_LE:
                case SA_PCM_FORMAT_S24_BE:
                case SA_PCM_FORMAT_S32_LE:
                case SA_PCM_FORMAT_S32_BE:
                    return SA_PCM_FORMAT_S32_NE;

                case SA_PCM_FORMAT_FLOAT32_LE:
                case SA_PCM_FORMAT_FLOAT32_BE:
                    return SA_PCM_FORMAT_FLOAT32_NE;

                case _SA_PCM_FORMAT_MAX:
                    ;
            }

            break;

        case SA_PCM_FORMAT_S16_LE:
        case SA_PCM_FORMAT_S16_BE:

            switch (b) {
                case SA_PCM_FORMAT_U8:
                case SA_PCM_FORMAT_ULAW:
                case SA_PCM_FORMAT_ALAW:
                case SA_PCM_FORMAT_S16_LE:
                case SA_PCM_FORMAT_S16_BE:
                    return SA_PCM_FORMAT_S16_NE;

                case SA_PCM_FORMAT_S24_LE:
                case SA_PCM_FORMAT_S24_BE:
                case SA_PCM_FORMAT_S32_LE:
                case SA_PCM_FORMAT_S32_BE:
                    return SA_PCM_FORMAT_S32_NE;

                case SA_PCM_FORMAT_FLOAT32_LE:
                case SA_PCM_FORMAT_FLOAT32_BE:
                    return SA_PCM_FORMAT_FLOAT32_NE;

                case _SA_PCM_FORMAT_MAX:
                    ;
            }
            break;

        case SA_PCM_FORMAT_S24_LE:
        case SA_PCM_FORMAT_S24_BE:
        case SA_PCM_FORMAT_S32_LE:
        case SA_PCM_FORMAT_S32_BE:

            switch (b) {
                case SA_PCM_FORMAT_U8:
                case SA_PCM_FORMAT_ULAW:
                case SA_PCM_FORMAT_ALAW:
                case SA_PCM_FORMAT_S16_LE:
                case SA_PCM_FORMAT_S16_BE:
                case SA_PCM_FORMAT_S24_LE:
                case SA_PCM_FORMAT_S24_BE:
                case SA_PCM_FORMAT_S32_LE:
                case SA_PCM_FORMAT_S32_BE:
                    return SA_PCM_FORMAT_S32_NE;

                case SA_PCM_FORMAT_FLOAT32_LE:
                case SA_PCM_FORMAT_FLOAT32_BE:
                    return SA_PCM_FORMAT_FLOAT32_NE;

                case _SA_PCM_FORMAT_MAX:
                    ;
            }
            break;

        case SA_PCM_FORMAT_FLOAT32_LE:
        case SA_PCM_FORMAT_FLOAT32_BE:
            return SA_PCM_FORMAT_FLOAT32_NE;

        case _SA_PCM_FORMAT_MAX:
            ;
    }

    sa_assert_not_reached();
}

static sa_pcm_format_t fix_work_format_for_resample(sa_pcm_format_t f) {

    switch (f) {
        case SA_PCM_FORMAT_U8:
        case SA_PCM_FORMAT_S16_NE:
            return  SA_PCM_FORMAT_S16_NE;

        case SA_PCM_FORMAT_S32_NE:
        case SA_PCM_FORMAT_FLOAT32_NE:
            return SA_PCM_FORMAT_FLOAT32_NE;

        default:
            ;
    }

    sa_assert_not_reached();
}

int sa_converter_init(
        sa_converter_t *c,
        const pcm_attrs_t *from,
        const pcm_attrs_t *to,
        sa_bool_t dynamic_rate_enabled) {

    unsigned u, t;
    int resample_required;

    sa_assert(c);
    sa_assert(from);
    sa_assert(to);

    memset(c, 0, sizeof(*c));

    c->from_pcm_format = from->format;
    c->to_pcm_format = to->format;

    c->from_rate = from->rate;
    c->to_rate = to->rate;

    c->from_nchannels = from->nchannels;
    c->to_nchannels = to->nchannels;

    if (!(c->channel_map_table = sa_new(int, (from->nchannels+1) * to->nchannels)))
        goto fail;

    for (u = 0; u < to->nchannels; u++) {
        unsigned k = 0;

        for (t = 0; t < from->nchannels; t++) {

            if (from->channel_map[u] == to->channel_map[t]) {
                if (u != t)
                    c->remap_required = TRUE;

                c->channel_map_table[u * (from->nchannels+1) + k++] += (int) t;
            }
        }

        if (k > 1)
            c->sum_required = TRUE;

        c->channel_map_table[k] = (int) -1;
    }

    if (c->from_nchannels != c->to_nchannels)
        c->remap_required = TRUE;

    resample_required =
        from->rate != to->rate ||
        dynamic_rate_enabled;

    c->work_pcm_format = get_work_format(from->format, to->format);
    if (resample_required)
        c->work_pcm_format = fix_work_format_for_resample(c->work_pcm_format);

    sa_assert(native_pcm_format_process(c->work_pcm_format));
    sa_assert(!resample_required || native_pcm_format_resample(c->work_pcm_format));

    c->from_sample_size = sa_get_pcm_sample_size(c->from_pcm_format);
    c->work_sample_size = sa_get_pcm_sample_size(c->work_pcm_format);
    c->to_sample_size = sa_get_pcm_sample_size(c->to_pcm_format);

    /* Get function pointers */
    c->pre_byteswap_func = sa_get_byteswap_func(from->format);

    if (byteswap_fix(from->format) !=  c->work_pcm_format) {
        c->pre_format_func = sa_get_format_func(byteswap_fix(from->format), c->work_pcm_format);
        sa_assert(c->pre_format_func);
    }

    c->volscale_func = sa_get_volscale_func(c->work_pcm_format);
    sa_assert(c->volscale_func);

    c->zero_func = sa_get_zero_func(c->work_pcm_format);
    sa_assert(c->zero_func);

    c->add_func = sa_get_add_func(c->work_pcm_format);
    sa_assert(c->add_func);

    if (resample_required) {
        c->resample_func = sa_get_resample_func(c->work_pcm_format);
        sa_assert(c->resample_func);
    }

    if (c->work_pcm_format != byteswap_fix(to->format)) {
        c->post_format_func = sa_get_format_func(c->work_pcm_format, byteswap_fix(to->format));
        sa_assert(c->post_format_func);
    }

    c->post_byteswap_func = sa_get_byteswap_func(to->format);

    c->interleave_func = sa_get_interleave_func(to->format);
    sa_assert(c->interleave_func);

    /* Initialize resampler */

    if (resample_required) {
        if (!(c->speex = speex_resampler_init(c->to_nchannels, c->from_rate, c->to_rate, SPEEX_RESAMPLER_QUALITY_DEFAULT, NULL)))
            goto fail;
    }

    /* Initialize processing variables */

    if (!(c->from_process_data = sa_new0(void*, c->from_nchannels)))
        goto fail;

    if (!(c->from_stride = sa_new0(size_t, c->from_nchannels)))
        goto fail;

    if (!(c->to_process_data = sa_new0(void*, c->to_nchannels)))
        goto fail;

    if (!(c->to_stride = sa_new0(size_t, c->to_nchannels)))
        goto fail;

    /* Initialize volume stuff */
    if (!(c->volume_factor = sa_new(int32_t, c->from_nchannels)))
        goto fail;

    if (!(c->volume_divisor = sa_new(int32_t, c->from_nchannels)))
        goto fail;

    c->no_volume = TRUE;

    /* Initialize bounce buffers */

    if (sa_bbuffer_init(&c->bb_pre_byteswap, c->from_nchannels, c->from_sample_size) < 0)
        goto fail;
    if (sa_bbuffer_init(&c->bb_pre_format, c->from_nchannels, c->work_sample_size) < 0)
        goto fail;
    if (sa_bbuffer_init(&c->bb_volscale, c->from_nchannels, c->work_sample_size) < 0)
        goto fail;
    if (sa_bbuffer_init(&c->bb_remap, c->to_nchannels, c->work_sample_size) < 0)
        goto fail;
    if (sa_bbuffer_init(&c->bb_resample, c->to_nchannels, c->work_sample_size) < 0)
        goto fail;
    if (sa_bbuffer_init(&c->bb_post_format, c->to_nchannels, c->to_sample_size) < 0)
        goto fail;
    if (sa_bbuffer_init(&c->bb_post_byteswap, c->to_nchannels, c->to_sample_size) < 0)
        goto fail;
    if (sa_bbuffer_init(&c->bb_interleave, c->to_nchannels, c->to_sample_size) < 0)
        goto fail;
    if (sa_bbuffer_init(&c->bb_tmp, 1, 1) < 0)
        goto fail;

    return 0;

fail:
    sa_converter_done(c);

    return SA_ERROR_OOM;
}

void sa_converter_done(sa_converter_t *c) {
    sa_assert(c);

    sa_free(c->channel_map_table);

    sa_free(c->from_process_data);
    sa_free(c->to_process_data);
    sa_free(c->from_stride);
    sa_free(c->to_stride);

    sa_bbuffer_done(&c->bb_pre_byteswap);
    sa_bbuffer_done(&c->bb_pre_format);
    sa_bbuffer_done(&c->bb_volscale);
    sa_bbuffer_done(&c->bb_remap);
    sa_bbuffer_done(&c->bb_resample);
    sa_bbuffer_done(&c->bb_post_format);
    sa_bbuffer_done(&c->bb_post_byteswap);
    sa_bbuffer_done(&c->bb_interleave);
    sa_bbuffer_done(&c->bb_tmp);

    if (c->speex)
        speex_resampler_destroy(c->speex);

    sa_free(c->zero_buffer);

    sa_free(c->volume_divisor);
    sa_free(c->volume_factor);
    memset(c, 0, sizeof(*c));
}

void* sa_converter_get_zero_buffer(sa_converter_t *c, size_t size) {
    void *b;

    sa_assert(c);
    sa_assert(size > 0);

    if (c->zero_buffer && c->zero_size >= size)
        return c->zero_buffer;

    sa_free(c->zero_buffer);

    if (!(c->zero_buffer = sa_malloc(size)))
        return NULL;

    c->zero_func(b, c->work_sample_size, size);
    return c->zero_buffer;
}

int sa_converter_go(
        sa_converter_t *c,
        const void *const src[], const size_t sstr[], sa_interleave_t sinterleave,
        void **dst[], size_t *dstr[], sa_interleave_t dinterleave,
        size_t *size) {

    size_t* stride;
    void** process_data;
    sa_bool_t is_bounce;
    sa_interleave_t interleave;
    unsigned i;

    sa_assert(c);

    is_bounce = FALSE;
    stride = (size_t*) sstr;
    process_data = (void**) src;

    if (c->no_volume &&
        !c->remap_required &&
        !c->resample_func &&
        c->from_pcm_format == c->to_pcm_format) {

        /* We can shortcut this, since we don't need to do any real work.*/

        goto do_interleave;
    }

    if (c->pre_byteswap_func) {
        size_t k;

        k = dinterleave == SA_INTERLEAVE_YES ? c->from_sample_size*c->from_nchannels : c->from_sample_size;

        for (i = 0; i < c->from_nchannels; i++) {
            void *b;

            if (!(b = sa_bbuffer_get(&c->bb_pre_byteswap, i, *size, dinterleave == SA_INTERLEAVE_YES)))
                return SA_ERROR_OOM;

            c->pre_byteswap_func(b, k, process_data[i], stride[i], *size);
            c->from_process_data[i] = b;
            c->from_stride[i] = k;
        }

        process_data = c->from_process_data;
        stride = c->from_stride;
        interleave = dinterleave;
        is_bounce = TRUE;
    }

    if (c->pre_format_func) {

        if (is_bounce && c->from_sample_size == c->to_sample_size) {

            /* The data to process is already in a bounce buffer, we
             * can do this in-place */

            for (i = 0; i < c->from_nchannels; i++)
                c->pre_format_func(&c->bb_tmp, process_data[i], stride[i], process_data[i], stride[i], *size);

        } else {
            size_t k, new_size;

            new_size = *size / c->from_sample_size * c->work_sample_size;
            k = dinterleave == SA_INTERLEAVE_YES ? c->work_sample_size*c->from_nchannels : c->work_sample_size;

            for (i = 0; i < c->from_nchannels; i++) {
                void *b;

                if (!(b = sa_bbuffer_get(&c->bb_pre_format, i, new_size, dinterleave == SA_INTERLEAVE_YES)))
                    return SA_ERROR_OOM;

                c->pre_format_func(&c->bb_tmp, b, k, process_data[i], stride[i], *size);
                c->from_process_data[i] = b;
                c->from_stride[i] = k;
            }

            process_data = c->from_process_data;
            stride = c->from_stride;
            *size = new_size;
            interleave = dinterleave;
            is_bounce = TRUE;
        }
    }

    if (!c->no_volume) {

        sa_assert(c->volscale_func);

        if (is_bounce) {

            /* The data to process is already in a bounce buffer, we
             * can do this in-place */

            for (i = 0; i < c->from_nchannels; i++);
                c->volscale_func(process_data[i], stride[i], process_data[i], stride[i], c->volume_factor[i], c->volume_divisor[i], *size);

        } else {
            size_t k;

            k = dinterleave == SA_INTERLEAVE_YES ? c->work_sample_size*c->from_nchannels : c->work_sample_size;

            for (i = 0; i < c->from_nchannels; i++) {
                void *b;

                if (!(b = sa_bbuffer_get(&c->bb_volscale, i, *size, dinterleave == SA_INTERLEAVE_YES)))
                    return SA_ERROR_OOM;

                c->volscale_func(b, k, process_data[i], stride[i], c->volume_factor[i], c->volume_divisor[i], *size);
                c->from_process_data[i] = b;
                c->from_stride[i] = k;
            }

            process_data = c->from_process_data;
            stride = c->from_stride;
            interleave = dinterleave;
            is_bounce = TRUE;
        }
    }

    if (c->remap_required) {
        size_t k;
        sa_bool_t need_proper_interleave = FALSE;

        k = dinterleave == SA_INTERLEAVE_YES ? c->work_sample_size*c->to_nchannels : c->work_sample_size;

        for (i = 0; i < c->to_nchannels; i++) {
            void *b;
            int *p = &c->channel_map_table[i * (c->from_nchannels+1)];

            if (p[0] == -1) {
                /* We have to write silence to this channel */

                if (!(b = sa_converter_get_zero_buffer(c, *size)))
                    return SA_ERROR_OOM;

                c->to_process_data[i] = b;
                c->to_stride[i] = c->work_sample_size;

                need_proper_interleave = TRUE;
            } else if (p[1] == -1) {
                /* Just one channel, nothing to mix */

                c->to_process_data[i] = process_data[p[0]];
                c->to_stride[i] = stride[p[0]];

                need_proper_interleave = TRUE;
            } else {
                int j;

                /* We have to mix two or more channels */

                if (!(b = sa_bbuffer_get(&c->bb_remap, i, *size, dinterleave == SA_INTERLEAVE_YES)))
                    return SA_ERROR_OOM;

                c->add_func(b, k, process_data[p[0]], stride[p[0]], process_data[p[1]], stride[p[1]], *size);

                for (j = 2; p[j] != -1; j++)
                    c->add_func(b, k, b, k, process_data[p[j]], stride[p[j]], *size);

                c->to_process_data[i] = b;
                c->to_stride[i] = k;
            }
        }

        process_data = c->to_process_data;
        stride = c->to_stride;
        interleave = need_proper_interleave ? SA_INTERLEAVE_ANY : dinterleave;
        is_bounce = TRUE;
    }

    if (c->resample_func) {
        size_t k;
        size_t new_size;

        k = dinterleave == SA_INTERLEAVE_YES ? c->work_sample_size*c->to_nchannels : c->work_sample_size;

        new_size = (size_t) (((((uint64_t) *size+c->work_sample_size-1)/c->work_sample_size)*c->to_rate)/c->from_rate+1)*c->work_sample_size; /* FIXME */

        for (i = 0; i < c->to_nchannels; i++) {
            void *b;

            if (!(b = sa_bbuffer_get(&c->bb_resample, i, new_size, dinterleave == SA_INTERLEAVE_YES)))
                return SA_ERROR_OOM;

            c->resample_func(c->speex, i, b, k, process_data[i], stride[i], *size, &new_size);
            c->to_process_data[i] = b;
            c->to_stride[i] = k;
        }

        process_data = c->to_process_data;
        stride = c->to_stride;
        *size = new_size;
        interleave = dinterleave;
        is_bounce = TRUE;
    }

    if (c->post_format_func) {

        if (is_bounce && c->work_sample_size == c->to_sample_size) {

            /* The data to process is already in a bounce buffer, we
             * can do this in-place */

            for (i = 0; i < c->to_nchannels; i++)
                c->post_format_func(&c->bb_tmp, process_data[i], stride[i], process_data[i], stride[i], *size);

        } else {
            size_t k, new_size;

            k = dinterleave == SA_INTERLEAVE_YES ? c->to_sample_size*c->to_nchannels : c->to_sample_size;
            new_size = *size / c->work_sample_size * c->to_sample_size;

            for (i = 0; i < c->to_nchannels; i++) {
                void *b;

                if (!(b = sa_bbuffer_get(&c->bb_post_format, i, new_size, dinterleave == SA_INTERLEAVE_YES)))
                    return SA_ERROR_OOM;

                c->post_format_func(&c->bb_tmp, b, k, process_data[i], stride[i], *size);
                c->to_process_data[i] = b;
                c->to_stride[i] = k;
            }

            process_data = c->to_process_data;
            stride = c->to_stride;
            *size = new_size;
            interleave = dinterleave;
            is_bounce = TRUE;
        }
    }

    if (c->post_byteswap_func) {

        if (is_bounce) {

            /* The data to process is already in a bounce buffer, we
             * can do this in-place */

            for (i = 0; i < c->to_nchannels; i++)
                c->post_byteswap_func(process_data[i], stride[i], process_data[i], stride[i], *size);

        } else {
            size_t k;

            k = dinterleave == SA_INTERLEAVE_YES ? c->to_sample_size*c->to_nchannels : c->to_sample_size;

            for (i = 0; i < c->to_nchannels; i++) {
                void *b;

                if (!(b = sa_bbuffer_get(&c->bb_post_byteswap, i, *size, dinterleave == SA_INTERLEAVE_YES)))
                    return SA_ERROR_OOM;

                c->post_byteswap_func(b, k, process_data[i], stride[i], *size);

                c->to_process_data[i] = b;
                c->to_stride[i] = k;
            }

            process_data = c->to_process_data;
            stride = c->to_stride;
            interleave = dinterleave;
            is_bounce = TRUE;
        }
    }

do_interleave:

    if (dinterleave != SA_INTERLEAVE_ANY && interleave != dinterleave) {
        size_t k;

        k = dinterleave == SA_INTERLEAVE_YES ? c->to_sample_size*c->to_nchannels : c->to_sample_size;

        for (i = 0; i < c->to_nchannels; i++) {
            void *b;

            if (!(b = sa_bbuffer_get(&c->bb_interleave, i, *size, dinterleave == SA_INTERLEAVE_ANY)))
                return SA_ERROR_OOM;

            c->interleave_func(b, k, process_data[i], stride[i], *size);

            c->to_process_data[i] = b;
            c->to_stride[i] = k;
        }

        process_data = c->to_process_data;
        stride = c->to_stride;
        interleave = dinterleave;
        is_bounce = TRUE;
    }

    *dstr = stride;
    *dst = process_data;

    return SA_SUCCESS;
}

void sa_converter_set_volume(sa_converter_t *c, const int32_t vol[]) {
    unsigned i;
    sa_bool_t no_volume = TRUE;

    sa_assert(c);
    sa_assert(vol);

    for (i = 0; i < c->from_nchannels; i++) {
        int num, denom;

        if (vol[i] == 0) {
            c->volume_factor[i] = 1;
            c->volume_divisor[i] = 1;
        } else if (vol[i] <= SA_VOLUME_MUTED) {
            c->volume_factor[i] = 0;
            c->volume_divisor[i] = 1;
            no_volume = 0;
        } else {
            float f = volume_to_linear(vol[i]);

            sa_continued_fraction(f, 0x7FFF, &num, &denom);

            if (num != 1 || denom != 1)
                no_volume = FALSE;

            c->volume_factor[i] = (int32_t) num;
            c->volume_divisor[i] = (int32_t) denom;
        }
    }

    c->no_volume = no_volume;
}

int sa_converter_go_interleaved(
        sa_converter_t *c,
        const void *const data,
        void **dst[], size_t *dstr[], sa_interleave_t dinterleave,
        size_t *size) {

    unsigned i;
    const uint8_t *d = data;
    size_t stride;

    sa_assert(c);
    sa_assert(data);

    stride = c->from_nchannels * c->from_sample_size;

    for (i = 0; i < c->from_nchannels; i++) {
        c->from_process_data[i] = (void*) d;
        d += c->from_sample_size;
        c->from_stride[i] = stride;
    }

    return sa_converter_go(c, (const void *const*) c->from_process_data, c->from_stride, SA_INTERLEAVE_YES, dst, dstr, dinterleave, size);
}

void sa_converter_set_ratio(sa_converter_t *c, unsigned rate1, unsigned rate2) {
    sa_assert(c);
    sa_assert(c->speex);

    speex_resampler_set_rate(c->speex, rate1, rate2);
}