#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_int16_t_mono(int16_t *dest,
                                     int16_t *src,
                                     size_t count,
                                     float bias_l,
                                     float bias_r) {
    size_t i;

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

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

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

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

    xas_mixer_input *input  = mixer->inputs,
                      *output = mixer->output;

    void (*mixer_apply)(int16_t *,
                        int16_t *,
                        size_t,
                        float,
                        float) = (output->channels == 2)?
        mixer_apply_int16_t_stereo:
        mixer_apply_int16_t_mono;

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

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

        ssize_t readlen;

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

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

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

        input = next;
    }

    return ret;

error_audio_read_stream:
    return -1;
}

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

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

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

    if ((mixer->output = xas_audio_stream_new_source((xas_audio_fill)mixer_fill,
                                                       NULL,
                                                       mixer,
                                                       sample_size,
                                                       sample_rate,
                                                       channels,
                                                       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;

        free(input);

        input = next;
    }

    xas_audio_stream_destroy(mixer->output);

    free(mixer);
}

static inline void input_set_pan(xas_mixer_input *input, float pan) {
    float angle,
          term,
          cosf_angle,
          sinf_angle;

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

    angle      = pan * (M_PI / 2.0f);
    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
     || stream->channels    != mixer->output->channels
     || stream->buffer_size != mixer->buffer_size) {
        errno = EINVAL;

        goto error_invalid_stream;
    }

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

    input->stream = stream;
    input->gain   = gain;

    input_set_pan(input, pan);

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

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