/*  dexp.c

This file is part of a program that implements a Software-Defined Radio.

Copyright (C) 2018, 2019 Warren Pratt, NR0V

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

The author can be reached by email at

warren@wpratt.com

*/

#include "comm.h"

DEXP pdexp[4];

DELRING calc_delring (int rsize, int size, int delay, double* in, double* out)
{
    DELRING a = (DELRING) malloc0 (sizeof (delring));
    a->rsize = rsize;
    a->size = size;
    a->rdelay = delay;
    a->in = in;
    a->out = out;
    a->ring = (double *) malloc0 (a->rsize * sizeof (complex));
    a->inptr = a->rdelay;
    a->outptr = 0;
    return a;
}

void decalc_delring (DELRING a)
{
    _aligned_free (a->ring);
    _aligned_free (a);
}

void flush_delring (DELRING a)
{
    memset (a->ring, 0, a->rsize * sizeof (complex));
    a->inptr = a->rdelay;
    a->outptr = 0;
}

void xdelring (DELRING a)
{
    int first, second;
    // copy in
    if (a->size > (a->rsize - a->inptr))
    {
        first = a->rsize - a->inptr;
        second = a->size - first;
    }
    else
    {
        first = a->size;
        second = 0;
    }
    memcpy (a->ring + 2 * a->inptr, a->in, first * sizeof (complex));
    memcpy (a->ring, a->in + 2 * first, second * sizeof (complex));
    a->inptr = (a->inptr + a->size) % a->rsize;
    // copy out
    if (a->size > (a->rsize - a->outptr))
    {
        first = a->rsize - a->outptr;
        second = a->size - first;
    }
    else
    {
        first = a->size;
        second = 0;
    }
    memcpy (a->out, a->ring + 2 * a->outptr, first * sizeof (complex));
    memcpy (a->out + 2 * first, a->ring, second * sizeof (complex));
    a->outptr = (a->outptr + a->size) % a->rsize;
}

void calc_slews (DEXP a)
{
    int i;
    double delta, theta;
    delta = PI / (double)a->nattack;
    theta = 0.0;
    for (i = 0; i <= a->nattack; i++)
    {
        a->cattack[i] = a->low_gain + (1.0 - a->low_gain) * 0.5 * (1.0 - cos (theta));
        theta += delta;
    }
    delta = PI / (double)a->ndecay;
    theta = 0.0;
    for (i = 0; i <= a->ndecay; i++)
    {
        a->cdecay[i] = a->low_gain + (1.0 - a->low_gain) * 0.5 * (1.0 + cos (theta));
        theta += delta;
    }
}

void calc_buffs (DEXP a)
{
    a->trigsig   = (double *)malloc0 (2 * a->size * sizeof(complex));   // allow for double-sized output of filter
    a->delsig    = (double *)malloc0 (    a->size * sizeof(complex));
    a->audbuffer = (double *)malloc0 (    a->size * sizeof(complex));
}

void decalc_buffs (DEXP a)
{
    _aligned_free (a->audbuffer);
    _aligned_free (a->delsig);
    _aligned_free (a->trigsig);
}

void calc_dexp (DEXP a)
{
    // trigger signal preparation
    a->avm = exp(-1.0 / (a->rate * a->dettau));
    a->onem_avm = 1.0 - a->avm;
    a->avsig = 0.0;
    // level change
    a->nattack = (int)(a->tattack * a->rate);
    a->ndecay = (int)(a->tdecay * a->rate);
    a->cattack = (double *)malloc0((a->nattack + 1) * sizeof(double));
    a->cdecay = (double *)malloc0((a->ndecay + 1) * sizeof(double));
    a->low_gain = 1.0 / a->exp_ratio;
    calc_slews(a);
    // control
    a->state = 0;
    a->count = 0;
    a->hold_thresh = a->hysteresis_ratio * a->attack_thresh;    // hysteresis ratio < 1.0
    a->nhold = (int)(a->thold * a->rate);
    // vox
    a->vox_count = (int)(a->audelay * a->rate);
    // audio delay
    a->audring = calc_delring ((int)a->rate, a->size, (int)(a->audelay * a->rate), a->audbuffer, a->out);
}

void decalc_dexp (DEXP a)
{
    decalc_delring (a->audring);
    _aligned_free (a->cdecay);
    _aligned_free (a->cattack);
}

void calc_filter (DEXP a)
{
    double* impulse;
    // 2.0 gain on filter is somewhat arbitrarily chosen to get trigger input similar to that without the filter, knowing
    //    that for any reasonable use of the filter there will be a reduction in trigger signal.
    impulse = fir_bandpass (a->nc, a->low_cut, a->high_cut, a->rate, a->wintype, 1, 2.0/(double)(2 * a->size));
    // print_impulse ("scf.txt", a->nc, impulse, 1, 0);
    a->p = create_fircore (a->size, a->in, a->trigsig, a->nc, 1, impulse);
    _aligned_free (impulse);
    a->scdring = calc_delring (a->size + a->nc / 2, a->size, a->nc / 64, a->in, a->delsig);
}

