#include "CgeSample.h"
#include <string.h>

static size_t bytesPerSample(int format) {
    switch (format) {
    default:
        return 0;

    case CGE_SAMPLE_U8:
    case CGE_SAMPLE_S8:
        return 1;

    case CGE_SAMPLE_U16:
    case CGE_SAMPLE_S16:
        return 2;

    case CGE_SAMPLE_U24:
    case CGE_SAMPLE_S24:
    case CGE_SAMPLE_U32:
    case CGE_SAMPLE_S32:
    case CGE_SAMPLE_F32:
        return 4;

    case CGE_SAMPLE_F64:
        return 8;
    }
}

size_t CgeSampleInitLayout(CgeSampleBuf *buf, int format, size_t count) {
    buf->count = count;
    buf->format = format;
    buf->data = NULL;
    buf->stride = bytesPerSample(format);

    return buf->stride * count;
}

float CgeSampleGetF32(const CgeSampleBuf *buf, size_t index) {
    void *data;

    data = (char *)buf->data + index * buf->stride;
    switch (buf->format) {
    default:
    case CGE_SAMPLE_UNKNOWN: return 0.0f;
    case CGE_SAMPLE_S8: return (float)(*(int8_t*)data) / 128.0f;
    case CGE_SAMPLE_U8: return ((float)(*(uint8_t*)data) - 128.0f) / 128.0f;
    case CGE_SAMPLE_S16: return (float)(*(int16_t*)data) / 32768.0f;
    case CGE_SAMPLE_U16: return ((float)(*(uint16_t*)data) - 32768.0f) / 32768.0f;
    case CGE_SAMPLE_S24: return (float)(*(int32_t*)data) / 8388608.0f;
    case CGE_SAMPLE_U24: return ((float)(*(uint32_t*)data) - 8388608.0f) / 8388608.0f;
    case CGE_SAMPLE_S32: return (float)(*(int32_t*)data) / 2147483648.0f;
    case CGE_SAMPLE_U32: return ((float)(*(uint32_t*)data) - 2147483648.0f) / 2147483648.0f;
    case CGE_SAMPLE_F32: return (*(float*)data);
    case CGE_SAMPLE_F64: return (*(double*)data);
    }
}

void CgeSampleSetF32(CgeSampleBuf *buf, size_t index, float value) {
    void *data;

    data = (char *)buf->data + index * buf->stride;
    switch (buf->format) {
    default:
    case CGE_SAMPLE_UNKNOWN:
        break;

    case CGE_SAMPLE_S8:
        value *= 128.0f;
        value += (value >= 0.0f) ? 0.5f : -0.5f;
        if (value > 127.0f) value = 127.0f;
        if (value < -128.0f) value = -128.0f;
        *(int8_t*)data = value;
        break;

    case CGE_SAMPLE_U8:
        value = value * 128.0f + 128.0f;
        value += (value >= 0.0f) ? 0.5f : -0.5f;
        if (value > 255.0f) value = 255.0f;
        if (value < 0.0f) value = 0.0f;
        *(uint8_t*)data = value;
        break;

    case CGE_SAMPLE_S16:
        value *= 32768.0f;
        value += (value >= 0.0f) ? 0.5f : -0.5f;
        if (value > 32767.0f) value = 32767.0f;
        if (value < -32768.0f) value = -32768.0f;
        *(int16_t*)data = value;
        break;

    case CGE_SAMPLE_U16:
        value = value * 32768.0f + 32768.0f;
        value += (value >= 0.0f) ? 0.5f : -0.5f;
        if (value > 65535.0f) value = 65535.0f;
        if (value < 0.0f) value = 0.0f;
        *(uint16_t*)data = value;
        break;

    case CGE_SAMPLE_S24:
        value *= 8388608.0f;
        value += (value >= 0.0f) ? 0.5f : -0.5f;
        if (value > 8388607.0f) value = 8388607.0f;
        if (value < -8388608.0f) value = -8388608.0f;
        *(int32_t*)data = value;
        break;

    case CGE_SAMPLE_U24:
        value = value * 8388608.0f + 8388608.0f;
        value += (value >= 0.0f) ? 0.5f : -0.5f;
        if (value > 16777215.0f) value = 16777215.0f;
        if (value < 0.0f) value = 0.0f;
        *(uint32_t*)data = value;
        break;

    case CGE_SAMPLE_S32: {
        double scaled = value;
        scaled *= 2147483648.0;
        scaled += (scaled >= 0.0) ? 0.5 : -0.5;
        if (scaled > 2147483647.0) scaled = 2147483647.0;
        if (scaled < -2147483648.0) scaled = -2147483648.0;
        *(int32_t*)data = scaled;
        break;
    }

    case CGE_SAMPLE_U32: {
        double scaled = value;
        scaled = scaled * 2147483648.0 + 2147483648.0;
        scaled += (scaled >= 0.0) ? 0.5 : -0.5;
        if (scaled > 4294967295.0) scaled = 4294967295.0;
        if (scaled < 0.0) scaled = 0.0;
        *(uint32_t*)data = scaled;
        break;
    }

    case CGE_SAMPLE_F32:
        *(float*)data = value;
        break;

    case CGE_SAMPLE_F64:
        *(double*)data = (double)value;
        break;
    }
}

