#include "audio.h"
#include <math.h>

void audio_multiple_channels_to_mono(float* restrict buffer, size_t buffer_len, float* restrict out, int channels) {
    float f;
    switch(channels) {
        case 1:
            memcpy(out, buffer, sizeof(float) * buffer_len);
            break;
        case 2:
            for (int i = 0; i < buffer_len; i += 2){
                out[i/2] = (buffer[i] + buffer[i+1]) / 2;
            }
            break;
        default:
            for (int i = 0; i < buffer_len; i += channels){
                f = buffer[i];
                for (int j = 1; j < channels; ++j) {
                    f += buffer[i + j];
                }
                out[i/channels] = f / (float)channels;
            }
    }
}

void audio_multiple_channels_to_mono_int32(int32_t* restrict buffer, size_t buffer_len, int32_t* restrict out, int channels) {
    int64_t s;
    switch(channels) {
        case 1:
            memcpy(out, buffer, sizeof(int32_t) * buffer_len);
            break;
        case 2:
            for (int i = 0; i < buffer_len; i += 2){
                out[i/2] = (int32_t)((((int64_t)buffer[i]) + ((int64_t)buffer[i+1])) / 2);
            }
            break;
        default:
            for (int i = 0; i < buffer_len; i += channels){
                s = buffer[i];
                for (int j = 1; j < channels; ++j) {
                    s += buffer[i + j];
                }
                out[i/channels] = (int32_t)(s / channels);
            }
    }
}

void audio_multiple_channels_to_stereo(float* restrict buffer, size_t buffer_len, float* restrict out, int channels) {
    int samples = 0;
    float surroundMix = 1 / sqrt(2);
    switch(channels) {
        case 1: //mono, duplicate channels
            for (int i = 0; i < buffer_len; i++){
                out[i*2] = buffer[i];
                out[i*2+1] = buffer[i];
            }
            break;
        case 2: //copy
            memcpy(out, buffer, sizeof(float) * buffer_len);
            break;
        case 3: //2.1, FL, FR, LFE
            for (int i = 0; i < buffer_len; i += 3){
                int FL = i;
                int FR = i+1;
                int LFE = i+2;
                out[samples*2] = buffer[FL];
                out[samples*2+1] = buffer[FR];
                ++samples;
            }
        case 5: //5.0, FL, FR, FC, RL, RR
            for (int i = 0; i < buffer_len; i += 5){
                int FL = i;
                int FR = i+1;
                int C = i+2;
                int RL = i+3;
                int RR = i+4;
			    //TODO: this is wrong
                out[samples*2] = buffer[FL] + surroundMix * buffer[C] + surroundMix * buffer[RL];
                out[samples*2+1] = buffer[FR] + surroundMix * buffer[C] + surroundMix * buffer[RR];
                ++samples;
            }
        case 6: //5.1, FL, FR, FC, LFE, RL, RR
            for (int i = 0; i < buffer_len; i += 6){
                int FL = i;
                int FR = i+1;
                int C = i+2;
                int LFE = i+3;
                int RL = i+4;
                int RR = i+5;
			    //TODO: this is wrong
                out[samples*2] = buffer[FL] + surroundMix * buffer[C] + surroundMix * buffer[RL];
                out[samples*2+1] = buffer[FR] + surroundMix * buffer[C] + surroundMix * buffer[RR];
                ++samples;
            }
            break;
        default: //no known formula, just take stereo out of it
            for (int i = 0; i < buffer_len; i += channels){
                out[samples*2] = buffer[i];
                out[samples*2+1] = buffer[i+1];
                ++samples;
            }
    }
}

