#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <math.h>
#include <errno.h>

#include <xas/audio.h>
#include <xas/mixer.h>

static void mixer_apply_mono_to_mono(int16_t *dest,
                                     int16_t *src,
                                     size_t count,
                                     float gain,
                                     float bias_l,
                                     float bias_r) {
    size_t i;

    for (i=0; i<count; i++) {
        dest[i] += gain * src[i];
    }
}

static void mixer_apply_mono_to_stereo(int16_t *dest,
                                       int16_t *src,
                                       size_t count,
                                       float gain,
                                       float bias_l,
                                       float bias_r) {
    size_t i;

    for (i=0; i<count; i++) {
        dest[i*2]   += gain * bias_l * src[i];
        dest[i*2+1] += gain * bias_r * src[i];
    }
}

static void mixer_apply_stereo_to_stereo(int16_t *dest,
                                         int16_t *src,
                                         size_t count,
                                         float gain,
                                         float bias_l,
                                         float bias_r) {
    size_t i;

    for (i=0; i<count; i++) {
        dest[i*2]   += gain * bias_l * src[i*2];
        dest[i*2+1] += gain * bias_r * src[i*2+1];
    }
}

static void mixer_apply_stereo_to_mono(int16_t *dest,
                                       int16_t *src,
                                       size_t count,
                                       float gain,
                                       float bias_l,
                                       float bias_r) {
    size_t i;

    for (i=0; i<count; i++) {
        dest[i] += gain * ((src[i*2] + src[i*2+1]) / 2);
    }
}

static ssize_t mixer_fill(xas_mixer *mixer,
                          void *samples,
                          size_t count,
                          xas_audio_stream *output) {
    ssize_t total = 0;

    xas_mixer_input *input = mixer->inputs;

    memset(mixer->buf,
           '\0',
           output->channels * output->sample_size * output->buffer_size);

    while (input) {
        void (*mixer_apply)(int16_t *,
                            int16_t *,
                            size_t,
                            float,
                            float,
                            float);

        xas_mixer_input *next = input->next;
        int16_t *buf;

        ssize_t readlen;

        if (output->channels == 2 && input->stream->channels == 2) {
            mixer_apply = mixer_apply_stereo_to_stereo;
        } else if (output->channels == 2 && input->stream->channels == 1) {
            mixer_apply = mixer_apply_mono_to_stereo;
        } else if (output->channels == 1 && input->stream->channels == 1) {
            mixer_apply = mixer_apply_mono_to_mono;
        } else if (output->channels == 1 && input->stream->channels == 2) {
            mixer_apply = mixer_apply_stereo_to_mono;
        } else {
            goto error_invalid_input;
        }

        if ((readlen = xas_audio_stream_read(input->stream,
                                               (void **)&buf,
                                               count)) < 0) {
            goto error_audio_read_stream;
        }

        mixer_apply((int16_t *)(mixer->buf),
                    buf,
                    readlen,
                    input->gain,
                    input->bias_l,
                    input->bias_r);

        if (total < readlen) {
            total = readlen;
        }

        input = next;
    }

    memcpy(samples,
           mixer->buf,
           total * output->channels * output->sample_size);

    return total;

error_invalid_input:
error_audio_read_stream:
    return -1;
}

xas_mixer *xas_mixer_new(size_t channels,
                             size_t sample_size,
                             size_t sample_rate,
                             size_t buffer_size) {
    xas_mixer *mixer;

    if ((mixer = malloc(sizeof(*mixer))) == NULL) {
        goto error_malloc_mixer;
    }

    if ((mixer->buf = malloc(channels * sample_size * buffer_size)) == NULL) {
        goto error_malloc_buf;
    }

    if ((mixer->output = xas_audio_stream_new_source((xas_audio_fill)mixer_fill,
                                                       NULL,
                                                       mixer,
                                                       channels,
                                                       sample_size,
                                                       sample_rate,
                                                       buffer_size)) == NULL) {
        goto error_audio_stream_new_source;
    }

    mixer->inputs      = NULL;
    mixer->last        = NULL;
    mixer->buffer_size = buffer_size;

    return mixer;

error_audio_stream_new_source:
    free(mixer->buf);

error_malloc_buf:
    free(mixer);

error_malloc_mixer:
    return NULL;
}

void xas_mixer_destroy(xas_mixer *mixer) {
    xas_mixer_input *input = mixer->inputs;

    while (input) {
        xas_mixer_input *next = input->next;

        free(input);

        input = next;
    }

    xas_audio_stream_destroy(mixer->output);

    free(mixer->buf);
    free(mixer);
}

xas_audio_stream *xas_mixer_output(xas_mixer *mixer) {
    return mixer->output;
}

static inline void input_set_pan(xas_mixer_input *input, float pan) {
    static float range = 45.0f * (M_PI / 180.0);

    float angle,
          term,
          cosf_angle,
          sinf_angle;

    if (pan < -1.0 || pan > 1.0) {
        return;
    }

    angle      = pan * range;
    term       = sqrtf(2.0f) / 2.0f;
    cosf_angle = cosf(angle);
    sinf_angle = sinf(angle);

    input->bias_l = term * (cosf_angle - sinf_angle);
    input->bias_r = term * (cosf_angle + sinf_angle);
}

xas_mixer_input *xas_mixer_input_add(xas_mixer *mixer,
                                         xas_audio_stream *stream,
                                         float gain,
                                         float pan) {
    xas_mixer_input *input;

    if (stream->sample_size != mixer->output->sample_size
     || stream->sample_rate != mixer->output->sample_rate) {
        errno = EINVAL;

        goto error_invalid_stream;
    }

    if ((input = malloc(sizeof(*input))) == NULL) {
        goto error_malloc_input;
    }

    input->stream = stream;
    input->gain   = gain;
    input->next   = NULL;

    input_set_pan(input, pan);

    if (mixer->inputs == NULL) {
        mixer->inputs = input;
        mixer->last   = input;
    } else {
        mixer->last->next = input;
        mixer->last       = input;
    }

    return input;

error_malloc_input:
error_invalid_stream:
    return NULL;
}

void xas_mixer_input_set_gain(xas_mixer_input *input, float gain) {
    if (gain < 0.0 || gain > 1.0) {
        return;
    }

    input->gain = gain;
}

void xas_mixer_input_set_pan(xas_mixer_input *input, float pan) {
    return input_set_pan(input, pan);
}