int16_t CgeSampleGetS16(const CgeSampleBuf *buf, size_t index) {
    void *data;

    data = (char *)buf->data + index * buf->stride;
    switch (buf->format) {
    default:
    case CGE_SAMPLE_UNKNOWN: return 0;
    case CGE_SAMPLE_S8: return (int16_t)*(int8_t*)data * 256;
    case CGE_SAMPLE_U8: return ((int16_t)*(uint8_t*)data - 128) * 256;
    case CGE_SAMPLE_S16: return *(int16_t*)data;
    case CGE_SAMPLE_U16: return ((uint32_t)*(uint16_t*)data - 32768);
    case CGE_SAMPLE_S24: return (*(int32_t*)data / 256);
    case CGE_SAMPLE_U24: return ((*(uint32_t*)data / 256) - 32768);
    case CGE_SAMPLE_S32: return (*(int32_t*)data / 65536);
    case CGE_SAMPLE_U32: return ((*(uint32_t*)data / 65536) - 32768);
    case CGE_SAMPLE_F32: {
        float value = *(float*)data;
        value *= 32768.0f;
        value += (value >= 0.0f) ? 0.5f : -0.5f;
        if (value > 32767.0f) value = 32767.0f;
        if (value < -32768.0f) value = -32768.0f;
        return (int16_t)value;
    }
    case CGE_SAMPLE_F64: {
        float value = (float)(*(double*)data);
        value *= 32768.0;
        value += (value >= 0.0) ? 0.5 : -0.5;
        if (value > 32767.0) value = 32767.0;
        if (value < -32768.0) value = -32768.0;
        return (int16_t)value;
    }
    }
}

void CgeSampleSetS16(CgeSampleBuf *buf, size_t index, int16_t value) {
    void *data;
    int32_t scaled;
    uint32_t unscaled;

    data = (char *)buf->data + index * buf->stride;
    switch (buf->format) {
    default:
    case CGE_SAMPLE_UNKNOWN:
        break;

    case CGE_SAMPLE_S8:
        *(int8_t*)data = (int8_t)(value / 256);
        break;

    case CGE_SAMPLE_U8:
        *(uint8_t*)data = (uint8_t)(value / 256 + 128);
        break;

    case CGE_SAMPLE_S16:
        *(int16_t*)data = value;
        break;

    case CGE_SAMPLE_U16:
        *(uint16_t*)data = (uint16_t)((int32_t)value + 32768);
        break;

    case CGE_SAMPLE_S24:
        scaled = (int32_t)value * 256;
        if (scaled > 8388607) scaled = 8388607;
        if (scaled < -8388608) scaled = -8388608;
        *(int32_t*)data = scaled;
        break;

    case CGE_SAMPLE_U24:
        scaled = (((int32_t)value + 32768) & 0xFFFF) * 256;
        if (scaled > 16777215) scaled = 16777215;
        *(uint32_t*)data = (uint32_t)scaled;
        break;

    case CGE_SAMPLE_S32:
        *(int32_t*)data = (int32_t)value * 65536;
        break;

    case CGE_SAMPLE_U32:
        unscaled = (((uint32_t)((int32_t)value + 32768)) & 0xFFFFu) * 65536u;
        *(uint32_t*)data = unscaled;
        break;

    case CGE_SAMPLE_F32:
        *(float*)data = (float)value / 32768.0f;
        break;

    case CGE_SAMPLE_F64:
        *(double*)data = (double)value / 32768.0;
        break;
    }
}

