/*  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 <array>

#include "comm.hpp"
#include "fir.hpp"
#include "fircore.hpp"
#include "bpsnba.hpp"
#include "nbp.hpp"

namespace WDSP {

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

NOTCHDB::NOTCHDB(int _master_run, int _maxnotches)
{
    master_run = _master_run;
    maxnotches = _maxnotches;
    nn = 0;
    fcenter.resize(maxnotches);
    fwidth.resize(maxnotches);
    nlow.resize(maxnotches);
    nhigh.resize(maxnotches);
    active.resize(maxnotches);
}

int NOTCHDB::addNotch(int notch, double _fcenter, double _fwidth, int _active)
{
    int i;
    int j;
    int rval;

    if (notch <= nn && nn < maxnotches)
    {
        nn++;

        for (i = nn - 2, j = nn - 1; i >= notch; i--, j--)
        {
            fcenter[j] = fcenter[i];
            fwidth[j] = fwidth[i];
            nlow[j] = nlow[i];
            nhigh[j] = nhigh[i];
            active[j] = active[i];
        }
        fcenter[notch] = _fcenter;
        fwidth[notch] = _fwidth;
        nlow[notch] = _fcenter - 0.5 * _fwidth;
        nhigh[notch] = _fcenter + 0.5 * _fwidth;
        active[notch] = _active;
        rval = 0;
    }
    else
        rval = -1;
    return rval;
}

int NOTCHDB::getNotch(int _notch, double* _fcenter, double* _fwidth, int* _active)
{
    int rval;

    if (_notch < nn)
    {
        *_fcenter = fcenter[_notch];
        *_fwidth = fwidth[_notch];
        *_active = active[_notch];
        rval = 0;
    }
    else
    {
        *_fcenter = -1.0;
        *_fwidth = 0.0;
        *_active = -1;
        rval = -1;
    }

    return rval;
}

int NOTCHDB::deleteNotch(int _notch)
{
    int i;
    int j;
    int rval;

    if (_notch < nn)
    {
        nn--;
        for (i = _notch, j = _notch + 1; i < nn; i++, j++)
        {
            fcenter[i] = fcenter[j];
            fwidth[i] = fwidth[j];
            nlow[i] = nlow[j];
            nhigh[i] = nhigh[j];
            active[i] = active[j];
        }
        rval = 0;
    }
    else
        rval = -1;
    return rval;
}

int NOTCHDB::editNotch(int _notch, double _fcenter, double _fwidth, int _active)
{
    int rval;

    if (_notch < nn)
    {
        fcenter[_notch] = _fcenter;
        fwidth[_notch] = _fwidth;
        nlow[_notch] = _fcenter - 0.5 * _fwidth;
        nhigh[_notch] = _fcenter + 0.5 * _fwidth;
        active[_notch] = _active;
        rval = 0;
    }
    else
        rval = -1;
    return rval;
}

void NOTCHDB::getNumNotches(int* _nnotches) const
{
    *_nnotches = nn;
}

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

void NBP::fir_mbandpass (std::vector<float>& impulse, int N, int nbp, const double* flow, const double* fhigh, double rate, double scale, int wintype)
{
    impulse.resize(N * 2);
    std::fill(impulse.begin(), impulse.end(), 0);
    for (int k = 0; k < nbp; k++)
    {
        std::vector<float> imp;
        FIR::fir_bandpass (imp, N, flow[k], fhigh[k], rate, wintype, 1, scale);
        for (int i = 0; i < N; i++)
        {
            impulse[2 * i + 0] += imp[2 * i + 0];
            impulse[2 * i + 1] += imp[2 * i + 1];
        }
    }
}

double NBP::min_notch_width() const
{
    double min_width;
    switch (wintype)
    {
    case 0:
        min_width = 1600.0 / (nc / 256) * (rate / 48000);
        break;
    case 1:
        min_width = 2200.0 / (nc / 256) * (rate / 48000);
        break;
    default:
        min_width = 0;
    }
    return min_width;
}

int NBP::make_nbp (
    int nn,
    std::vector<int>& active,
    std::vector<double>& center,
    std::vector<double>& width,
    std::vector<double>& nlow,
    std::vector<double>& nhigh,
    double minwidth,
    int autoincr,
    double flow,
    double fhigh,
    std::vector<double>& bplow,
    std::vector<double>& bphigh,
    int* havnotch
)
{
    int nbp;
    int nnbp;
    int adds;
    int i;
    double nl;
    double nh;
    std::array<int, 1024> del;
    if (fhigh > flow)
    {
        bplow[0]  = flow;
        bphigh[0] = fhigh;
        nbp = 1;
    }
    else
    {
        nbp = 0;
        return nbp;
    }
    *havnotch = 0;
    for (int 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 (int j = i; j < nnbp; j++)
                    {
                        bplow[j] = bplow[j + 1];
                        bphigh[j] = bphigh[j + 1];
                    }
                    del[i] = 0;
                }
            }
            nbp = nnbp;
        }
    }
    return nbp;
}

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

void NBP::calc_impulse ()
{   // calculates impulse response; for create_fircore() and parameter changes
    double fl;
    double fh;
    double offset;
    NOTCHDB *b = notchdb;

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

NBP::NBP(
    int _run,
    int _fnfrun,
    int _position,
    int _size,
    int _nc,
    int _mp,
    float* _in,
    float* _out,
    double _flow,
    double _fhigh,
    int _rate,
    int _wintype,
    double _gain,
    int _autoincr,
    int _maxpb,
    NOTCHDB* _notchdb
) :
    run(_run),
    fnfrun(_fnfrun),
    position(_position),
    size(_size),
    nc(_nc),
    mp(_mp),
    rate((double) _rate),
    wintype(_wintype),
    gain(_gain),
    in(_in),
    out(_out),
    autoincr(_autoincr),
    flow(_flow),
    fhigh(_fhigh),
    maxpb(_maxpb),
    notchdb(_notchdb)
{
    bplow.resize(maxpb);
    bphigh.resize(maxpb);
    calc_impulse ();
    fircore = new FIRCORE(size, in, out, nc, mp, impulse.data());
}

NBP::~NBP()
{
    delete fircore;
}

void NBP::flush()
{
    fircore->flush();
}

void NBP::execute (int pos)
{
    if (run && pos == position)
        fircore->execute();
    else if (in != out)
        std::copy( in,  in + size * 2, out);
}

void NBP::setBuffers(float* _in, float* _out)
{
    in = _in;
    out = _out;
    fircore->setBuffers(in, out);
}

void NBP::setSamplerate(int _rate)
{
    rate = _rate;
    calc_impulse ();
    fircore->setImpulse(impulse.data(), 1);
}

void NBP::setSize(int _size)
{
    // NOTE:  'size' must be <= 'nc'
    size = _size;
    fircore->setSize(size);
    calc_impulse ();
    fircore->setImpulse(impulse.data(), 1);
}

void NBP::setNc()
{
    calc_impulse();
    fircore->setNc(nc, impulse.data());
}

void NBP::setMp()
{
    fircore->setMp(mp);
}

/********************************************************************************************************
*                                                                                                       *
*                                           Public Properties                                           *
*                                                                                                       *
********************************************************************************************************/

// FILTER PROPERTIES

void NBP::SetRun(int _run)
{
    run = _run;
}

void NBP::SetFreqs(double _flow, double _fhigh)
{
    if ((flow != _flow) || (fhigh != _fhigh))
    {
        flow = _flow;
        fhigh = _fhigh;
        calc_impulse();
        fircore->setImpulse(impulse.data(), 1);
    }
}

void NBP::SetNC(int _nc)
{
    // NOTE:  'nc' must be >= 'size'
    if (nc != _nc)
    {
        nc = _nc;
        setNc();
    }
}

void NBP::SetMP(int _mp)
{
    if (mp != _mp)
    {
        mp = _mp;
        setMp();
    }
}

void NBP::GetMinNotchWidth(double* minwidth) const
{
    *minwidth = min_notch_width();
}

} // namespace WDSP