void decalc_filter (DEXP a)
{
    destroy_fircore (a->p);
    decalc_delring (a->scdring);
}

void calc_antivox(DEXP a)
{
    a->antivox_mult = exp(-1.0 / (a->antivox_rate * a->antivox_tau));
    a->antivox_onemmult = 1.0 - a->antivox_mult;
    a->antivox_data = (double *) malloc0 (a->antivox_size * sizeof (complex));
}

void decalc_antivox(DEXP a)
{
    _aligned_free (a->antivox_data);
}

PORT
void create_dexp (int id, int run_dexp, int size, double* in, double* out, int rate, double dettau, double tattack, double tdecay,
    double thold, double exp_ratio, double hyst_ratio, double attack_thresh, int nc, int wtype, double lowcut, double highcut,
    int run_filt, int run_vox, int run_audelay, double audelay, void (__stdcall *pushvox)(int id, int active),
    int antivox_run, int antivox_size, int antivox_rate, double antivox_gain, double antivox_tau)
{
    DEXP a = (DEXP) malloc0 (sizeof (dexp));
    a->id = id;
    a->run_dexp = run_dexp;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = (double)rate;
    a->dettau = dettau;
    a->tattack = tattack;
    a->tdecay = tdecay;
    a->thold = thold;
    a->exp_ratio = exp_ratio;
    a->hysteresis_ratio = hyst_ratio;
    a->attack_thresh = attack_thresh;
    a->nc = nc;
    a->wintype = wtype;
    a->low_cut = lowcut;
    a->high_cut = highcut;
    a->run_filt = run_filt;
    a->run_vox = run_vox;
    a->run_audelay = run_audelay;
    a->audelay = audelay;
    a->pushvox = pushvox;
    a->antivox_run = antivox_run;
    a->antivox_size = antivox_size;
    a->antivox_rate = (double)antivox_rate;
    a->antivox_gain = antivox_gain;
    a->antivox_tau = antivox_tau;
    calc_buffs (a);
    calc_dexp (a);
    calc_filter (a);
    calc_antivox (a);
    InitializeCriticalSectionAndSpinCount(&a->cs_update, 2500);
    pdexp[id] = a;
    return;
}

PORT
void destroy_dexp (int id)
{
    DEXP a = pdexp[id];
    DeleteCriticalSection (&a->cs_update);
    decalc_antivox (a);
    decalc_filter (a);
    decalc_dexp (a);
    decalc_buffs (a);
    _aligned_free (a);
}

PORT
void flush_dexp (int id)
{
    DEXP a = pdexp[id];
    memset (a->audbuffer, 0, a->size * sizeof (complex));
    memset (a->trigsig, 0, a->size * sizeof (complex));
    memset (a->delsig,  0, a->size * sizeof (complex));
    a->avsig = 0.0;
    a->state = 0;
    a->count = 0;
    flush_fircore (a->p);
    flush_delring (a->scdring);
    flush_delring (a->audring);
}

enum _dexpstate
{
    DEXP_LOW,
    DEXP_ATTACK,
    DEXP_HIGH,
    DEXP_HOLD,
    DEXP_DECAY
};

