/*---------------------------------------------------------------------------*/
/*
  File: dyncompr.cpp

  Dynamic compressor.
*/
/*---------------------------------------------------------------------------*/

#include "platform.h"
#include "sdsputil.h"
#include "dyncompr.h"

#define FILE_ "dyncompr.cpp"

/*---------------------------------------------------------------------------*/

#define DYNAMIC_COMPRESSOR_TIME_CONSTANT_RATIO3 0x4000
#define DYNAMIC_COMPRESSOR_TIME_CONSTANT_RATIO2 0x6000
#define DYNAMIC_COMPRESSOR_NOISE_TIME           0x0400

void dynamic_compressor_init(t_dynamic_compressor *p_dyncompr, dword energy_threshold, dword max_gain, dword noise_threshold, word noise_attenuation, int mode, dword limit_threshold)
{
    short s;
    dword d;
    word w, u, v, lim;

    p_dyncompr->mode = mode;
    p_dyncompr->energy_threshold = energy_threshold;
    p_dyncompr->limit_threshold = limit_threshold;
    if (mode == DYNAMIC_COMPRESSOR_MODE_RATIO3) {
        p_dyncompr->time_constant = DYNAMIC_COMPRESSOR_TIME_CONSTANT_RATIO3;
        d = (dword)(sdsp_sqrt(max_gain)) << 8;
    }
    else {
        p_dyncompr->time_constant = DYNAMIC_COMPRESSOR_TIME_CONSTANT_RATIO2;
        d = max_gain;
    }
    p_dyncompr->max_gain = d;
    s = 1 - sdsp_dword_leading_zeroes(p_dyncompr->max_gain);
    d <<= -s;
    w = (word)(((dword)(0x3fffffff / (d >> 16))) >> 1);
    w >>= s + 15;
    p_dyncompr->gain = ((dword) w) << 16;
    u = sdsp_sqrt(energy_threshold);
    d = ((((dword) u) * ((dword)(p_dyncompr->max_gain >> 16))) << 1) +
        (((dword)(((dword) u) * ((dword)(p_dyncompr->max_gain & 0xffff)))) >> 15);
    if (d > 0x8000) {
        d = 0x8000;
    }
    lim = sdsp_sqrt(limit_threshold);
    if (mode == DYNAMIC_COMPRESSOR_MODE_RATIO3) {
        u = sdsp_cbrt(((dword) u) << 16, 0);
        v = sdsp_cbrt(d << 16, 0);
        lim = sdsp_sqrt(lim << 16);
    }
    else {
        u = sdsp_sqrt(((dword) u) << 16);
        v = sdsp_sqrt(d << 16);
    }
    p_dyncompr->gain_range = (word)((((dword) w) * ((dword)((lim <= u) ? lim : u))) >> 15);
    p_dyncompr->gain_range_for_gain = (word)((((dword) w) * ((dword)((lim <= v) ? lim : v))) >> 15);
    p_dyncompr->noise_threshold = noise_threshold;
    if (mode == DYNAMIC_COMPRESSOR_MODE_RATIO3) {
        p_dyncompr->noise_attenuation = sdsp_sqrt(((dword) noise_attenuation) << 16);
    }
    else {
        p_dyncompr->noise_attenuation = noise_attenuation;
    }
}

short dynamic_compressor_exec(t_dynamic_compressor *p_dyncompr, short in_sample)
{
    short s;
    int32 l, m;
    word g;

    g = (word)(p_dyncompr->gain >> 16);
    m = ((((dword) g) * ((dword)(p_dyncompr->max_gain >> 16))) << 1) +
        (((dword)(((dword) g) * ((dword)(p_dyncompr->max_gain & 0xffff)))) >> 15);
    l = ((((int32) in_sample) * (((dword) m) >> 16)) << 1) + (((int32)(((int32) in_sample) * ((dword)(m & 0xffff)))) >> 15);
    if (l > 32767)
        l = 32767;
    else if (l < -32768)
        l = -32768;
    if (p_dyncompr->mode == DYNAMIC_COMPRESSOR_MODE_RATIO3) {
        l = ((((int32) l) * (((dword) m) >> 16)) << 1) + (((int32)(((int32) l) * ((dword)(m & 0xffff)))) >> 15);
        if (l > 32767)
            l = 32767;
        else if (l < -32768)
            l = -32768;
    }
    s = (short) l;
    if ((g >= p_dyncompr->noise_attenuation) && (((dword)((((int32) in_sample) * ((int32) in_sample)) << 1)) < p_dyncompr->noise_threshold)) {
        p_dyncompr->gain = (((dword) g) * ((dword)(0x8000 - DYNAMIC_COMPRESSOR_NOISE_TIME))) << 1;
        if (((dword)(p_dyncompr->gain)) < (((dword)(p_dyncompr->noise_attenuation)) << 16)) {
            p_dyncompr->gain = (p_dyncompr->noise_attenuation << 16);
        }
    }
    else {
        l = ((((int32) s) * (((dword) m) >> 16)) << 1) + (((int32)(((int32) s) * ((dword)(m & 0xffff)))) >> 15);
        if (l > 32767)
            l = 32767;
        else if (l < -32768)
            l = -32768;
        l = ((dword)((((int32) l) * ((int32) l)) << 1)) - p_dyncompr->energy_threshold;
        m = ((dword)((((int32) s) * ((int32) s)) << 1)) - p_dyncompr->limit_threshold;
        if (m > l) {
            l = m;
        }
        if (l >= 0)
        {
            l = (((dword)((((dword)(l & 0xffff)) * ((dword) g)))) >> 15) +
                ((((dword)(l >> 16)) * ((dword) g)) << 1);
            l = (((dword)((((dword)(l & 0xffff)) * ((dword) p_dyncompr->time_constant)))) >> 15) +
                ((((dword)(l >> 16)) * ((dword) p_dyncompr->time_constant)) << 1);
            p_dyncompr->gain =
                (p_dyncompr->gain > ((dword) l) + (((dword)(p_dyncompr->gain_range)) << 16)) ?
                p_dyncompr->gain - l : (((dword)(p_dyncompr->gain_range)) << 16);
        }
        else
        {
            l = -l;
            l = (((dword)((((dword)(l & 0xffff)) * ((dword) g)))) >> 15) +
                ((((dword)(l >> 16)) * ((dword) g)) << 1);
            l = (((dword)((((dword)(l & 0xffff)) * ((dword) p_dyncompr->time_constant)))) >> 15) +
                ((((dword)(l >> 16)) * ((dword) p_dyncompr->time_constant)) << 1);
            p_dyncompr->gain =
                (((dword) l) < (((dword) 0x8000) << 16) - p_dyncompr->gain) ?
                p_dyncompr->gain + l : (((dword) 0x8000) << 16);
        }
    }
    return s;
}

