/***
  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 <inttypes.h>
#include <sys/types.h>

#include <liboil/liboil.h>

#include "format.h"
#include "g711.h"
#include "macro.h"

/* u8 --> */

static int format_u8_to_s16(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int16_t d = -0x80, f = 0x100;

    oil_conv_s16_u8(dst, (int) dstr, src, (int) sstr, (int) bytes);
    oil_scalaradd_s16(dst, (int) dstr, dst, (int) dstr, &d, (int) bytes);
    oil_scalarmult_s16(dst, (int) dstr, dst, (int) dstr, &f, (int) bytes);

    return SA_SUCCESS;
}

static int format_u8_to_s32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int32_t d = -0x80, f = 0x1000000;

    oil_conv_s16_u8(dst, (int) dstr, src, (int) sstr, (int) bytes);
    oil_scalaradd_s32(dst, (int) dstr, dst, (int) dstr, &d, (int) bytes);
    oil_scalarmult_s32(dst, (int) dstr, dst, (int) dstr, &f, (int) bytes);

    return SA_SUCCESS;
}

static int format_u8_to_f32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    float d = (float) -0x80, f = 1.0f/0x7F;

    oil_conv_f32_u8(dst, (int) dstr, src, (int) sstr, (int) bytes);
    oil_scalaradd_f32(dst, (int) dstr, dst, (int) dstr, &d, (int) bytes);
    oil_scalarmult_f32(dst, (int) dstr, dst, (int) dstr, &f, (int) bytes);

    return SA_SUCCESS;
}

/* ulaw --> */

static int format_ulaw_to_s16(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int16_t *d = dst;
    const uint8_t *s = src;

    for (; bytes > 0; bytes --, d += dstr/sizeof(int16_t), s += sstr)
        *d = sa_ulaw2linear16(*s);

    return SA_SUCCESS;
}

static int format_ulaw_to_s32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int32_t *d = dst;
    const uint8_t *s = src;

    for (; bytes > 0; bytes --, d += dstr/sizeof(int32_t), s += sstr)
        *d = (int32_t) sa_ulaw2linear16(*s) * 0x10000;

    return SA_SUCCESS;
}

static int format_ulaw_to_f32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    float *d = dst;
    const uint8_t *s = src;

    for (; bytes > 0; bytes --, d += dstr/sizeof(float), s += sstr)
        *d = (float) sa_ulaw2linear16(*s) / (float) 0x7FFF;

    return SA_SUCCESS;
}

/* alaw --> */

static int format_alaw_to_s16(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int16_t *d = dst;
    const uint8_t *s = src;

    for (; bytes > 0; bytes --, d += dstr/sizeof(int16_t), s += sstr)
        *d = sa_alaw2linear16(*s);

    return SA_SUCCESS;
}

static int format_alaw_to_s32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int32_t *d = dst;
    const uint8_t *s = src;

    for (; bytes > 0; bytes --, d += dstr/sizeof(int32_t), s += sstr)
        *d = (int32_t) sa_alaw2linear16(*s) * 0x10000;

    return SA_SUCCESS;
}

static int format_alaw_to_f32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    float *d = dst;
    const uint8_t *s = src;

    for (; bytes > 0; bytes --, d += dstr/sizeof(float), s += sstr)
        *d = (float) sa_alaw2linear16(*s) / (float) 0x7FFF;

    return SA_SUCCESS;
}

/* s16 --> */

static int format_s16_to_u8(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const int16_t *s = src;
    size_t n = bytes/sizeof(int16_t);

    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int16_t))
        *d = (uint8_t) (*s / 0x100 + 0x80);

    return SA_SUCCESS;
}

static int format_s16_to_ulaw(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const int16_t *s = src;
    size_t n = bytes/sizeof(int16_t);

    for (; n > 0; n --, d += dstr, s += sstr/sizeof(int16_t))
        *d = sa_14linear2ulaw(*s);

    return SA_SUCCESS;
}

static int format_s16_to_alaw(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const int16_t *s = src;
    size_t n = bytes/sizeof(int16_t);

    for (; n > 0; n --, d += dstr, s += sstr/sizeof(int16_t))
        *d = sa_13linear2alaw(*s);

    return SA_SUCCESS;
}

static int format_s16_to_s32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int f = 0x10000;
    size_t n = bytes/sizeof(int16_t);

    oil_conv_s32_s16(dst, (int) dstr, src, (int) sstr, (int) n);
    oil_scalarmult_s32(dst, (int) dstr, dst, (int) dstr, &f, (int) n);

    return SA_SUCCESS;
}

static int format_s16_to_f32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    float f = 1.0f/0x7ffff;
    size_t n = bytes/sizeof(int16_t);

    oil_conv_f32_s16(dst, (int) dstr, src, (int) sstr, (int) n);
    oil_scalarmult_f32(dst, (int) dstr, dst, (int) dstr, &f, (int) n);

    return SA_SUCCESS;
}

/* s24 --> */

static int format_s24_to_s32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int32_t *d = dst;
    const uint8_t *s = src;
    size_t n = bytes/3;

    for (; n > 0; n--, d += dstr/sizeof(int32_t), s += sstr/3)
