#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;
        *dst = t < -0x80 ? 0 : (t > 0x7F ? 0xFF : (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;
        *dst = t < -0x8000 ? 0x8000 : (t > 0x7FFF ? 0x7FFF : (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;
        *dst = t < -0x80000000L ? (int32_t) 0x80000000L : (t > 0x7fffffffL ? (int32_t) 0x7fffffffL : (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));
}

volscale_func_t get_volscale_func(sa_pcm_format_t f) {
    
    static const 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];
}