void audio_multiple_channels_to_stereo_int32(int32_t* restrict buffer, size_t buffer_len, int32_t* restrict out, int channels) {
    int samples = 0;
    float surroundMix = 1 / sqrt(2);
    switch(channels) {
        case 1: //mono, duplicate channels
            for (int i = 0; i < buffer_len; i++){
                out[i*2] = buffer[i];
                out[i*2+1] = buffer[i];
            }
            break;
        case 2: //copy
            memcpy(out, buffer, sizeof(int32_t) * buffer_len);
            break;
        case 3: //2.1, FL, FR, LFE
            for (int i = 0; i < buffer_len; i += 3){
                int FL = i;
                int FR = i+1;
                int LFE = i+2;
                out[samples*2] = buffer[FL];
                out[samples*2+1] = buffer[FR];
                ++samples;
            }
        case 5: //5.0, FL, FR, FC, RL, RR
            for (int i = 0; i < buffer_len; i += 5){
                int FL = i;
                int FR = i+1;
                int C = i+2;
                int RL = i+3;
                int RR = i+4;
			    //TODO: this is wrong
                out[samples*2] = ((int64_t)buffer[FL]) + (int64_t)(surroundMix * buffer[C] + surroundMix * buffer[RL]);
                out[samples*2+1] = ((int64_t)buffer[FR]) + (int64_t)(surroundMix * buffer[C] + surroundMix * buffer[RR]);
                ++samples;
            }
        case 6: //5.1, FL, FR, FC, LFE, RL, RR
            for (int i = 0; i < buffer_len; i += 6){
                int FL = i;
                int FR = i+1;
                int C = i+2;
                int LFE = i+3;
                int RL = i+4;
                int RR = i+5;
			    //TODO: this is wrong
                out[samples*2] = ((int64_t)buffer[FL]) + (int64_t)(surroundMix * buffer[C] + surroundMix * buffer[RL]);
                out[samples*2+1] = ((int64_t)buffer[FR]) + (int64_t)(surroundMix * buffer[C] + surroundMix * buffer[RR]);
                ++samples;
            }
            break;
        default: //no known formula, just take stereo out of it
            for (int i = 0; i < buffer_len; i += channels){
                out[samples*2] = buffer[i];
                out[samples*2+1] = buffer[i+1];
                ++samples;
            }
    }
}

void audio_int32_to_int16(int32_t* restrict data, size_t data_len, int16_t* restrict buffer, int bitDepth){
    switch(bitDepth) {
        case 8:
            for (int i = 0; i < data_len; i++){
                buffer[i] = data[i];
            }
            break;
        case 16:
            for (int i = 0; i < data_len; i++){
                buffer[i] = (int16_t)data[i];
            }
            break;
        case 24:
            for (int i = 0; i < data_len; i++){
                buffer[i] = (int16_t)(data[i] >> 8);
            }
            break;
        case 32:
            for (int i = 0; i < data_len; i++){
                buffer[i] = (int16_t)(data[i] >> 16);
            }
            break;
        default:
        {
            int diff = bitDepth - 16;
            if (diff >= 0) {
                for (int i = 0; i < data_len; i++){
                    buffer[i] = (int16_t)(data[i] >> diff);
                }
            } else {
                for (int i = 0; i < data_len; i++){
                    buffer[i] = (int16_t)(data[i] << diff);
                }
            }
        }

    }
}

void audio_int32_to_bytes(int32_t* restrict data, size_t data_len, int8_t* restrict buffer, int bitDepth){
    switch(bitDepth) {
        case 8:
            for (int i = 0; i < data_len; i++){
                buffer[i] = data[i];
            }
            break;
        case 16:
            for (int i = 0; i < data_len; i++){
                *((int16_t*)&buffer[i*2]) = (int16_t)data[i];
            }
            break;
        case 24:
            {
                uint32_t sample;

                for (int i = 0; i < data_len; i++){
                    buffer[i*3] = data[i] & 0xFF;
                    buffer[i*3+1] = (data[i] >> 8) & 0xFF;
                    buffer[i*3+2] = (data[i] >> 16) & 0xFF;
                }
            }
            break;
        case 32:
            memcpy(buffer, data, sizeof(int32_t) * data_len);
            break;
    }
}

void audio_bytes_to_int32(int8_t* restrict data, size_t data_len, int32_t* restrict buffer, int bitDepth){
    switch(bitDepth) {
        case 8:
            for (int i = 0; i < data_len; i++){
                buffer[i] = data[i];
            }
            break;
        case 16:
            for (int i = 0; i < data_len/2; i++){
                buffer[i] = ((int16_t*)data)[i];
            }
            break;
        case 24:
            {
                uint32_t sample;

                for (int i = 0; i < data_len; i += 3){
                    sample = data[i];
                    sample += ((uint32_t) data[i+1]) << 8;
                    sample += ((uint32_t) data[i+2]) << 16;
                    //sign extend
                    buffer[i/3] = (int32_t)(sample << 8) >> 8;
                }
            }
            break;
        case 32:
            memcpy(buffer, data, data_len);
            break;
    }
}

