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

#include <liboil/liboil.h>

#include "macro.h"
#include "volscale.h"

static void volscale_u8(void *_dst, size_t dstr, const void *_src, size_t sstr, int32_t factor, int32_t divisor, size_t bytes) {
    uint8_t *dst = _dst;
    const uint8_t *src = _src;

    for (; bytes > 0; bytes --) {
        int32_t t = (((int32_t) *src - 0x80) * factor) / divisor;
        t = CLAMP(t, -0x80, 0x7F);
        *dst = (int8_t) (t+0x80);

        src += sstr;
        dst += dstr;
    }
}

static void volscale_s16(void *_dst, size_t dstr, const void *_src, size_t sstr, int32_t factor, int32_t divisor, size_t bytes) {
    int16_t *dst = _dst;
    const int16_t *src = _src;
    unsigned n = bytes / sizeof(int16_t);

    for (; n > 0; n--) {
        int32_t t = ((int32_t) *src * factor) / divisor;
        t = CLAMP(t, -0x8000, 0x7FFF);
        *dst = (int16_t) t;

        src += sstr / sizeof(int16_t);
        dst += dstr / sizeof(int16_t);
    }
}

static void volscale_s32(void *_dst, size_t dstr, const void *_src, size_t sstr, int32_t factor, int32_t divisor, size_t bytes) {
    int32_t *dst = _dst;
    const int32_t *src = _src;
    unsigned n = bytes / sizeof(int32_t);

    for (; n > 0; n--) {
        int64_t t = ((int64_t) *src * factor) / divisor;
        t = CLAMP(t, -0x80000000L, 0x7FFFFFFFL);
        *dst = (int32_t) t;

        src += sstr / sizeof(int32_t);
        dst += dstr / sizeof(int32_t);
    }
}

static void volscale_f32(void *_dst, size_t dstr, const void *_src, size_t sstr, int32_t factor, int32_t divisor, size_t size) {
    float *dst = _dst;
    const float *src = _src;
    float f = (float) factor / (float) divisor;

    oil_scalarmult_f32(dst, dstr, src, sstr, &f, size / sizeof(float));
}

sa_volscale_func_t sa_get_volscale_func(sa_pcm_format_t f) {

    static const sa_volscale_func_t funcs[_SA_PCM_FORMAT_MAX] = {
        [SA_PCM_FORMAT_U8] = volscale_u8,
        [SA_PCM_FORMAT_S16_NE] = volscale_s16,
        [SA_PCM_FORMAT_S32_NE] = volscale_s32,
        [SA_PCM_FORMAT_FLOAT32_NE] = volscale_f32
    };

    sa_assert(f < _SA_PCM_FORMAT_MAX);

    return funcs[f];
}