/*  nbp.c

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

Copyright (C) 2015, 2016 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@wpratt.com

*/

#include "comm.hpp"
#include "fir.hpp"
#include "firmin.hpp"
#include "snb.hpp"
#include "nbp.hpp"
#include "RXA.hpp"

namespace WDSP {

/********************************************************************************************************
*                                                                                                       *
*                                           Notch Database                                              *
*                                                                                                       *
********************************************************************************************************/

 NOTCHDB* NOTCHDB::create_notchdb (int master_run, int maxnotches)
{
    NOTCHDB *a = new NOTCHDB;
    a->master_run = master_run;
    a->maxnotches = maxnotches;
    a->nn = 0;
    a->fcenter = new float[a->maxnotches]; // (float *) malloc0 (a->maxnotches * sizeof (float));
    a->fwidth  = new float[a->maxnotches]; // (float *) malloc0 (a->maxnotches * sizeof (float));
    a->nlow    = new float[a->maxnotches]; // (float *) malloc0 (a->maxnotches * sizeof (float));
    a->nhigh   = new float[a->maxnotches]; // (float *) malloc0 (a->maxnotches * sizeof (float));
    a->active  = new int[a->maxnotches]; // (int    *) malloc0 (a->maxnotches * sizeof (int   ));
    return a;
}

void NOTCHDB::destroy_notchdb (NOTCHDB *b)
{
    delete[] (b->active);
    delete[] (b->nhigh);
    delete[] (b->nlow);
    delete[] (b->fwidth);
    delete[] (b->fcenter);
}

/********************************************************************************************************
*                                                                                                       *
*                                       Notched Bandpass Filter                                         *
*                                                                                                       *
********************************************************************************************************/

float* NBP::fir_mbandpass (int N, int nbp, float* flow, float* fhigh, float rate, float scale, int wintype)
{
    int i, k;
    float* impulse = new float[N * 2]; // (float *) malloc0 (N * sizeof (complex));
    float* imp;
    for (k = 0; k < nbp; k++)
    {
        imp = FIR::fir_bandpass (N, flow[k], fhigh[k], rate, wintype, 1, scale);
        for (i = 0; i < N; i++)
        {
            impulse[2 * i + 0] += imp[2 * i + 0];
            impulse[2 * i + 1] += imp[2 * i + 1];
        }
        delete[] (imp);
    }
    return impulse;
}

float NBP::min_notch_width (NBP *a)
{
    float min_width;
    switch (a->wintype)
    {
    case 0:
        min_width = 1600.0 / (a->nc / 256) * (a->rate / 48000);
        break;
    case 1:
        min_width = 2200.0 / (a->nc / 256) * (a->rate / 48000);
        break;
    }
    return min_width;
}

int NBP::make_nbp (
    int nn,
    int* active,
    float* center,
    float* width,
    float* nlow,
    float* nhigh,
    float minwidth,
    int autoincr,
    float flow,
    float fhigh,
    float* bplow,
    float* bphigh,
    int* havnotch
)
{
    int nbp;
    int nnbp, adds;
    int i, j, k;
    float nl, nh;
    int* del = new int[1024]; // (int *) malloc0 (1024 * sizeof (int));
    if (fhigh > flow)
    {
        bplow[0]  = flow;
        bphigh[0] = fhigh;
        nbp = 1;
    }
    else
    {
        nbp = 0;
        return nbp;
    }
    *havnotch = 0;
    for (k = 0; k < nn; k++)
    {
        if (autoincr && width[k] < minwidth)
        {
            nl = center[k] - 0.5 * minwidth;
            nh = center[k] + 0.5 * minwidth;
        }
        else
        {
            nl = nlow[k];
            nh = nhigh[k];
        }
        if (active[k] && (nh > flow && nl < fhigh))
        {
            *havnotch = 1;
            adds = 0;
            for (i = 0; i < nbp; i++)
            {
                if (nh > bplow[i] && nl < bphigh[i])
                {
                    if (nl <= bplow[i] && nh >= bphigh[i])
                    {
                        del[i] = 1;
                    }
                    else if (nl > bplow[i] && nh < bphigh[i])
                    {

                        bplow[nbp + adds] = nh;
                        bphigh[nbp + adds] = bphigh[i];
                        bphigh[i] = nl;
                        adds++;
                    }
                    else if (nl <= bplow[i] && nh > bplow[i])
                    {
                        bplow[i] = nh;
                    }
                    else if (nl < bphigh[i] && nh >= bphigh[i])
                    {
                        bphigh[i] = nl;
                    }
                }
            }
            nbp += adds;
            nnbp = nbp;
            for (i = 0; i < nbp; i++)
            {
                if (del[i] == 1)
                {
                    nnbp--;
                    for (j = i; j < nnbp; j++)
                    {
                        bplow[j] = bplow[j + 1];
                        bphigh[j] = bphigh[j + 1];
                    }
                    del[i] = 0;
                }
            }
            nbp = nnbp;
        }
    }
    delete[] (del);
    return nbp;
}

void NBP::calc_nbp_lightweight (NBP *a)
{   // calculate and set new impulse response; used when changing tune freq or shift freq
    int i;
    float fl, fh;
    float offset;
    NOTCHDB *b = a->ptraddr;
    if (a->fnfrun)
    {
        offset = b->tunefreq + b->shift;
        fl = a->flow  + offset;
        fh = a->fhigh + offset;
        a->numpb = make_nbp (
            b->nn,
            b->active,
            b->fcenter,
            b->fwidth,
            b->nlow,
            b->nhigh,
            min_notch_width (a),
            a->autoincr,
            fl,
            fh,
            a->bplow,
            a->bphigh,
            &a->havnotch
        );
        // when tuning, no need to recalc filter if there were not and are not any notches in passband
        if (a->hadnotch || a->havnotch)
        {
            for (i = 0; i < a->numpb; i++)
            {
                a->bplow[i]  -= offset;
                a->bphigh[i] -= offset;
            }
            a->impulse = fir_mbandpass (a->nc, a->numpb, a->bplow, a->bphigh,
                a->rate, a->gain / (float)(2 * a->size), a->wintype);
            FIRCORE::setImpulse_fircore (a->p, a->impulse, 1);
            // print_impulse ("nbp.txt", a->size + 1, impulse, 1, 0);
            delete[](a->impulse);
        }
        a->hadnotch = a->havnotch;
    }
    else
        a->hadnotch = 1;
}

void NBP::calc_nbp_impulse (NBP *a)
{   // calculates impulse response; for create_fircore() and parameter changes
    int i;
    float fl, fh;
    float offset;
    NOTCHDB *b = a->ptraddr;
    if (a->fnfrun)
    {
        offset = b->tunefreq + b->shift;
        fl = a->flow  + offset;
        fh = a->fhigh + offset;
        a->numpb = make_nbp (
            b->nn,
            b->active,
            b->fcenter,
            b->fwidth,
            b->nlow,
            b->nhigh,
            min_notch_width (a),
            a->autoincr,
            fl,
            fh,
            a->bplow,
            a->bphigh,
            &a->havnotch
        );
        for (i = 0; i < a->numpb; i++)
        {
            a->bplow[i]  -= offset;
            a->bphigh[i] -= offset;
        }
        a->impulse = fir_mbandpass (
            a->nc,
            a->numpb,
            a->bplow,
            a->bphigh,
            a->rate,
            a->gain / (float)(2 * a->size),
            a->wintype
        );
    }
    else
    {
        a->impulse = FIR::fir_bandpass(
            a->nc,
            a->flow,
            a->fhigh,
            a->rate,
            a->wintype,
            1,
            a->gain / (float)(2 * a->size)
        );
    }
}

NBP* NBP::create_nbp(
    int run,
    int fnfrun,
    int position,
    int size,
    int nc,
    int mp,
    float* in,
    float* out,
    float flow,
    float fhigh,
    int rate,
    int wintype,
    float gain,
    int autoincr,
    int maxpb,
    NOTCHDB* ptraddr
)
{
    NBP *a = new NBP;
    a->run = run;
    a->fnfrun = fnfrun;
    a->position = position;
    a->size = size;
    a->nc = nc;
    a->mp = mp;
    a->rate = (float)rate;
    a->wintype = wintype;
    a->gain = gain;
    a->in = in;
    a->out = out;
    a->autoincr = autoincr;
    a->flow = flow;
    a->fhigh = fhigh;
    a->maxpb = maxpb;
    a->ptraddr = ptraddr;
    a->bplow   = new float[a->maxpb]; // (float *) malloc0 (a->maxpb * sizeof (float));
    a->bphigh  = new float[a->maxpb]; // (float *) malloc0 (a->maxpb * sizeof (float));
    calc_nbp_impulse (a);
    a->p = FIRCORE::create_fircore (a->size, a->in, a->out, a->nc, a->mp, a->impulse);
    // print_impulse ("nbp.txt", a->size + 1, impulse, 1, 0);
    delete[](a->impulse);
    return a;
}

void NBP::destroy_nbp (NBP *a)
{
    FIRCORE::destroy_fircore (a->p);
    delete[] (a->bphigh);
    delete[] (a->bplow);
    delete (a);
}

void NBP::flush_nbp (NBP *a)
{
    FIRCORE::flush_fircore (a->p);
}

void NBP::xnbp (NBP *a, int pos)
{
    if (a->run && pos == a->position)
        FIRCORE::xfircore (a->p);
    else if (a->in != a->out)
        memcpy (a->out, a->in, a->size * sizeof (wcomplex));
}

void NBP::setBuffers_nbp (NBP *a, float* in, float* out)
{
    a->in = in;
    a->out = out;
    FIRCORE::setBuffers_fircore (a->p, a->in, a->out);
}

void NBP::setSamplerate_nbp (NBP *a, int rate)
{
    a->rate = rate;
    calc_nbp_impulse (a);
    FIRCORE::setImpulse_fircore (a->p, a->impulse, 1);
    delete[] (a->impulse);
}

void NBP::setSize_nbp (NBP *a, int size)
{
    // NOTE:  'size' must be <= 'nc'
    a->size = size;
    FIRCORE::setSize_fircore (a->p, a->size);
    calc_nbp_impulse (a);
    FIRCORE::setImpulse_fircore (a->p, a->impulse, 1);
    delete[] (a->impulse);
}

void NBP::setNc_nbp (NBP *a)
{
    calc_nbp_impulse (a);
    FIRCORE::setNc_fircore (a->p, a->nc, a->impulse);
    delete[] (a->impulse);
}

void NBP::setMp_nbp (NBP *a)
{
    FIRCORE::setMp_fircore (a->p, a->mp);
}

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

// DATABASE PROPERTIES

void NBP::UpdateNBPFiltersLightWeight (RXA& rxa)
{   // called when setting tune freq or shift freq
    calc_nbp_lightweight (rxa.nbp0.p);
    calc_nbp_lightweight (rxa.bpsnba.p->bpsnba);
}

void NBP::UpdateNBPFilters(RXA& rxa)
{
    NBP *a = rxa.nbp0.p;
    BPSNBA *b = rxa.bpsnba.p;
    if (a->fnfrun)
    {
        calc_nbp_impulse (a);
        FIRCORE::setImpulse_fircore (a->p, a->impulse, 1);
        delete[] (a->impulse);
    }
    if (b->bpsnba->fnfrun)
    {
        BPSNBA::recalc_bpsnba_filter (b, 1);
    }
}

int NBP::NBPAddNotch (RXA& rxa, int notch, float fcenter, float fwidth, int active)
{
    NOTCHDB *b;
    int i, j;
    int rval;
    b = rxa.ndb.p;
    if (notch <= b->nn && b->nn < b->maxnotches)
    {
        b->nn++;
        for (i = b->nn - 2, j = b->nn - 1; i >= notch; i--, j--)
        {
            b->fcenter[j] = b->fcenter[i];
            b->fwidth[j] = b->fwidth[i];
            b->nlow[j] = b->nlow[i];
            b->nhigh[j] = b->nhigh[i];
            b->active[j] = b->active[i];
        }
        b->fcenter[notch] = fcenter;
        b->fwidth[notch] = fwidth;
        b->nlow[notch] = fcenter - 0.5 * fwidth;
        b->nhigh[notch] = fcenter + 0.5 * fwidth;
        b->active[notch] = active;
        UpdateNBPFilters (rxa);
        rval = 0;
    }
    else
        rval = -1;
    return rval;
}

int NBP::NBPGetNotch (RXA& rxa, int notch, float* fcenter, float* fwidth, int* active)
{
    NOTCHDB *a;
    int rval;
    rxa.csDSP.lock();
    a = rxa.ndb.p;
    if (notch < a->nn)
    {
        *fcenter = a->fcenter[notch];
        *fwidth = a->fwidth[notch];
        *active = a->active[notch];
        rval = 0;
    }
    else
    {
        *fcenter = -1.0;
        *fwidth = 0.0;
        *active = -1;
        rval = -1;
    }
    rxa.csDSP.unlock();
    return rval;
}

int NBP::NBPDeleteNotch (RXA& rxa, int notch)
{
    int i, j;
    int rval;
    NOTCHDB *a;
    a = rxa.ndb.p;
    if (notch < a->nn)
    {
        a->nn--;
        for (i = notch, j = notch + 1; i < a->nn; i++, j++)
        {
            a->fcenter[i] = a->fcenter[j];
            a->fwidth[i] = a->fwidth[j];
            a->nlow[i] = a->nlow[j];
            a->nhigh[i] = a->nhigh[j];
            a->active[i] = a->active[j];
        }
        UpdateNBPFilters (rxa);
        rval = 0;
    }
    else
        rval = -1;
    return rval;
}

int NBP::NBPEditNotch (RXA& rxa, int notch, float fcenter, float fwidth, int active)
{
    NOTCHDB *a;
    int rval;
    a = rxa.ndb.p;
    if (notch < a->nn)
    {
        a->fcenter[notch] = fcenter;
        a->fwidth[notch] = fwidth;
        a->nlow[notch] = fcenter - 0.5 * fwidth;
        a->nhigh[notch] = fcenter + 0.5 * fwidth;
        a->active[notch] = active;
        UpdateNBPFilters (rxa);
        rval = 0;
    }
    else
        rval = -1;
    return rval;
}

void NBP::NBPGetNumNotches (RXA& rxa, int* nnotches)
{
    NOTCHDB *a;
    rxa.csDSP.lock();
    a = rxa.ndb.p;
    *nnotches = a->nn;
    rxa.csDSP.unlock();
}

void NBP::NBPSetTuneFrequency (RXA& rxa, float tunefreq)
{
    NOTCHDB *a;
    a = rxa.ndb.p;
    if (tunefreq != a->tunefreq)
    {
        a->tunefreq = tunefreq;
        UpdateNBPFiltersLightWeight (rxa);
    }
}

void NBP::NBPSetShiftFrequency (RXA& rxa, float shift)
{
    NOTCHDB *a;
    a = rxa.ndb.p;
    if (shift != a->shift)
    {
        a->shift = shift;
        UpdateNBPFiltersLightWeight (rxa);
    }
}

void NBP::NBPSetNotchesRun (RXA& rxa, int run)
{
    NOTCHDB *a = rxa.ndb.p;
    NBP *b = rxa.nbp0.p;
    if ( run != a->master_run)
    {
        a->master_run = run;                            // update variables
        b->fnfrun = a->master_run;
        BPSNBA::bpsnbaCheck (rxa, rxa.mode, run);
        calc_nbp_impulse (b);                           // recalc nbp impulse response
        FIRCORE::setImpulse_fircore (b->p, b->impulse, 0);       // calculate new filter masks
        delete[] (b->impulse);
        rxa.csDSP.lock();      // block DSP channel processing
        BPSNBA::bpsnbaSet (rxa);
        FIRCORE::setUpdate_fircore (b->p);                       // apply new filter masks
        rxa.csDSP.unlock();      // unblock channel processing
    }
}

// FILTER PROPERTIES

void NBP::NBPSetRun (RXA& rxa, int run)
{
    NBP *a;
    rxa.csDSP.lock();
    a = rxa.nbp0.p;
    a->run = run;
    rxa.csDSP.unlock();
}

void NBP::NBPSetFreqs (RXA& rxa, float flow, float fhigh)
{
    NBP *a;
    a = rxa.nbp0.p;
    if ((flow != a->flow) || (fhigh != a->fhigh))
    {
        a->flow = flow;
        a->fhigh = fhigh;
        calc_nbp_impulse (a);
        FIRCORE::setImpulse_fircore (a->p, a->impulse, 1);
        delete[] (a->impulse);
    }
}

void NBP::NBPSetWindow (RXA& rxa, int wintype)
{
    NBP *a;
    BPSNBA *b;
    a = rxa.nbp0.p;
    b = rxa.bpsnba.p;
    if ((a->wintype != wintype))
    {
        a->wintype = wintype;
        calc_nbp_impulse (a);
        FIRCORE::setImpulse_fircore (a->p, a->impulse, 1);
        delete[] (a->impulse);
    }
    if ((b->wintype != wintype))
    {
        b->wintype = wintype;
        BPSNBA::recalc_bpsnba_filter (b, 1);
    }
}

void NBP::NBPSetNC (RXA& rxa, int nc)
{
    // NOTE:  'nc' must be >= 'size'
    NBP *a;
    rxa.csDSP.lock();
    a = rxa.nbp0.p;
    if (a->nc != nc)
    {
        a->nc = nc;
        setNc_nbp (a);
    }
    rxa.csDSP.unlock();
}

void NBP::NBPSetMP (RXA& rxa, int mp)
{
    NBP *a;
    a = rxa.nbp0.p;
    if (a->mp != mp)
    {
        a->mp = mp;
        setMp_nbp (a);
    }
}

void NBP::NBPGetMinNotchWidth (RXA& rxa, float* minwidth)
{
    NBP *a;
    rxa.csDSP.lock();
    a = rxa.nbp0.p;
    *minwidth = min_notch_width (a);
    rxa.csDSP.unlock();
}

void NBP::NBPSetAutoIncrease (RXA& rxa, int autoincr)
{
    NBP *a;
    BPSNBA *b;
    a = rxa.nbp0.p;
    b = rxa.bpsnba.p;
    if ((a->autoincr != autoincr))
    {
        a->autoincr = autoincr;
        calc_nbp_impulse (a);
        FIRCORE::setImpulse_fircore (a->p, a->impulse, 1);
        delete[] (a->impulse);
    }
    if ((b->autoincr != autoincr))
    {
        b->autoincr = autoincr;
        BPSNBA::recalc_bpsnba_filter (b, 1);
    }
}

} // namespace WDSP