static void fastCopy(void *src, size_t srcStride, void *dst,
                     size_t dstStride, size_t count, size_t size) {
    size_t i;

    for (i = 0; i < count; i++) {
        memcpy(dst, src, size);
        src = (char*)src + srcStride;
        dst = (char*)dst + dstStride;
    }
}

static void fastS16ToF32(void *src, size_t srcStride, void *dst,
                         size_t dstStride, size_t count) {
    const float scale = 1.0f / 32768.0f;
    size_t i;

    for (i = 0; i < count; i++) {
        *(float *)dst = *(int16_t *)src * scale;
        src = (char *)src + srcStride;
        dst = (char *)dst + dstStride;
    }
}

static void fastF32ToS16(void *src, size_t srcStride, void *dst,
                         size_t dstStride, size_t count) {
    size_t i;

    for (i = 0; i < count; i++) {
        int32_t v = (int32_t)(*(float *)src * 32768.0f);

        if (v > 32767)
            v = 32767;
        else if (v < -32768)
            v = -32768;

        *(int16_t *)dst = (int16_t)v;
        src = (void *)((char *)src + srcStride);
        dst = (void *)((char *)dst + dstStride);
    }
}

static void fastS24ToF32(void *src, size_t srcStride, void *dst,
                         size_t dstStride, size_t count) {
    const float scale = 1.0f / 8388608.0f;
    size_t i;

    for (i = 0; i < count; i++) {
        *(float *)dst = *(int32_t *)src * scale;
        src = (void *)((char *)src + srcStride);
        dst = (void *)((char *)dst + dstStride);
    }
}

static void fastF32ToS24(void *src, size_t srcStride, void *dst,
                         size_t dstStride, size_t count) {
    size_t i;
    for (i = 0; i < count; i++) {
        int32_t v = (int32_t)(*(float *)src * 8388608.0f);

        if (v > 8388607)
            v = 8388607;
        else if (v < -8388608)
            v = -8388608;

        *(int32_t *)dst = (int32_t)v;
        src = (void *)((char *)src + srcStride);
        dst = (void *)((char *)dst + dstStride);
    }
}

void CgeSampleConvert(void *src, int srcFormat, size_t srcStride,
                      void *dst, int dstFormat, size_t dstStride,
                      size_t count) {
    CgeSampleBuf srcBuf, dstBuf;
    size_t i;

    srcStride = (srcStride) ? (srcStride) : bytesPerSample(srcFormat);
    dstStride = (dstStride) ? (dstStride) : bytesPerSample(dstFormat);

    if (srcFormat == dstFormat) {
        fastCopy(src, srcStride, dst, dstStride, count, bytesPerSample(srcFormat));
    } else if (srcFormat == CGE_SAMPLE_S16 && dstFormat == CGE_SAMPLE_F32) {
        fastS16ToF32(src, srcStride, dst, dstStride, count);
    } else if (srcFormat == CGE_SAMPLE_F32 && dstFormat == CGE_SAMPLE_S16) {
        fastF32ToS16(src, srcStride, dst, dstStride, count);
    } else if (srcFormat == CGE_SAMPLE_F32 && dstFormat == CGE_SAMPLE_S24) {
        fastF32ToS24(src, srcStride, dst, dstStride, count);
    } else if (srcFormat == CGE_SAMPLE_S24 && dstFormat == CGE_SAMPLE_F32) {
        fastS24ToF32(src, srcStride, dst, dstStride, count);
    } else {
        CgeSampleInitLayout(&srcBuf, srcFormat, count);
        CgeSampleInitLayout(&dstBuf, dstFormat, count);
        srcBuf.stride = srcStride;
        dstBuf.stride = dstStride;
        srcBuf.data = src;
        dstBuf.data = dst;

        for (i = 0; i < count; i++)
           CgeSampleSetF32(&dstBuf, i, CgeSampleGetF32(&srcBuf, i));
    }
}