/*  nbp.c

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

Copyright (C) 2015, 2016 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"

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

NOTCHDB create_notchdb (int master_run, int maxnotches)
{
    NOTCHDB a = (NOTCHDB) malloc0 (sizeof (notchdb));
    a->master_run = master_run;
    a->maxnotches = maxnotches;
    a->nn = 0;
    a->fcenter = (double *) malloc0 (a->maxnotches * sizeof (double));
    a->fwidth  = (double *) malloc0 (a->maxnotches * sizeof (double));
    a->nlow    = (double *) malloc0 (a->maxnotches * sizeof (double));
    a->nhigh   = (double *) malloc0 (a->maxnotches * sizeof (double));
    a->active  = (int    *) malloc0 (a->maxnotches * sizeof (int   ));
    return a;
}

void destroy_notchdb (NOTCHDB b)
{
    _aligned_free (b->active);
    _aligned_free (b->nhigh);
    _aligned_free (b->nlow);
    _aligned_free (b->fwidth);
    _aligned_free (b->fcenter);
}

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

double* fir_mbandpass (int N, int nbp, double* flow, double* fhigh, double rate, double scale, int wintype)
{
    int i, k;
    double* impulse = (double *) malloc0 (N * sizeof (complex));
    double* imp;
    for (k = 0; k < nbp; k++)
    {
        imp = 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];
        }
        _aligned_free (imp);
    }
    return impulse;
}

double min_notch_width (NBP a)
{
    double 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 make_nbp (int nn, int* active, double* center, double* width, double* nlow, double* nhigh,
    double minwidth, int autoincr, double flow, double fhigh, double* bplow, double* bphigh, int* havnotch)
{
    int nbp;
    int nnbp, adds;
    int i, j, k;
    double nl, nh;
    int* del = (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;
        }
    }
    _aligned_free (del);
    return nbp;
}

void calc_nbp_lightweight (NBP a)
{   // calculate and set new impulse response; used when changing tune freq or shift freq
    int i;
    double fl, fh;
    double 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 / (double)(2 * a->size), a->wintype);
            setImpulse_fircore (a->p, a->impulse, 1);
            // print_impulse ("nbp.txt", a->size + 1, impulse, 1, 0);
            _aligned_free(a->impulse);
        }
        a->hadnotch = a->havnotch;
    }
    else
        a->hadnotch = 1;
}

void calc_nbp_impulse (NBP a)
{   // calculates impulse response; for create_fircore() and parameter changes
    int i;
    double fl, fh;
    double 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 / (double)(2 * a->size), a->wintype);
    }
    else
    {
        a->impulse = fir_bandpass(a->nc, a->flow, a->fhigh, a->rate, a->wintype, 1, a->gain / (double)(2 * a->size));
    }
}

NBP create_nbp(int run, int fnfrun, int position, int size, int nc, int mp, double* in, double* out,
    double flow, double fhigh, int rate, int wintype, double gain, int autoincr, int maxpb, NOTCHDB* ptraddr)
{
    NBP a = (NBP) malloc0 (sizeof (nbp));
    a->run = run;
    a->fnfrun = fnfrun;
    a->position = position;
    a->size = size;
    a->nc = nc;
    a->mp = mp;
    a->rate = (double)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   = (double *) malloc0 (a->maxpb * sizeof (double));
    a->bphigh  = (double *) malloc0 (a->maxpb * sizeof (double));
    calc_nbp_impulse (a);
    a->p = create_fircore (a->size, a->in, a->out, a->nc, a->mp, a->impulse);
    // print_impulse ("nbp.txt", a->size + 1, impulse, 1, 0);
    _aligned_free(a->impulse);
    return a;
}

void destroy_nbp (NBP a)
{
    destroy_fircore (a->p);
    _aligned_free (a->bphigh);
    _aligned_free (a->bplow);
    _aligned_free (a);
}

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

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

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

void setSamplerate_nbp (NBP a, int rate)
{
    a->rate = rate;
    calc_nbp_impulse (a);
    setImpulse_fircore (a->p, a->impulse, 1);
    _aligned_free (a->impulse);
}

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

void setNc_nbp (NBP a)
{
    calc_nbp_impulse (a);
    setNc_fircore (a->p, a->nc, a->impulse);
    _aligned_free (a->impulse);
}

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

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

// DATABASE PROPERTIES

void UpdateNBPFiltersLightWeight (int channel)
{   // called when setting tune freq or shift freq
    calc_nbp_lightweight (rxa[channel].nbp0.p);
    calc_nbp_lightweight (rxa[channel].bpsnba.p->bpsnba);
}

void UpdateNBPFilters(int channel)
{
    NBP a = rxa[channel].nbp0.p;
    BPSNBA b = rxa[channel].bpsnba.p;
    if (a->fnfrun)
    {
        calc_nbp_impulse (a);
        setImpulse_fircore (a->p, a->impulse, 1);
        _aligned_free (a->impulse);
    }
    if (b->bpsnba->fnfrun)
    {
        recalc_bpsnba_filter (b, 1);
    }
}

PORT
int RXANBPAddNotch (int channel, int notch, double fcenter, double fwidth, int active)
{
    NOTCHDB b;
    int i, j;
    int rval;
    b = rxa[channel].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 (channel);
        rval = 0;
    }
    else
        rval = -1;
    return rval;
}

PORT
int RXANBPGetNotch (int channel, int notch, double* fcenter, double* fwidth, int* active)
{
    NOTCHDB a;
    int rval;
    EnterCriticalSection (&ch[channel].csDSP);
    a = rxa[channel].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;
    }
    LeaveCriticalSection (&ch[channel].csDSP);
    return rval;
}

PORT
int RXANBPDeleteNotch (int channel, int notch)
{
    int i, j;
    int rval;
    NOTCHDB a;
    a = rxa[channel].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 (channel);
        rval = 0;
    }
    else
        rval = -1;
    return rval;
}

PORT
int RXANBPEditNotch (int channel, int notch, double fcenter, double fwidth, int active)
{
    NOTCHDB a;
    int rval;
    a = rxa[channel].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 (channel);
        rval = 0;
    }
    else
        rval = -1;
    return rval;
}

PORT
void RXANBPGetNumNotches (int channel, int* nnotches)
{
    NOTCHDB a;
    EnterCriticalSection (&ch[channel].csDSP);
    a = rxa[channel].ndb.p;
    *nnotches = a->nn;
    LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void RXANBPSetTuneFrequency (int channel, double tunefreq)
{
    NOTCHDB a;
    a = rxa[channel].ndb.p;
    if (tunefreq != a->tunefreq)
    {
        a->tunefreq = tunefreq;
        UpdateNBPFiltersLightWeight (channel);
    }
}

PORT
void RXANBPSetShiftFrequency (int channel, double shift)
{
    NOTCHDB a;
    a = rxa[channel].ndb.p;
    if (shift != a->shift)
    {
        a->shift = shift;
        UpdateNBPFiltersLightWeight (channel);
    }
}

PORT
void RXANBPSetNotchesRun (int channel, int run)
{
    NOTCHDB a = rxa[channel].ndb.p;
    NBP b = rxa[channel].nbp0.p;
    if ( run != a->master_run)
    {
        a->master_run = run;                            // update variables
        b->fnfrun = a->master_run;
        RXAbpsnbaCheck (channel, rxa[channel].mode, run);
        calc_nbp_impulse (b);                           // recalc nbp impulse response
        setImpulse_fircore (b->p, b->impulse, 0);       // calculate new filter masks
        _aligned_free (b->impulse);
        EnterCriticalSection (&ch[channel].csDSP);      // block DSP channel processing
        RXAbpsnbaSet (channel);
        setUpdate_fircore (b->p);                       // apply new filter masks
        LeaveCriticalSection (&ch[channel].csDSP);      // unblock channel processing
    }
}

// FILTER PROPERTIES

PORT
void RXANBPSetRun (int channel, int run)
{
    NBP a;
    EnterCriticalSection (&ch[channel].csDSP);
    a = rxa[channel].nbp0.p;
    a->run = run;
    LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void RXANBPSetFreqs (int channel, double flow, double fhigh)
{
    NBP a;
    a = rxa[channel].nbp0.p;
    if ((flow != a->flow) || (fhigh != a->fhigh))
    {
        a->flow = flow;
        a->fhigh = fhigh;
        calc_nbp_impulse (a);
        setImpulse_fircore (a->p, a->impulse, 1);
        _aligned_free (a->impulse);
    }
}

PORT
void RXANBPSetWindow (int channel, int wintype)
{
    NBP a;
    BPSNBA b;
    a = rxa[channel].nbp0.p;
    b = rxa[channel].bpsnba.p;
    if ((a->wintype != wintype))
    {
        a->wintype = wintype;
        calc_nbp_impulse (a);
        setImpulse_fircore (a->p, a->impulse, 1);
        _aligned_free (a->impulse);
    }
    if ((b->wintype != wintype))
    {
        b->wintype = wintype;
        recalc_bpsnba_filter (b, 1);
    }
}

PORT
void RXANBPSetNC (int channel, int nc)
{
    // NOTE:  'nc' must be >= 'size'
    NBP a;
    EnterCriticalSection (&ch[channel].csDSP);
    a = rxa[channel].nbp0.p;
    if (a->nc != nc)
    {
        a->nc = nc;
        setNc_nbp (a);
    }
    LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void RXANBPSetMP (int channel, int mp)
{
    NBP a;
    a = rxa[channel].nbp0.p;
    if (a->mp != mp)
    {
        a->mp = mp;
        setMp_nbp (a);
    }
}

PORT
void RXANBPGetMinNotchWidth (int channel, double* minwidth)
{
    NBP a;
    EnterCriticalSection (&ch[channel].csDSP);
    a = rxa[channel].nbp0.p;
    *minwidth = min_notch_width (a);
    LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void RXANBPSetAutoIncrease (int channel, int autoincr)
{
    NBP a;
    BPSNBA b;
    a = rxa[channel].nbp0.p;
    b = rxa[channel].bpsnba.p;
    if ((a->autoincr != autoincr))
    {
        a->autoincr = autoincr;
        calc_nbp_impulse (a);
        setImpulse_fircore (a->p, a->impulse, 1);
        _aligned_free (a->impulse);
    }
    if ((b->autoincr != autoincr))
    {
        b->autoincr = autoincr;
        recalc_bpsnba_filter (b, 1);
    }
}