void audio_int32_to_float32(int32_t* restrict data, size_t data_len, float* restrict buffer, int bitDepth){
    switch(bitDepth) {
        case 16:
            for (int i = 0; i < data_len; ++i){
                buffer[i] = ((float)data[i]) / BITS_TO_DIV(16);
            }
            break;
        case 24:
            for (int i = 0; i < data_len; ++i){
                buffer[i] = ((float)data[i]) / BITS_TO_DIV(24);
            }
            break;
        default:
            for (int i = 0; i < data_len; ++i){
                buffer[i] = ((float)data[i]) / BITS_TO_DIV(bitDepth);
            }
    }
}

//special case
void audio_int24_to_float32(int8_t* restrict data, size_t data_len, float* restrict buffer, int bitDepth){
    uint32_t sample;

    for (int i = 0; i < data_len; i += 3){
        sample = data[i];
        sample += ((uint32_t) data[i+1]) << 8;
        sample += ((uint32_t) data[i+2]) << 16;

        //sign extend
        buffer[i/3] = ((float)((int32_t)(sample << 8) >> 8)) / BITS_TO_DIV(bitDepth);
    }
}

void audio_int16_to_float32(int16_t* restrict data, size_t data_len, float* restrict buffer, int bitDepth){
    switch(bitDepth) {
        case 16:
            for (int i = 0; i < data_len; ++i){
                buffer[i] = ((float)data[i]) / BITS_TO_DIV(16);
            }
            break;
        default:
            for (int i = 0; i < data_len; ++i){
                buffer[i] = ((float)data[i]) / BITS_TO_DIV(bitDepth);
            }
    }
}

void audio_int8_to_float32(int8_t* restrict data, size_t data_len, float* restrict buffer, int bitDepth){
    switch(bitDepth) {
        case 8:
            for (int i = 0; i < data_len; ++i){
                buffer[i] = ((float)data[i]) / BITS_TO_DIV(8);
            }
            break;
        default:
            for (int i = 0; i < data_len; ++i){
                buffer[i] = ((float)data[i]) / BITS_TO_DIV(bitDepth);
            }
    }
}

void audio_float32_to_int32(float* restrict data, size_t data_len, int32_t* restrict buffer, int bitDepth){
    for (int i = 0; i < data_len; ++i){
        float f = data[i];
        if (f < -1.0) {
            f = -1.0;
        }
        if (f > 1.0) {
            f = 1.0;
        }
        buffer[i] = (int32_t)(f * BITS_TO_DIV(bitDepth));
    }
}

void audio_float32_to_int24(float* restrict data, size_t data_len, int8_t* restrict buffer){
    for (int i = 0; i < data_len; ++i){
        float f = data[i];
        if (f < -1.0) {
            f = -1.0;
        }
        if (f > 1.0) {
            f = 1.0;
        }

        int32_t value = (int32_t)(f * BITS_TO_DIV(24));
        buffer[i*3] = (int8_t) (value & 0xFF);
        buffer[i*3+1] = (int8_t) ((value >> 8) & 0xFF);
        buffer[i*3+2] = (int8_t) ((value >> 16) & 0xFF);
    }
}

void audio_float32_to_int16(float* restrict data, size_t data_len, int16_t* restrict buffer){
    for (int i = 0; i < data_len; ++i){
        float f = data[i];
        if (f < -1.0) {
            f = -1.0;
        }
        if (f > 1.0) {
            f = 1.0;
        }
        buffer[i] = (int16_t)(f * BITS_TO_DIV(16));
    }
}

void audio_float32_to_int8(float* restrict data, size_t data_len, int8_t* restrict buffer){
    for (int i = 0; i < data_len; ++i){
        float f = data[i];
        if (f < -1.0) {
            f = -1.0;
        }
        if (f > 1.0) {
            f = 1.0;
        }
        buffer[i] = (int8_t)(f * BITS_TO_DIV(8));
    }
}