#if defined(SA_LITTLE_ENDIAN)
        *d = (int32_t) ((int32_t) (int8_t) s[2] * 0x1000000 + (int32_t) s[1] * 0x10000 + (int32_t) s[0] * 0x100);
#elif defined(SA_BIG_ENDIAN)
        *d = (int32_t) ((int32_t) (int8_t) s[0] * 0x1000000 + (int32_t) s[1] * 0x10000 + (int32_t) s[2] * 0x100);
#else
#error "Unknown byte order"
#endif

    return SA_SUCCESS;
}

static int format_s24_to_f32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    float *d = dst;
    const uint8_t *s = src;
    size_t n = bytes/3;

    for (; n > 0; n--, d += dstr/sizeof(float), s += sstr/3)
#if defined(SA_LITTLE_ENDIAN)
        *d = (float) ((int32_t) (int8_t) s[2] * 0x10000 + (int32_t) s[1] * 0x100 + (int32_t) s[0]) / (float) 0x7fffff;
#elif defined(SA_BIG_ENDIAN)
        *d = (float) ((int32_t) (int8_t) s[0] * 0x10000 + (int32_t) s[1] * 0x100 + (int32_t) s[2]) / (float) 0x7fffff;
#else
#error "Unknown byte order"
#endif

    return SA_SUCCESS;
}

/* s32 --> */

static int format_s32_to_u8(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const int32_t *s = src;
    size_t n = bytes/sizeof(int32_t);

    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int32_t))
        *d = (uint8_t) (*s / 0x1000000 + 0x80);

    return SA_SUCCESS;
}

static int format_s32_to_ulaw(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const int32_t *s = src;
    size_t n = bytes/sizeof(int32_t);

    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int32_t))
        *d = sa_14linear2ulaw((int16_t) (*s / 0x10000));

    return SA_SUCCESS;
}

static int format_s32_to_alaw(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const int32_t *s = src;
    size_t n = bytes/sizeof(int32_t);

    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int32_t))
        *d = sa_13linear2alaw((int16_t) (*s / 0x10000));

    return SA_SUCCESS;
}

static int format_s32_to_s16(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int16_t *d = dst;
    const int32_t *s = src;
    size_t n = bytes/sizeof(int32_t);

    for (; n > 0; n--, d += dstr/sizeof(int16_t), s += sstr/sizeof(int32_t))
        *d = (int16_t) (*s / 0x10000);

    return SA_SUCCESS;
}

static int format_s32_to_s24(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const int32_t *s = src;
    size_t n = bytes/sizeof(float);

    for (; n > 0; n--, d += dstr/3, s += sstr/sizeof(int32_t)) {
        uint32_t j = (uint32_t) (*s) >> 8;

#if defined(SA_LITTLE_ENDIAN)
        d[0] = (uint8_t) j;
        d[1] = (uint8_t) (j >> 8);
        d[2] = (uint8_t) (j >> 16);
#elif defined(SA_BIG_ENDIAN)
        d[2] = (uint8_t) j;
        d[1] = (uint8_t) (j >> 8);
        d[0] = (uint8_t) (j >> 16);
#else
#error "Unknown byte order"
#endif
    }

    return SA_SUCCESS;
}

static int format_s32_to_f32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    float f = 1.0f/(float) 0x7fffffff;
    size_t n = bytes/sizeof(int32_t);

    oil_conv_f32_s32(dst, (int) dstr, src, (int) sstr, (int) n);
    oil_scalarmult_f32(dst, (int) dstr, dst, (int) dstr, &f, (int) n);

    return SA_SUCCESS;
}

/* f32 --> */

static int format_f32_to_u8(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const float *s = src;
    float *buf;
    float f = 0x7F, p = 0x80;
    size_t n = bytes/sizeof(float);

    if (!(buf = sa_bbuffer_get(b, 0, bytes, 1)))
        return SA_ERROR_OOM;

    oil_scalarmult_f32(buf, sizeof(float), s, (int) sstr, &f, (int) n);
    oil_scalaradd_f32(buf, sizeof(float), buf, sizeof(float), &p, (int) n);
    oil_clipconv_u8_f32(d, (int) dstr, buf, sizeof(float), (int) n);

    return SA_SUCCESS;
}

static int format_f32_to_ulaw(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const float *s = src;
    size_t n = bytes/sizeof(float);

    for (; n > 0; n --, d += dstr, s += sstr/sizeof(float)) {
        float v = *s * 0x7FFF;
        v = SA_CLAMP(v, (float) -0x8000, (float) 0x7FFF);
        *d = sa_13linear2alaw((int16_t) v);
    }

    return SA_SUCCESS;
}

static int format_f32_to_alaw(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const float *s = src;
    size_t n = bytes/sizeof(float);

    for (; n > 0; n --, d += dstr, s += sstr/sizeof(float)) {
        float v = *s * 0x7FFF;
        v = SA_CLAMP(v, (float) -0x8000, (float) 0x7FFF);
        *d = sa_13linear2alaw((int16_t) v);
    }

    return SA_SUCCESS;
}