PORT
void xdexp (int id)
{
    DEXP a = pdexp[id];
    int i;
    double sig, gain, asig;
    double max = 0.0;
    EnterCriticalSection (&a->cs_update);

    // ******* BEGIN SIDE-CHANNEL FILTER *******
    if (a->run_filt)
    {
        xdelring (a->scdring);      // input is 'a->in'; output is 'a->delsig'
        xfircore (a->p);            // input is 'a->in'; output is 'a->trigsig'
    }
    else
    {
        memcpy (a->delsig,  a->in, a->size * sizeof (complex));
        memcpy (a->trigsig, a->in, a->size * sizeof (complex));
    }
    // ******* END SIDE-CHANNEL FILTER *******

    // ******* CALCULATE ANTIVOX LEVEL *******
    if (a->state == DEXP_LOW && a->antivox_new != 0)
    {
        // if VOX is currently NOT triggered, and, if we have new antivox data to process
        for (i = 0; i < a->antivox_size; i++)
        {
            sig = sqrt (a->antivox_data[2 * i + 0] * a->antivox_data[2 * i + 0] + a->antivox_data[2 * i + 1] * a->antivox_data[2 * i + 1]);
            a->antivox_level = a->antivox_mult * a->antivox_level + a->antivox_onemmult * sig;
        }
        // set the new_data flag to zero
        a->antivox_new = 0;
    }
    // ******* END CALCULATE ANTIVOX LEVEL *******

    // ******* BEGIN DEXP *******
    // uses 'a->trigsig' as trigger signal; uses 'a->delsig' as audio input
    // 'a->audbuffer' is audio output
    // DEXP code runs continuously so it can be used to trigger VOX also.
    for (i = 0; i < a->size; i++)
    {
        sig = sqrt (a->trigsig[2 * i + 0] * a->trigsig[2 * i + 0] + a->trigsig[2 * i + 1] * a->trigsig[2 * i + 1]);
        a->avsig = a->avm * a->avsig + a->onem_avm * sig;
        if (a->avsig > max)  max = a->avsig;
        switch (a->state)
        {
        case DEXP_LOW:
            if (a->antivox_run)
                asig = a->avsig - a->antivox_gain * a->antivox_level;
            else
                asig = a->avsig;
            if (asig > a->attack_thresh)
            {
                a->state = DEXP_ATTACK;
                a->count = a->nattack;
            }
            a->audbuffer[2 * i + 0] = a->low_gain * a->delsig[2 * i + 0];
            a->audbuffer[2 * i + 1] = a->low_gain * a->delsig[2 * i + 1];

            // ******* BEGIN VOX *******
            // If we're going to attack, turn on VOX immediately.
            // Prepare 'vox_count' for the next turnoff too.
            if (a->run_vox && a->state == DEXP_ATTACK)
            {
                (a->pushvox)(a->id, 1);
                // Set vox_count for delay IF the audio delay is also enabled.
                if (a->run_audelay)
                    a->vox_count = (int)(a->audelay * a->rate);
                else
                    a->vox_count = 1;
            }
            // If we're sitting in this state and the delayed vox count expires, turn OFF VOX.
            else if (a->run_vox && --(a->vox_count) == 0)
                (a->pushvox)(a->id, 0);
            // Don't let 'vox_count' keep counting down.
            else if (a->vox_count < 0)
                a->vox_count = 0;
            // ******* END VOX *******

            break;
        case DEXP_ATTACK:
            gain = a->cattack[a->nattack - a->count];
            a->audbuffer[2 * i + 0] = a->delsig[2 * i + 0] * gain;
            a->audbuffer[2 * i + 1] = a->delsig[2 * i + 1] * gain;
            if (a->count-- == 0)
                a->state = DEXP_HIGH;
            break;
        case DEXP_HIGH:
            if (a->avsig < a->hold_thresh)
            {
                a->state = DEXP_HOLD;
                a->count = a->nhold;
            }
            a->audbuffer[2 * i + 0] = a->delsig[2 * i + 0];
            a->audbuffer[2 * i + 1] = a->delsig[2 * i + 1];
            break;
        case DEXP_HOLD:
            a->audbuffer[2 * i + 0] = a->delsig[2 * i + 0];
            a->audbuffer[2 * i + 1] = a->delsig[2 * i + 1];
            if (a->avsig > a->attack_thresh)
                a->state = DEXP_HIGH;
            else if (a->count-- == 0)
            {
                a->state = DEXP_DECAY;
                a->count = a->ndecay;
            }
            break;
        case DEXP_DECAY:
            gain = a->cdecay[a->ndecay - a->count];
            a->audbuffer[2 * i + 0] = a->delsig[2 * i + 0] * gain;
            a->audbuffer[2 * i + 1] = a->delsig[2 * i + 1] * gain;
            if (a->count-- == 0)
                a->state = DEXP_LOW;
            break;
        }
    }
    a->peak = max;
    // If DEXP functionality is set to OFF, copy its input to overwrite its output.
    if (!a->run_dexp)
        memcpy (a->audbuffer, a->delsig, a->size * sizeof (complex));
    // ******* END DEXP *******

    // ******* BEGIN AUDIO DELAY *******
    if (a->run_audelay)
        xdelring (a->audring);      // uses 'a->audbuffer' as audio input; uses 'a->out' as audio output
    else
        memcpy (a->out, a->audbuffer, a->size * sizeof (complex));
    // ******* END AUDIO DELAY *******

    LeaveCriticalSection (&a->cs_update);
}

PORT
void SendCBPushDexpVox (int id, void (__stdcall *pushvox)(int id, int active))
{
    // Call to set the address of the callback to operate VOX.
    DEXP a = pdexp[id];
    a->pushvox = pushvox;
}

