/*  ssql.c

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

Copyright (C) 2023 Warren Pratt, NR0V
Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel

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@pratt.one

*/

#include "comm.hpp"
#include "cblock.hpp"
#include "ssql.hpp"
#include "dbqlp.hpp"
#include "RXA.hpp"

namespace WDSP {

/********************************************************************************************************
*                                                                                                       *
*                                      Frequency to Voltage Converter                                   *
*                                                                                                       *
********************************************************************************************************/

FTOV::FTOV(
    int _run,
    int _size,
    int _rate,
    int _rsize,
    double _fmax,
    float* _in,
    float* _out
)
{
    run = _run;
    size = _size;
    rate = _rate;
    rsize = _rsize;
    fmax = _fmax;
    in = _in;
    out = _out;
    eps = 0.01;
    ring.resize(rsize); // (int*) malloc0 (rsize * sizeof (int));
    rptr = 0;
    inlast = 0.0;
    rcount = 0;
    div = fmax * 2.0 * rsize / rate;                // fmax * 2 = zero-crossings/sec
                                                                // rsize / rate = sec of data in ring
                                                                // product is # zero-crossings in ring at fmax
}

void FTOV::flush()
{
    std::fill(ring.begin(), ring.end(), 0);
    rptr = 0;
    rcount = 0;
    inlast = 0.0;
}

void FTOV::execute()
{
    // 'ftov' does frequency to voltage conversion looking only at zero crossings of an
    //     AC (DC blocked) signal, i.e., ignoring signal amplitude.
    if (run)
    {
        if (ring[rptr] == 1)                              // if current ring location is a '1' ...
        {
            rcount--;                                        // decrement the count
            ring[rptr] = 0;                               // set the location to '0'
        }
        if ((inlast * in[0] < 0.0) &&                     // different signs mean zero-crossing
            (fabs (inlast - in[0]) > eps))
        {
            ring[rptr] = 1;                               // set the ring location to '1'
            rcount++;                                        // increment the count
        }
        if (++rptr == rsize) rptr = 0;                 // increment and wrap the pointer as needed
        out[0] = std::min (1.0, (double)rcount / div);      // calculate the output sample
        inlast = in[size - 1];                         // save the last input sample for next buffer
        for (int i = 1; i < size; i++)
        {
            if (ring[rptr] == 1)                          // if current ring location is '1' ...
            {
                rcount--;                                    // decrement the count
                ring[rptr] = 0;                           // set the location to '0'
            }
            if ((in[i - 1] * in[i] < 0.0) &&              // different signs mean zero-crossing
                (fabs (in[i - 1] - in[i]) > eps))
            {
                ring[rptr] = 1;                           // set the ring location to '1'
                rcount++;                                    // increment the count
            }
            if (++rptr == rsize) rptr = 0;             // increment and wrap the pointer as needed
            out[i] = std::min(1.0, (double)rcount / div);   // calculate the output sample
        }
    }
}
/*******************************************************************************************************/
/********************************** END Frequency to Voltage Converter *********************************/



void SSQL::compute_ssql_slews(SSQL *a)
{
    int i;
    double delta, theta;
    delta = PI / (double)a->ntup;
    theta = 0.0;
    for (i = 0; i <= a->ntup; i++)
    {
        a->cup[i] = a->muted_gain + (1.0 - a->muted_gain) * 0.5 * (1.0 - cos(theta));
        theta += delta;
    }
    delta = PI / (double)a->ntdown;
    theta = 0.0;
    for (i = 0; i <= a->ntdown; i++)
    {
        a->cdown[i] = a->muted_gain + (1.0 - a->muted_gain) * 0.5 * (1.0 + cos(theta));
        theta += delta;
    }
}

void SSQL::calc_ssql (SSQL *a)
{
    a->b1 = new float[a->size * 2]; // (float*) malloc0 (a->size * sizeof (complex));
    a->dcbl = new CBL(1, a->size, a->in, a->b1, 0, a->rate, 0.02);
    a->ibuff = new float[a->size]; // (float*) malloc0 (a->size * sizeof (float));
    a->ftovbuff = new float[a->size]; // (float*) malloc0(a->size * sizeof (float));
    a->cvtr = new FTOV(1, a->size, a->rate, a->ftov_rsize, a->ftov_fmax, a->ibuff, a->ftovbuff);
    a->lpbuff = new float[a->size]; // (float*) malloc0 (a->size * sizeof (float));
    a->filt = new DBQLP(1, a->size, a->ftovbuff, a->lpbuff, a->rate, 11.3, 1.0, 1.0, 1);
    a->wdbuff = new int[a->size]; // (int*) malloc0 (a->size * sizeof (int));
    a->tr_signal = new int[a->size]; // (int*) malloc0 (a->size * sizeof (int));
    // window detector
    a->wdmult = exp (-1.0 / (a->rate * a->wdtau));
    a->wdaverage = 0.0;
    // trigger
    a->tr_voltage = a->tr_thresh;
    a->mute_mult = 1.0 - exp (-1.0 / (a->rate * a->tr_tau_mute));
    a->unmute_mult = 1.0 - exp (-1.0 / (a->rate * a->tr_tau_unmute));
    // level change
    a->ntup = (int)(a->tup * a->rate);
    a->ntdown = (int)(a->tdown * a->rate);
    a->cup = new float[a->ntup + 1]; // (float*) malloc0 ((a->ntup + 1) * sizeof (float));
    a->cdown = new float[a->ntdown + 1]; // (float*) malloc0 ((a->ntdown + 1) * sizeof (float));
    compute_ssql_slews (a);
    // control
    a->state = 0;
    a->count = 0;
}

void SSQL::decalc_ssql (SSQL *a)
{
    delete[] (a->tr_signal);
    delete[] (a->wdbuff);
    delete (a->filt);
    delete[] (a->lpbuff);
    delete (a->cvtr);
    delete[] (a->ftovbuff);
    delete[] (a->ibuff);
    delete (a->dcbl);
    delete[] (a->b1);
    delete[] (a->cdown);
    delete[] (a->cup);
}

SSQL* SSQL::create_ssql (
    int run,
    int size,
    float* in,
    float* out,
    int rate,
    double tup,
    double tdown,
    double muted_gain,
    double tau_mute,
    double tau_unmute,
    double wthresh,
    double tr_thresh,
    int rsize,
    double fmax
)
{
    SSQL *a = new SSQL;
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->tup = tup;
    a->tdown = tdown;
    a->muted_gain = muted_gain;
    a->tr_tau_mute = tau_mute;
    a->tr_tau_unmute = tau_unmute;
    a->wthresh = wthresh;           // PRIMARY SQUELCH THRESHOLD CONTROL
    a->tr_thresh = tr_thresh;       // value between tr_ss_unmute and tr_ss_mute, default = 0.8197
    a->tr_ss_mute = 1.0;
    a->tr_ss_unmute = 0.3125;
    a->wdtau = 0.5;
    a->ftov_rsize = rsize;
    a->ftov_fmax = fmax;
    calc_ssql (a);
    return a;
}

void SSQL::destroy_ssql (SSQL *a)
{
    decalc_ssql (a);
    delete (a);
}

void SSQL::flush_ssql (SSQL *a)
{

    std::fill(a->b1, a->b1 + a->size * 2, 0);
    a->dcbl->flush();
    memset (a->ibuff, 0, a->size * sizeof (float));
    memset (a->ftovbuff, 0, a->size * sizeof (float));
    a->cvtr->flush();
    memset (a->lpbuff, 0, a->size * sizeof (float));
    a->filt->flush();
    memset (a->wdbuff, 0, a->size * sizeof (int));
    memset (a->tr_signal, 0, a->size * sizeof (int));
}

enum _ssqlstate
{
    MUTED,
    INCREASE,
    UNMUTED,
    DECREASE
};

void SSQL::xssql (SSQL *a)
{
    if (a->run)
    {
        a->dcbl->execute();                                         // dc block the input signal
        for (int i = 0; i < a->size; i++)                       // extract 'I' component
            a->ibuff[i] = a->b1[2 * i];
        a->cvtr->execute();                                        // convert frequency to voltage, ignoring amplitude
        // WriteAudioWDSP(20.0, a->rate, a->size, a->ftovbuff, 4, 0.99);
        a->filt->execute();                                       // low-pass filter
        // WriteAudioWDSP(20.0, a->rate, a->size, a->lpbuff, 4, 0.99);
        // calculate the output of the window detector for each sample
        for (int i = 0; i < a->size; i++)
        {
            a->wdaverage = a->wdmult * a->wdaverage + (1.0 - a->wdmult) * a->lpbuff[i];
            if ((a->lpbuff[i] - a->wdaverage) > a->wthresh || (a->wdaverage - a->lpbuff[i]) > a->wthresh)
                a->wdbuff[i] = 0;       // signal unmute
            else
                a->wdbuff[i] = 1;       // signal mute
        }
        // calculate the trigger signal for each sample
        for (int i = 0; i < a->size; i++)
        {
            if (a->wdbuff[i] == 0)
                a->tr_voltage += (a->tr_ss_unmute - a->tr_voltage) * a->unmute_mult;
            if (a->wdbuff[i] == 1)
                a->tr_voltage += (a->tr_ss_mute - a->tr_voltage) * a->mute_mult;
            if (a->tr_voltage > a->tr_thresh) a->tr_signal[i] = 0;  // muted
            else                              a->tr_signal[i] = 1;  // unmuted
        }
        // execute state machine; calculate audio output
        for (int i = 0; i < a->size; i++)
        {
            switch (a->state)
            {
            case MUTED:
                if (a->tr_signal[i] == 1)
                {
                    a->state = INCREASE;
                    a->count = a->ntup;
                }
                a->out[2 * i + 0] = a->muted_gain * a->in[2 * i + 0];
                a->out[2 * i + 1] = a->muted_gain * a->in[2 * i + 1];
                break;
            case INCREASE:
                a->out[2 * i + 0] = a->in[2 * i + 0] * a->cup[a->ntup - a->count];
                a->out[2 * i + 1] = a->in[2 * i + 1] * a->cup[a->ntup - a->count];
                if (a->count-- == 0)
                    a->state = UNMUTED;
                break;
            case UNMUTED:
                if (a->tr_signal[i] == 0)
                {
                    a->state = DECREASE;
                    a->count = a->ntdown;
                }
                a->out[2 * i + 0] = a->in[2 * i + 0];
                a->out[2 * i + 1] = a->in[2 * i + 1];
                break;
            case DECREASE:
                a->out[2 * i + 0] = a->in[2 * i + 0] * a->cdown[a->ntdown - a->count];
                a->out[2 * i + 1] = a->in[2 * i + 1] * a->cdown[a->ntdown - a->count];
                if (a->count-- == 0)
                    a->state = MUTED;
                break;
            }
        }
    }
    else if (a->in != a->out)
        std::copy(a->in, a->in + a->size * 2, a->out);
}

void SSQL::setBuffers_ssql (SSQL *a, float* in, float* out)
{
    decalc_ssql (a);
    a->in = in;
    a->out = out;
    calc_ssql (a);
}

void SSQL::setSamplerate_ssql (SSQL *a, int rate)
{
    decalc_ssql (a);
    a->rate = rate;
    calc_ssql (a);
}

void SSQL::setSize_ssql (SSQL *a, int size)
{
    decalc_ssql (a);
    a->size = size;
    calc_ssql (a);
}

/********************************************************************************************************
*                                                                                                       *
*                                           RXA Properties                                              *
*                                                                                                       *
********************************************************************************************************/

void SSQL::SetSSQLRun (RXA& rxa, int run)
{
    rxa.ssql->run = run;
}

void SSQL::SetSSQLThreshold (RXA& rxa, double threshold)
{
    // 'threshold' should be between 0.0 and 1.0
    // WU2O testing:  0.16 is a good default for 'threshold'; => 0.08 for 'wthresh'
    rxa.ssql->wthresh = threshold / 2.0;
}

void SSQL::SetSSQLTauMute (RXA& rxa, double tau_mute)
{
    // reasonable (wide) range is 0.1 to 2.0
    // WU2O testing:  0.1 is good default value
    SSQL *a = rxa.ssql;
    a->tr_tau_mute = tau_mute;
    a->mute_mult = 1.0 - exp (-1.0 / (a->rate * a->tr_tau_mute));
}

void SSQL::SetSSQLTauUnMute (RXA& rxa, double tau_unmute)
{
    // reasonable (wide) range is 0.1 to 1.0
    // WU2O testing:  0.1 is good default value
    SSQL *a = rxa.ssql;
    a->tr_tau_unmute = tau_unmute;
    a->unmute_mult = 1.0 - exp (-1.0 / (a->rate * a->tr_tau_unmute));
}

} // namespace WDSP