#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 "byteswap.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 */

/* 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;
            break;
            
        case SA_PCM_FORMAT_S32_NE:
        case SA_PCM_FORMAT_FLOAT32_NE:
            return SA_PCM_FORMAT_FLOAT32_NE;
            break;
            
        default:
            ;
    }

    sa_assert_not_reached();
}

int sa_converter_init(
        sa_converter_t *c,
        const pcm_attrs_t *from,
        const pcm_attrs_t *to,
        int 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 = 1;
                
                c->channel_map_table[u * (from->nchannels+1) + k++] += t;
            }
        }

        if (k > 1)
            c->sum_required = 1;
        
        c->channel_map_table[k] = (unsigned) -1;
    }

    if (c->from_nchannels != c->to_nchannels)
        c->remap_required = 1;    
    
    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 = 1;

    /* 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));
}

static void* 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_func;
}

int sa_converter_go(
        sa_converter_t *c,
        const void *const src[], const size_t sstr[], int sinterleave,
        void **dst[], size_t *dstr[], int dinterleave,
        size_t *size) {
    
    size_t* stride;
    void** process_data;
    int is_bounce;
    int interleave;
    unsigned i;
    
    sa_assert(c);

    is_bounce = 0;
    stride = (size_t*) sstr;
    process_data = (void**) src;
    interleave = !!sinterleave;
    dinterleave = !!dinterleave;
    
    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 ? 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)))
                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 = 1;
    }
    
    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 ? 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)))
                    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 = 1;
        }
    }

    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 ? 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)))
                    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 = 1;
        }
    }

    if (c->remap_required) {
        size_t k;
        int need_proper_interleave = 0;

        k = dinterleave ? 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 = 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 = 1;
            } 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 = 1;
            } else {
                int j;
                
                /* We have to mix two or more channels */ 

                if (!(b = sa_bbuffer_get(&c->bb_remap, i, *size, dinterleave)))
                    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 ? -1 : dinterleave;
        is_bounce = 1;
    }

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

        k = dinterleave ? 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)))
                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 = 1;
    }

    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 ? 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)))
                    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 = 1;
        }
    }
    
    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 ? 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)))
                    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 = 1;
        }
    }

do_interleave:
    
    if (interleave != dinterleave) {
        size_t k;

        k = dinterleave ? 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)))
                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 = 1;
    }

    *dstr = stride;
    *dst = process_data;

    return SA_SUCCESS;
}

void sa_converter_set_volume(sa_converter_t *c, const int32_t vol[]) {
    unsigned i;
    int no_volume = 1;
    
    sa_assert(c);

    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 = powf(10.0, (float) vol[i] / 2000);
            
            sa_continued_fraction(f, 0x7FFF, &num, &denom);
            
            if (num != 1 || denom != 1)
                no_volume = 0;
            
            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[], int dinterleave,
        size_t *size) {

    unsigned i;
    const uint8_t *d = data;
    unsigned stride = c->from_nchannels * c->from_sample_size;

    for (i = 0; i < c->from_nchannels; i++) {
        c->from_process_data[i] = (void*) data;
        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, 1, dst, dstr, dinterleave, size);
}

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

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