/***
  This file is part of libsydney.

  Copyright 2007-2008 Lennart Poettering

  libsydney is free software; you can redistribute it and/or modify it
  under the terms of the GNU Lesser General Public License as
  published by the Free Software Foundation, either version 2.1 of the
  License, or (at your option) any later version.

  libsydney is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with libsydney. If not, see
  <http://www.gnu.org/licenses/>.
***/

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

#include <liboil/liboil.h>

#include "macro.h"
#include "add.h"

static void add_u8(void *dst, size_t dstr, const void *src1, size_t sstr1, const void *src2, size_t sstr2, size_t bytes) {
    uint8_t *d = dst;
    const uint8_t *s1 = src1, *s2 = src2;

    for (; bytes > 0; bytes--, d += dstr, s1 += sstr1, s2 += sstr2) {
        int16_t v = (int16_t) *s1 + (int16_t) *s2 - (int16_t) 0x80;

        v = SA_CLAMP(v, (int16_t) 0, (int16_t) 0xFF);
        *d = (uint8_t) v;
    }
}

static void add_s16(void *dst, size_t dstr, const void *src1, size_t sstr1, const void *src2, size_t sstr2, size_t bytes) {
    oil_vectoradd_s_s16(dst, (int) dstr, src1, (int) sstr1, src2, (int) sstr2, (int) (bytes/sizeof(int16_t)));
}

static void add_s32(void *dst, size_t dstr, const void *src1, size_t sstr1, const void *src2, size_t sstr2, size_t bytes) {
    int32_t *d = dst;
    const int32_t *s1 = src1, *s2 = src2;

    for (; bytes > 0; bytes--, d += dstr/sizeof(int32_t), s1 += sstr1/sizeof(int32_t), s2 += sstr2/sizeof(int32_t)) {
        int64_t v = (int64_t) *s1 + (int64_t) *s2;

        v = SA_CLAMP(v, -0x80000000LL, 0x7FFFFFFFLL);
        *d = (int32_t) v;
    }
}

static void add_f32(void *dst, size_t dstr, const void *src1, size_t sstr1, const void *src2, size_t sstr2, size_t bytes) {
    float a = 1, b = 1;
    oil_vectoradd_f32(dst, (int) dstr, src1, (int) sstr1, src2, (int) sstr2, (int) (bytes/sizeof(int32_t)), &a, &b);
}

sa_add_func_t sa_get_add_func(sa_pcm_format_t f) {

    static const sa_add_func_t funcs[_SA_PCM_FORMAT_MAX] = {
        [SA_PCM_FORMAT_U8] = add_u8,
        [SA_PCM_FORMAT_S16_NE] = add_s16,
        [SA_PCM_FORMAT_S32_NE] = add_s32,
        [SA_PCM_FORMAT_FLOAT32_NE] = add_f32
    };

    sa_assert(f < _SA_PCM_FORMAT_MAX);

    return funcs[f];
}