short dynamic_compressor_exec_for_gain(t_dynamic_compressor *p_dyncompr, short in_sample)
{
    short s;
    int32 l, m;
    word g;

    g = (word)(p_dyncompr->gain >> 16);
    m = ((((dword) g) * ((dword)(p_dyncompr->max_gain >> 16))) << 1) +
        (((dword)(((dword) g) * ((dword)(p_dyncompr->max_gain & 0xffff)))) >> 15);
    l = ((((int32) in_sample) * (((dword) m) >> 16)) << 1) + (((int32)(((int32) in_sample) * ((dword)(m & 0xffff)))) >> 15);
    if (l > 32767)
        l = 32767;
    else if (l < -32768)
        l = -32768;
    if (p_dyncompr->mode == DYNAMIC_COMPRESSOR_MODE_RATIO3) {
        l = ((((int32) l) * (((dword) m) >> 16)) << 1) + (((int32)(((int32) l) * ((dword)(m & 0xffff)))) >> 15);
        if (l > 32767)
            l = 32767;
        else if (l < -32768)
            l = -32768;
    }
    s = (short) l;
    if ((g >= p_dyncompr->noise_attenuation) && (((dword)((((int32) in_sample) * ((int32) in_sample)) << 1)) < p_dyncompr->noise_threshold)) {
        p_dyncompr->gain = (((dword) g) * ((dword)(0x8000 - DYNAMIC_COMPRESSOR_NOISE_TIME))) << 1;
        if (((dword)(p_dyncompr->gain)) < (((dword)(p_dyncompr->noise_attenuation)) << 16)) {
            p_dyncompr->gain = (p_dyncompr->noise_attenuation << 16);
        }
    }
    else {
        l = ((int32)(((int32) s) * ((dword) g))) >> 15;
        l = ((dword)((((int32) l) * ((int32) l)) << 1)) - p_dyncompr->energy_threshold;
        m = ((dword)((((int32) s) * ((int32) s)) << 1)) - p_dyncompr->limit_threshold;
        if (m > l) {
            l = m;
        }
        if (l >= 0)
        {
            l = (((dword)((((dword)(l & 0xffff)) * ((dword) g)))) >> 15) +
                ((((dword)(l >> 16)) * ((dword) g)) << 1);
            l = (((dword)((((dword)(l & 0xffff)) * ((dword) p_dyncompr->time_constant)))) >> 15) +
                ((((dword)(l >> 16)) * ((dword) p_dyncompr->time_constant)) << 1);
            p_dyncompr->gain =
                (p_dyncompr->gain > ((dword) l) + (((dword)(p_dyncompr->gain_range_for_gain)) << 16)) ?
                p_dyncompr->gain - l : (((dword)(p_dyncompr->gain_range_for_gain)) << 16);
        }
        else
        {
            l = -l;
            l = (((dword)((((dword)(l & 0xffff)) * ((dword) g)))) >> 15) +
                ((((dword)(l >> 16)) * ((dword) g)) << 1);
            l = (((dword)((((dword)(l & 0xffff)) * ((dword) p_dyncompr->time_constant)))) >> 15) +
                ((((dword)(l >> 16)) * ((dword) p_dyncompr->time_constant)) << 1);
            p_dyncompr->gain =
                (((dword) l) < (((dword) 0x8000) << 16) - p_dyncompr->gain) ?
                p_dyncompr->gain + l : (((dword) 0x8000) << 16);
        }
    }
    return s;
}


/*---------------------------------------------------------------------------*/
