#include <inttypes.h>

#include "macro.h"
#include "interleave.h"

static void interleave8(void *_dst, size_t dstr, const void *_src, size_t sstr, size_t bytes) {
    uint8_t *dst = _dst;
    const uint8_t *src = _src;

    for (; bytes > 0; bytes--, src += sstr, dst += dstr)
        *dst = *src;
}

static void interleave16(void *_dst, size_t dstr, const void *_src, size_t sstr, size_t bytes) {
    uint16_t *dst = _dst;
    const uint16_t *src = _src;
    unsigned n = bytes / sizeof(uint16_t);

    for (; n > 0; n--, src += sstr/sizeof(uint16_t), dst += dstr/sizeof(uint16_t))
        *dst = *src;
}

static void interleave24(void *_dst, size_t dstr, const void *_src, size_t sstr, size_t bytes) {
    uint8_t *dst = _dst;
    const uint8_t *src = _src;
    unsigned n = bytes / 3;

    for (; n > 0; n--, src += sstr/3, dst += dstr/3) {
        dst[0] = src[0];
        dst[1] = src[1];
        dst[2] = src[2];
    }
}

static void interleave32(void *_dst, size_t dstr, const void *_src, size_t sstr, size_t bytes) {
    uint32_t *dst = _dst;
    const uint32_t *src = _src;
    unsigned n = bytes / sizeof(uint32_t);

    for (; n > 0; n--, src += sstr/sizeof(uint32_t), dst += dstr/sizeof(uint32_t))
        *dst = *src;
}

interleave_func_t get_interleave_func(sa_pcm_format_t f) {
    
    static const interleave_func_t funcs[_SA_PCM_FORMAT_MAX] = {
        [SA_PCM_FORMAT_U8] = interleave8,
        [SA_PCM_FORMAT_ULAW] = interleave8,
        [SA_PCM_FORMAT_ALAW] = interleave8,
        [SA_PCM_FORMAT_S16_LE] = interleave16,
        [SA_PCM_FORMAT_S16_BE] = interleave16,
        [SA_PCM_FORMAT_S24_LE] = interleave24,
        [SA_PCM_FORMAT_S24_BE] = interleave24,
        [SA_PCM_FORMAT_S32_LE] = interleave32,
        [SA_PCM_FORMAT_S32_BE] = interleave32,
        [SA_PCM_FORMAT_FLOAT32_LE] = interleave32,
        [SA_PCM_FORMAT_FLOAT32_BE] = interleave32,
    };
            
    sa_assert(f < _SA_PCM_FORMAT_MAX); 

    return funcs[f];
}