/*---------------------------------------------------------------------------*/
/*
  File: peaklim.cpp

  Peak limiter.
*/
/*---------------------------------------------------------------------------*/

#include "platform.h"
#include "peaklim.h"

#define FILE_ "peaklim.cpp"

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

static word hamming_table[PEAK_LIMITER_TIME_CONSTANT] =
{
        0*2,    51*2,   202*2,   453*2,   802*2,  1247*2,  1786*2,  2414*2,
     3129*2,  3926*2,  4799*2,  5743*2,  6754*2,  7823*2,  8946*2, 10114*2,
    11321*2, 12559*2, 13821*2, 15099*2, 16384*2, 17669*2, 18947*2, 20209*2,
    21447*2, 22654*2, 23822*2, 24945*2, 26014*2, 27025*2, 27969*2, 28842*2,
    29639*2, 30354*2, 30982*2, 31521*2, 31966*2, 32315*2, 32566*2, 32717*2,
};


void peak_limiter_init(t_peak_limiter *p_peaklim, short limit, const short *equalizer_coeffs, word equalizer_length)
{
    word i;

    p_peaklim->limit = limit;
    p_peaklim->equalizer_coeffs = equalizer_coeffs;
    p_peaklim->equalizer_length = equalizer_length;
    for (i = 0; i < PEAK_LIMITER_TIME_CONSTANT; i++) {
        p_peaklim->tail[i] = 0x80000000;
    }
    for (i = 0; i < PEAK_LIMITER_TIME_CONSTANT; i++) {
        p_peaklim->queue[i] = 0;
    }
    p_peaklim->pos = 0;
}

short peak_limiter_exec(t_peak_limiter *p_peaklim, short in_sample)
{
    int32 l;
    short out_sample, s;
    word i, j, n, w, pos, lag;

    out_sample = (short)(p_peaklim->queue[p_peaklim->pos] >> 16);
    p_peaklim->queue[p_peaklim->pos] = ((((int32)(in_sample)) * (p_peaklim->tail[p_peaklim->pos] >> 16)) << 1) +
        (((int32)(((int32)(in_sample)) * ((dword)(p_peaklim->tail[p_peaklim->pos] & 0xffff)))) >> 15);
    p_peaklim->tail[p_peaklim->pos] = 0x80000000;
    pos = (p_peaklim->pos < PEAK_LIMITER_TIME_CONSTANT - 1) ? p_peaklim->pos + 1 : 0;
    l = 0;
    if (p_peaklim->equalizer_length <= 2 * PEAK_LIMITER_LAG + 1) {
        lag = (p_peaklim->equalizer_length + 1) / 2;
        n = p_peaklim->equalizer_length;
        i = 0;
        j = ((pos >= n) ? pos : pos + PEAK_LIMITER_TIME_CONSTANT) - n;
    }
    else {
        lag = PEAK_LIMITER_LAG;
        n = ((p_peaklim->equalizer_length + 1) / 2) + PEAK_LIMITER_LAG;
        if (n >= PEAK_LIMITER_TIME_CONSTANT) {
            i = n - PEAK_LIMITER_TIME_CONSTANT;
            n = PEAK_LIMITER_TIME_CONSTANT;
            j = pos;
        }
        else {
            i = 0;
            j = ((pos >= n) ? pos : pos + PEAK_LIMITER_TIME_CONSTANT) - n;
        }
    }
    if (PEAK_LIMITER_TIME_CONSTANT - j <= n) {
        do {
            l += (p_peaklim->queue[j++] >> 16) * ((int32)(p_peaklim->equalizer_coeffs[i++]));
        }
        while (j < PEAK_LIMITER_TIME_CONSTANT);
        j = 0;
    }
    while (j < pos) {
        l += (p_peaklim->queue[j++] >> 16) * ((int32)(p_peaklim->equalizer_coeffs[i++]));
    }
    if (l > 0x3fffffffL)
        s = 32767;
    else if (l < -0x40000000L)
        s = -32768;
    else
        s = (short)(l >> 15);
    if ((s > p_peaklim->limit) || (s < -p_peaklim->limit)) {
        w = (s >= 0) ? (word)s : (word)-s;
#if (PEAK_LIMITER_LAG != 0)
        dword q = (((dword)w) * ((dword)(hamming_table[PEAK_LIMITER_TIME_CONSTANT - lag]))) >> 16;
        w = (((word)(w - p_peaklim->limit)) >= ((word)(q << 1))) ? 0xffff : (word)(((((dword)(w - p_peaklim->limit)) << 16) / q) >> 1);
#else
        w = (word)(((((dword)(w - p_peaklim->limit)) << 16) / ((dword)w)) >> 1);
#endif
        i = 1;
        j = pos;
        if (pos != 0) {
            do {
                p_peaklim->queue[j] -= ((p_peaklim->queue[j] >> 16) * ((dword)((((dword)(hamming_table[i++])) * ((dword)w))) >> 15));
                j++;
            }
            while (j < PEAK_LIMITER_TIME_CONSTANT);
            j = 0;
        }
        while (j < p_peaklim->pos) {
            p_peaklim->queue[j] -= ((p_peaklim->queue[j] >> 16) * ((dword)((((dword)(hamming_table[i++])) * ((dword)w))) >> 15));
            j++;
        }
        p_peaklim->queue[j] -= ((p_peaklim->queue[j] >> 16) * ((dword)w)) << 1;
        j = pos;
        if (pos != 0) {
            do {
                p_peaklim->tail[j] -= ((p_peaklim->tail[j] >> 16) * ((dword)((((dword)(hamming_table[--i])) * ((dword)w))) >> 15));
                j++;
            }
            while (j < PEAK_LIMITER_TIME_CONSTANT);
            j = 0;
        }
        while (j < p_peaklim->pos) {
            p_peaklim->tail[j] -= ((p_peaklim->tail[j] >> 16) * ((dword)((((dword)(hamming_table[--i])) * ((dword)w))) >> 15));
            j++;
        }
    }
    p_peaklim->pos = pos;
    return out_sample;
}


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