/*  fmsq.c

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

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

void calc_fmsq (FMSQ a)
{
    double delta, theta;
    double* impulse;
    int i;
    // noise filter
    a->noise = (double *)malloc0(2 * a->size * sizeof(complex));
    a->F[0] = 0.0;
    a->F[1] = a->fc;
    a->F[2] = *a->pllpole;
    a->F[3] = 20000.0;
    a->G[0] = 0.0;
    a->G[1] = 0.0;
    a->G[2] = 3.0;
    a->G[3] = +20.0 * log10(20000.0 / *a->pllpole);
    impulse = eq_impulse (a->nc, 3, a->F, a->G, a->rate, 1.0 / (2.0 * a->size), 0, 0);
    a->p = create_fircore (a->size, a->trigger, a->noise, a->nc, a->mp, impulse);
    _aligned_free (impulse);
    // noise averaging
    a->avm = exp(-1.0 / (a->rate * a->avtau));
    a->onem_avm = 1.0 - a->avm;
    a->avnoise = 100.0;
    a->longavm = exp(-1.0 / (a->rate * a->longtau));
    a->onem_longavm = 1.0 - a->longavm;
    a->longnoise = 1.0;
    // level change
    a->ntup   = (int)(a->tup   * a->rate);
    a->ntdown = (int)(a->tdown * a->rate);
    a->cup   = (double *)malloc0 ((a->ntup   + 1) * sizeof(double));
    a->cdown = (double *)malloc0 ((a->ntdown + 1) * sizeof(double));
    delta = PI / (double)a->ntup;
    theta = 0.0;
    for (i = 0; i <= a->ntup; i++)
    {
        a->cup[i] = 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] = 0.5 * (1 + cos(theta));
        theta += delta;
    }
    // control
    a->state = 0;
    a->ready = 0;
    a->ramp = 0.0;
    a->rstep = 1.0 / a->rate;
}

void decalc_fmsq (FMSQ a)
{
    _aligned_free(a->cdown);
    _aligned_free(a->cup);
    destroy_fircore (a->p);
    _aligned_free(a->noise);
}

FMSQ create_fmsq (int run, int size, double* insig, double* outsig, double* trigger, int rate, double fc,
    double* pllpole, double tdelay, double avtau, double longtau, double tup, double tdown, double tail_thresh,
    double unmute_thresh, double min_tail, double max_tail, int nc, int mp)
{
    FMSQ a = (FMSQ) malloc0 (sizeof (fmsq));
    a->run = run;
    a->size = size;
    a->insig = insig;
    a->outsig = outsig;
    a->trigger = trigger;
    a->rate = (double)rate;
    a->fc = fc;
    a->pllpole = pllpole;
    a->tdelay = tdelay;
    a->avtau = avtau;
    a->longtau = longtau;
    a->tup = tup;
    a->tdown = tdown;
    a->tail_thresh = tail_thresh;
    a->unmute_thresh = unmute_thresh;
    a->min_tail = min_tail;
    a->max_tail = max_tail;
    a->nc = nc;
    a->mp = mp;
    calc_fmsq (a);
    return a;
}

void destroy_fmsq (FMSQ a)
{
    decalc_fmsq (a);
    _aligned_free (a);
}

void flush_fmsq (FMSQ a)
{
    flush_fircore (a->p);
    a->avnoise = 100.0;
    a->longnoise = 1.0;
    a->state = 0;
    a->ready = 0;
    a->ramp = 0.0;
}

enum _fmsqstate
{
    MUTED,
    INCREASE,
    UNMUTED,
    TAIL,
    DECREASE
};

void xfmsq (FMSQ a)
{
    if (a->run)
    {
        int i;
        double noise, lnlimit;
        xfircore (a->p);
        for (i = 0; i < a->size; i++)
        {
            noise = sqrt(a->noise[2 * i + 0] * a->noise[2 * i + 0] + a->noise[2 * i + 1] * a->noise[2 * i + 1]);
            a->avnoise = a->avm * a->avnoise + a->onem_avm * noise;
            a->longnoise = a->longavm * a->longnoise + a->onem_longavm * noise;
            if (!a->ready) a->ramp += a->rstep;
            if (a->ramp >= a->tdelay) a->ready = 1;

            switch (a->state)
            {
            case MUTED:
                if (a->avnoise < a->unmute_thresh && a->ready)
                {
                    a->state = INCREASE;
                    a->count = a->ntup;
                }
                a->outsig[2 * i + 0] = 0.0;
                a->outsig[2 * i + 1] = 0.0;
                break;
            case INCREASE:
                a->outsig[2 * i + 0] = a->insig[2 * i + 0] * a->cup[a->ntup - a->count];
                a->outsig[2 * i + 1] = a->insig[2 * i + 1] * a->cup[a->ntup - a->count];
                if (a->count-- == 0)
                    a->state = UNMUTED;
                break;
            case UNMUTED:
                if (a->avnoise > a->tail_thresh)
                {
                    a->state = TAIL;
                    if ((lnlimit = a->longnoise) > 1.0) lnlimit = 1.0;
                    a->count = (int)((a->min_tail + (a->max_tail - a->min_tail) * lnlimit) * a->rate);
                }
                a->outsig[2 * i + 0] = a->insig[2 * i + 0];
                a->outsig[2 * i + 1] = a->insig[2 * i + 1];
                break;
            case TAIL:
                a->outsig[2 * i + 0] = a->insig[2 * i + 0];
                a->outsig[2 * i + 1] = a->insig[2 * i + 1];
                if (a->avnoise < a->unmute_thresh)
                    a->state = UNMUTED;
                else if (a->count-- == 0)
                {
                    a->state = DECREASE;
                    a->count = a->ntdown;
                }
                break;
            case DECREASE:
                a->outsig[2 * i + 0] = a->insig[2 * i + 0] * a->cdown[a->ntdown - a->count];
                a->outsig[2 * i + 1] = a->insig[2 * i + 1] * a->cdown[a->ntdown - a->count];
                if (a->count-- == 0)
                    a->state = MUTED;
                break;
            }
        }
    }
    else if (a->insig != a->outsig)
        memcpy (a->outsig, a->insig, a->size * sizeof (complex));
}

void setBuffers_fmsq (FMSQ a, double* in, double* out, double* trig)
{
    a->insig = in;
    a->outsig = out;
    a->trigger = trig;
    setBuffers_fircore (a->p, a->trigger, a->noise);
}

void setSamplerate_fmsq (FMSQ a, int rate)
{
    decalc_fmsq (a);
    a->rate = rate;
    calc_fmsq (a);
}

void setSize_fmsq (FMSQ a, int size)
{
    decalc_fmsq (a);
    a->size = size;
    calc_fmsq (a);
}

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

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

PORT
void SetRXAFMSQThreshold (int channel, double threshold)
{
    EnterCriticalSection (&ch[channel].csDSP);
    rxa[channel].fmsq.p->tail_thresh = threshold;
    rxa[channel].fmsq.p->unmute_thresh = 0.9 * threshold;
    LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetRXAFMSQNC (int channel, int nc)
{
    FMSQ a;
    double* impulse;
    EnterCriticalSection (&ch[channel].csDSP);
    a = rxa[channel].fmsq.p;
    if (a->nc != nc)
    {
        a->nc = nc;
        impulse = eq_impulse (a->nc, 3, a->F, a->G, a->rate, 1.0 / (2.0 * a->size), 0, 0);
        setNc_fircore (a->p, a->nc, impulse);
        _aligned_free (impulse);
    }
    LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetRXAFMSQMP (int channel, int mp)
{
    FMSQ a;
    a = rxa[channel].fmsq.p;
    if (a->mp != mp)
    {
        a->mp = mp;
        setMp_fircore (a->p, a->mp);
    }
}