PORT
void SetDEXPRun (int id, int run)
{
    // run != 0, puts dexp in the audio processing path; otherwise, it's only used to trigger VOX
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    a->run_dexp = run;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPSize (int id, int size)
{
    // There are some constraints on the input/output buffer sizes.
    //   * must be a power-of-two
    //   * must be less than or equal to 'nc', the number of filter coefficients, which is also a power-of-two
    //   * must be less than 'rate' samples because of the sizing of 'audring'
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_filter (a);
    decalc_dexp (a);
    decalc_buffs (a);
    a->size = size;
    calc_buffs (a);
    calc_dexp (a);
    calc_filter (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPIOBuffers (int id, double* in, double* out)
{
    // Sets the input/output buffers.  They can be the same.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_filter (a);
    decalc_dexp (a);
    a->in = in;
    a->out = out;
    calc_dexp (a);
    calc_filter (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPRate (int id, double rate)
{
    // Sets the sample rate.
    // This is used for timing and filter calculations as well as sizing 'audring'.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_filter (a);
    decalc_dexp (a);
    a->rate = rate;
    calc_dexp (a);
    calc_filter (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPDetectorTau (int id, double tau)
{
    // Time-constant for smoothing the signal for detection (seconds).
    // 0.01 seconds is a good starting point to try.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_dexp (a);
    a->dettau = tau;
    calc_dexp (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPAttackTime (int id, double time)
{
    // Set attack time, seconds.
    // 0.002 - 0.100 should be a good range.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_dexp (a);
    a->tattack = time;
    calc_dexp (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPReleaseTime (int id, double time)
{
    // Set release time, seconds.
    // 0.002 - 0.999 should be a good range.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_dexp (a);
    a->tdecay = time;
    calc_dexp (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPHoldTime (int id, double time)
{
    // Set hold time, seconds.
    // 0.000 - 2.000 should be a good range.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_dexp (a);
    a->thold = time;
    calc_dexp (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPExpansionRatio (int id, double ratio)
{
    // Set expansion ratio.  High_gain = 1.0; Low_gain = 1.0/exp_ratio.
    // Range of 1.0 - 30.0 should be good.  Could use dB:  0.0 - 30.0dB.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_dexp (a);
    a->exp_ratio = ratio;
    calc_dexp (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPHysteresisRatio (int id, double ratio)
{
    // Set Hysteresis Ratio.  Hold_thresh = hysteresis_ratio * Attack_thresh.
    // Expose to operator in dB:  0.0dB - 9.9dB should be good (1.000 - 0.320).
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_dexp (a);
    a->hysteresis_ratio = ratio;
    calc_dexp (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPAttackThreshold (int id, double thresh)
{
    // Set attack threshold.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_dexp (a);
    a->attack_thresh = thresh;
    calc_dexp (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPFilterTaps (int id, int taps)
{
    // Set number of taps.  Must be a power of two and an even multiple of 'size'.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_filter (a);
    a->nc = taps;
    calc_filter (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPWindowType (int id, int type)
{
    // Set filter window type.
    //   * 0 - 4-term Blackman-Harris.
    //   * 1 - 7-term Blackman-Harris.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_filter (a);
    a->wintype = type;
    calc_filter (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPLowCut (int id, double lowcut)
{
    // Set side-channel filter low_cut (Hertz).
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_filter (a);
    a->low_cut = lowcut;
    calc_filter (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPHighCut (int id, double highcut)
{
    // Set side-channel filter high_cut (Hertz).
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_filter (a);
    a->high_cut = highcut;
    calc_filter (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPRunSideChannelFilter (int id, int run)
{
    // Turn OFF/ON the side-channel filter and its compensating delay.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    a->run_filt = run;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPRunVox (int id, int run)
{
    // Turn OFF/ON calls to 'pushvox(...)'.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    a->run_vox = run;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPRunAudioDelay (int id, int run)
{
    // Turn OFF/ON audio delay line.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    a->run_audelay = run;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetDEXPAudioDelay (int id, double delay)
{
    // Set the audio delay, seconds.
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_dexp (a);
    a->audelay = delay;
    calc_dexp (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void GetDEXPPeakSignal (int id, double* peak)
{
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    *peak = a->peak;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetAntiVOXRun (int id, int run)
{
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    a->antivox_run = run;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetAntiVOXSize (int id, int size)
{
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_antivox(a);
    a->antivox_size = size;
    calc_antivox(a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetAntiVOXRate (int id, double rate)
{
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_antivox(a);
    a->antivox_rate = rate;
    calc_antivox(a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetAntiVOXGain (int id, double gain)
{
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    a->antivox_gain = gain;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetAntiVOXDetectorTau (int id, double tau)
{
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    decalc_antivox (a);
    a->antivox_tau = tau;
    calc_antivox (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SendAntiVOXData (int id, int nsamples, double* data)
{
    // note:  'nsamples' is not used as it has been previously specified
    DEXP a = pdexp[id];
    EnterCriticalSection (&a->cs_update);
    memcpy (a->antivox_data, data, a->antivox_size * sizeof (complex));
    a->antivox_new = 1;
    LeaveCriticalSection (&a->cs_update);
}