static int format_f32_to_s16(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int16_t *d = dst;
    const float *s = src;
    float *buf;
    float f = 0x7FFF;
    size_t n = bytes/sizeof(float);

    if (!(buf = sa_bbuffer_get(b, 0, bytes, 1)))
        return SA_ERROR_OOM;

    oil_scalarmult_f32(buf, sizeof(float), s, (int) sstr, &f, (int) n);
    oil_clipconv_s16_f32(d, (int) dstr, buf, sizeof(float), (int) n);

    return SA_SUCCESS;
}

static int format_f32_to_s24(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    uint8_t *d = dst;
    const float *s = src;
    size_t n = bytes/sizeof(float);

    for (; n > 0; n--, d += dstr/3, s += sstr/sizeof(float)) {
        float f = *s / 0x7fffff;
        uint32_t j;

        f = SA_CLAMP(f, (float) -0x800000, (float) 0x7FFFFF);

        j = (uint32_t) ((int32_t) f);

#if defined(SA_LITTLE_ENDIAN)
        d[0] = (uint8_t) j;
        d[1] = (uint8_t) (j >> 8);
        d[2] = (uint8_t) (j >> 16);
#elif defined(SA_BIG_ENDIAN)
        d[2] = (uint8_t) j;
        d[1] = (uint8_t) (j >> 8);
        d[0] = (uint8_t) (j >> 16);
#else
#error "Unknown byte order"
#endif
    }

    return SA_SUCCESS;
}

static int format_f32_to_s32(sa_bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
    int32_t *d = dst;
    const float *s = src;
    float *buf;
    float f = (float) 0x7FFFFFFF;
    size_t n = bytes/sizeof(float);

    if (!(buf = sa_bbuffer_get(b, 0, bytes, 1)))
        return SA_ERROR_OOM;

    oil_scalarmult_f32(buf, sizeof(float), s, (int) sstr, &f, (int) n);
    oil_clipconv_s32_f32(d, (int) dstr, buf, sizeof(float), (int) n);

    return SA_SUCCESS;
}

sa_format_func_t sa_get_format_func(sa_pcm_format_t from, sa_pcm_format_t to) {

    /* Rules: we need conversion functions:
     *
     *  1. from all formats to all "better" work formats
     *  2. from all work formats to all "lower" formats
     *  3. only for NE types
     */

    static const sa_format_func_t funcs[_SA_PCM_FORMAT_MAX * _SA_PCM_FORMAT_MAX] = {
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_U8         + SA_PCM_FORMAT_S16_NE    ] = format_u8_to_s16,      /* done */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_U8         + SA_PCM_FORMAT_S32_NE    ] = format_u8_to_s32,      /* done */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_U8         + SA_PCM_FORMAT_FLOAT32_NE] = format_u8_to_f32,      /* done */

        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ULAW       + SA_PCM_FORMAT_S16_NE    ] = format_ulaw_to_s16,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ULAW       + SA_PCM_FORMAT_S32_NE    ] = format_ulaw_to_s32,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ULAW       + SA_PCM_FORMAT_FLOAT32_NE] = format_ulaw_to_f32,    /* done, no liboil */

        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ALAW       + SA_PCM_FORMAT_S16_NE    ] = format_alaw_to_s16,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ALAW       + SA_PCM_FORMAT_S32_NE    ] = format_alaw_to_s32,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ALAW       + SA_PCM_FORMAT_FLOAT32_NE] = format_alaw_to_f32,    /* done, no liboil */

        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_U8        ] = format_s16_to_u8,      /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_ULAW      ] = format_s16_to_ulaw,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_ALAW      ] = format_s16_to_alaw,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_S32_NE    ] = format_s16_to_s32,     /* done */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_FLOAT32_NE] = format_s16_to_f32,     /* done */

        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S24_NE     + SA_PCM_FORMAT_S32_NE    ] = format_s24_to_s32,     /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S24_NE     + SA_PCM_FORMAT_FLOAT32_NE] = format_s24_to_f32,     /* done, no liboil */

        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_U8        ] = format_s32_to_u8,      /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_ULAW      ] = format_s32_to_ulaw,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_ALAW      ] = format_s32_to_alaw,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_S16_NE    ] = format_s32_to_s16,     /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_S24_NE    ] = format_s32_to_s24,     /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_FLOAT32_NE] = format_s32_to_f32,     /* done, but suboptimal */

        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_U8        ] = format_f32_to_u8,      /* done */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_ULAW      ] = format_f32_to_ulaw,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_ALAW      ] = format_f32_to_alaw,    /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_S16_NE    ] = format_f32_to_s16,     /* done */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_S24_NE    ] = format_f32_to_s24,     /* done, no liboil */
        [_SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_S32_NE    ] = format_f32_to_s32,     /* done, but suboptimal */
    };

    sa_assert(from < _SA_PCM_FORMAT_MAX);
    sa_assert(to < _SA_PCM_FORMAT_MAX);

    return funcs[from * _SA_PCM_FORMAT_MAX + to];
}