#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, dstr, src, sstr, bytes);
    oil_scalaradd_s16(dst, dstr, dst, dstr, &d, bytes);
    oil_scalarmult_s16(dst, dstr, dst, dstr, &f, 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, dstr, src, sstr, bytes);
    oil_scalaradd_s32(dst, dstr, dst, dstr, &d, bytes);
    oil_scalarmult_s32(dst, dstr, dst, dstr, &f, 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 = -0x80, f = 1.0/0x7F;

    oil_conv_f32_u8(dst, dstr, src, sstr, bytes);
    oil_scalaradd_f32(dst, dstr, dst, dstr, &d, bytes);
    oil_scalarmult_f32(dst, dstr, dst, dstr, &f, 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 = sa_ulaw2linear16(*s * 1.0F / 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 = sa_alaw2linear16(*(s++) * 1.0F / 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;
    unsigned 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;
    unsigned 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;
    unsigned 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;
    unsigned n = bytes/sizeof(int16_t);

    oil_conv_s32_s16(dst, dstr, src, sstr, n);
    oil_scalarmult_s32(dst, dstr, dst, dstr, &f, 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.0/0x7ffff;
    unsigned n = bytes/sizeof(int16_t);

    oil_conv_f32_s16(dst, dstr, src, sstr, n);
    oil_scalarmult_f32(dst, dstr, dst, dstr, &f, 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;
    unsigned n = bytes/3;

    for (; n > 0; n--, d += dstr/sizeof(int32_t), s += sstr/3)
#if defined(SA_LITTLE_ENDIAN)
        *d = (int32_t) ((int8_t) s[2]) * 0x1000000 + s[1] * 0x10000 + s[0] * 0x100;
#elif defined(SA_BIG_ENDIAN)
        *d = (int32_t) ((int8_t) s[0]) * 0x1000000 + s[1] * 0x10000 + 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;
    unsigned n = bytes/3;

    for (; n > 0; n--, d += dstr/sizeof(float), s += sstr/3)
#if defined(SA_LITTLE_ENDIAN)
        *d = ((float) ((int8_t) s[2]) * 0x10000 + s[1] * 0x100 + s[0]) / 0x7fffff;
#elif defined(SA_BIG_ENDIAN)
        *d = ((float) ((int8_t) s[0]) * 0x10000 + s[1] * 0x100 + s[2]) / 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;
    unsigned 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;
    unsigned n = bytes/sizeof(int32_t);

    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int32_t))
        *d = sa_14linear2ulaw(*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;
    unsigned n = bytes/sizeof(int32_t);

    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int32_t))
        *d = sa_13linear2alaw(*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;
    unsigned 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;
    unsigned 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] = j & 0xFF;
        d[1] = (j >> 8) & 0xFF;
        d[2] = (j >> 16);
#elif defined(SA_BIG_ENDIAN)
        d[2] = j & 0xFF;
        d[1] = (j >> 8) & 0xFF;
        d[0] = (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.0/0x7fffffff;
    unsigned n = bytes/sizeof(int32_t);

    oil_conv_f32_s32(dst, dstr, src, sstr, n);
    oil_scalarmult_f32(dst, dstr, dst, dstr, &f, 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;
    unsigned n = bytes/sizeof(float);

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

    oil_scalarmult_f32(buf, sizeof(float), s, sstr, &f, n);
    oil_scalaradd_f32(buf, sizeof(float), buf, sizeof(float), &p, n);
    oil_clipconv_u8_f32(d, dstr, buf, sizeof(float), 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;
    unsigned n = bytes/sizeof(float);

    for (; n > 0; n --, d += dstr, s += sstr/sizeof(float)) {
        float v = *s * 0x7FFF;
        if (v < -0x8000) v = -0x8000;
        if (v > -0x7FFF) v = -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;
    unsigned n = bytes/sizeof(float);

    for (; n > 0; n --, d += dstr, s += sstr/sizeof(float)) {
        float v = *s * 0x7FFF;
        if (v < -0x8000) v = -0x8000;
        if (v > -0x7FFF) v = -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;
    unsigned n = bytes/sizeof(float);

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

    oil_scalarmult_f32(buf, sizeof(float), s, sstr, &f, n);
    oil_clipconv_s16_f32(d, dstr, buf, sizeof(float), 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;
    unsigned n = bytes/sizeof(float);

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

        if (f >  0x7fffff) f =  0x7fffff;
        if (f < -0x800000) f = -0x800000;

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

#if defined(SA_LITTLE_ENDIAN)
        d[0] = j & 0xFF;
        d[1] = (j >> 8) & 0xFF;
        d[2] = (j >> 16);
#elif defined(SA_BIG_ENDIAN)
        d[2] = j & 0xFF;
        d[1] = (j >> 8) & 0xFF;
        d[0] = (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 = 0x7FFFFFFF;
    unsigned n = bytes/sizeof(float);

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

    oil_scalarmult_f32(buf, sizeof(float), s, sstr, &f, n);
    oil_clipconv_s32_f32(d, dstr, buf, sizeof(float), 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];
}