/* For information on usage and redistribution, and for a DISCLAIMER OF ALL
* WARRANTIES, see the file, "LICENSE.txt," in this distribution.

NLMSCC normalized LMS algorithm with coefficient constraints
lib iem_adaptfilt written by Markus Noisternig & Thomas Musil
noisternig_AT_iem.at; musil_AT_iem.at
(c) Institute of Electronic Music and Acoustics, Graz Austria 2005 */

#ifdef _MSC_VER
#pragma warning( disable : 4305 )  /* uncast const double to float */
#pragma warning( disable : 4244 )  /* uncast float/int conversion etc. */
#pragma warning( disable : 4101 )  /* unused automatic variables */
#endif /* _MSC_VER */


#include "m_pd.h"
#include "iemlib.h"
#include "iemarray.h"
#include <math.h>
#include <stdio.h>
#include <string.h>


/* ----------------------- NLMSCC~ ------------------------------ */
/* -- Normalized Least Mean Square (linear adaptive FIR-filter) -- */
/* --   with Coefficient Constraint  -- */
/* -- first input:  reference signal -- */
/* -- second input: desired signal -- */
/* --  -- */
/* for further information on adaptive filter design we refer to */
/* [1] Haykin, "Adaptive Filter Theory", 4th ed, Prentice Hall */
/* [2] Benesty, "Adaptive Signal Processing", Springer */
/*  */


typedef struct NLMSCC_tilde
{
    t_object            x_obj;
    t_symbol            *x_w_array_sym_name;
    iemarray_t          *x_w_array_mem_beg;
    t_symbol            *x_wmin_array_sym_name;
    iemarray_t          *x_wmin_array_mem_beg;
    t_symbol            *x_wmax_array_sym_name;
    iemarray_t          *x_wmax_array_mem_beg;
    t_float             *x_io_ptr_beg[4];// memory: 2 sig-in and 2 sig-out vectors
    t_float             *x_in_hist;// start point double buffer for sig-in history
    int                 x_rw_index;// read-write-index
    int                 x_n_order;// order of filter
    int                 x_update;// 2^n rounded value, downsampling of update speed
    t_float             x_beta;// learn rate [0 .. 2]
    t_float             x_gamma;// regularization
    t_outlet            *x_out_clipping_bang;
    t_clock             *x_clock;
    t_float             x_msi;
} t_NLMSCC_tilde;

t_class *NLMSCC_tilde_class;

static void NLMSCC_tilde_tick(t_NLMSCC_tilde *x)
{
    outlet_bang(x->x_out_clipping_bang);
}

static void NLMSCC_tilde_beta(t_NLMSCC_tilde *x, t_floatarg f) // learn rate
{
    if(f < 0.0f)
        f = 0.0f;
    if(f > 2.0f)
        f = 2.0f;

    x->x_beta = f;
}

static void NLMSCC_tilde_gamma(t_NLMSCC_tilde *x, t_floatarg f) // regularization factor (dither)
{
    if(f < 0.0f)
        f = 0.0f;
    if(f > 1.0f)
        f = 1.0f;

    x->x_gamma = f;
}


static void NLMSCC_tilde_update(t_NLMSCC_tilde *x, t_floatarg f) // downsample of learn-rate
{
    int i=1, u = (int)f;

    if(u < 0)
        u = 0;
    else
    {
        while(i <= u)   // convert u for 2^N
            i *= 2;     // round downwards
        i /= 2;
        u = i;
    }
    x->x_update = u;
}

/* ============== DSP ======================= */

static t_int *NLMSCC_tilde_perform_zero(t_int *w)
{
    t_NLMSCC_tilde *x = (t_NLMSCC_tilde *)(w[1]);
    t_int n = (t_int)(w[2]), i;

    t_float **io = x->x_io_ptr_beg;
    t_sample *out;
    int j;

    for(j=0; j<2; j++)/* output-vector-row */
    {
        out = io[j+2];
        for(i=0; i<n; i++)
        {
            *out++ = 0.0f;
        }
    }
    return (w+3);
}

static t_int *NLMSCC_tilde_perform(t_int *w)
{
    t_NLMSCC_tilde *x = (t_NLMSCC_tilde *)(w[1]);
    t_int n = (t_int)(w[2]), i;
    int n_order = x->x_n_order;   /* filter-order */
    int rw_index = x->x_rw_index;
    t_sample *in = x->x_io_ptr_beg[0];// first sig in
    t_sample *desired_in = x->x_io_ptr_beg[1], din;// second sig in
    t_sample *filt_out = x->x_io_ptr_beg[2];// first sig out
    t_sample *err_out = x->x_io_ptr_beg[3], eout;// second sig out
    t_float *write_in_hist1 = x->x_in_hist;
    t_float *write_in_hist2 = write_in_hist1+n_order;
    t_float *read_in_hist = write_in_hist2;
    iemarray_t *w_filt_coeff = x->x_w_array_mem_beg;
    iemarray_t *wmin_filt_coeff = x->x_wmin_array_mem_beg;
    iemarray_t *wmax_filt_coeff = x->x_wmax_array_mem_beg;
    t_float my, my_err, sum;
    t_float beta = x->x_beta;
    t_float gammax = x->x_gamma;
    int j, update_counter;
    int update = x->x_update;
    int ord8=n_order&0xfffffff8;
    int ord_residual=n_order&0x7;
    int clipped = 0;

    if(!w_filt_coeff)
        goto NLMSCC_tildeperfzero;// this is Musil/Miller style
    if(!wmin_filt_coeff)
        goto NLMSCC_tildeperfzero;
    if(!wmax_filt_coeff)
        goto NLMSCC_tildeperfzero;// if not constrained, perform zero

    for(i=0, update_counter=0; i<n; i++)// store in history and convolve
    {
        write_in_hist1[rw_index] = in[i]; // save inputs into variabel & history
        write_in_hist2[rw_index] = in[i];
        din = desired_in[i];

                // begin convolution
        sum = 0.0f;
        w_filt_coeff = x->x_w_array_mem_beg; // Musil's special convolution buffer struct
        read_in_hist = &write_in_hist2[rw_index];
        for(j=0; j<ord8; j+=8)  // loop unroll 8 taps
        {
#define update_sum_with_hist(idx)                                        \
            sum += iemarray_getfloat(w_filt_coeff, idx) * read_in_hist[-idx];
            update_sum_with_hist(0);
            update_sum_with_hist(1);
            update_sum_with_hist(2);
            update_sum_with_hist(3);
            update_sum_with_hist(4);
            update_sum_with_hist(5);
            update_sum_with_hist(6);
            update_sum_with_hist(7);
            w_filt_coeff += 8;
            read_in_hist -= 8;
        }
        for(j=0; j<ord_residual; j++)   // for filter order < 2^N
            update_sum_with_hist(j);

        filt_out[i] = sum;
        eout = din - filt_out[i];       // buffer-struct for further use
        err_out[i] = eout;

        if(update)      // downsampling for learn rate
        {
            update_counter++;
            if(update_counter >= update)
            {
                update_counter = 0;

                sum = 0.0f;// calculate energy for last n-order samples in filter
                read_in_hist = &write_in_hist2[rw_index];
                for(j=0; j<ord8; j+=8)  // unrolling quadrature calc
                {
#define update_sum_square(idx) \
            sum += read_in_hist[-idx] * read_in_hist[-idx]
                    update_sum_square(0);
                    update_sum_square(1);
                    update_sum_square(2);
                    update_sum_square(3);
                    update_sum_square(4);
                    update_sum_square(5);
                    update_sum_square(6);
                    update_sum_square(7);
                    read_in_hist -= 8;
                }
                for(j=0; j<ord_residual; j++)   // residual
                    update_sum_square(j); // [-j] only valid for Musil's double buffer structure
                sum += gammax * gammax * (float)n_order; // convert gammax corresponding to filter order
                my = beta / sum;// calculate mue


                my_err = my * eout;
                w_filt_coeff = x->x_w_array_mem_beg; // coefficient constraints
                wmin_filt_coeff = x->x_wmin_array_mem_beg;
                wmax_filt_coeff = x->x_wmax_array_mem_beg;
                read_in_hist = &write_in_hist2[rw_index];
                for(j=0; j<n_order; j++) // without unroll
                {
                    iemarray_setfloat(w_filt_coeff, j, iemarray_getfloat(w_filt_coeff, j) + read_in_hist[-j] * my_err);
                    if(iemarray_getfloat(w_filt_coeff, j) > iemarray_getfloat(wmax_filt_coeff, j))
                    {
                        iemarray_setfloat(w_filt_coeff, j, iemarray_getfloat(wmax_filt_coeff, j));
                        clipped = 1;
                    }
                    else if(iemarray_getfloat(w_filt_coeff, j) < iemarray_getfloat(wmin_filt_coeff, j))
                    {
                        iemarray_setfloat(w_filt_coeff, j, iemarray_getfloat(wmin_filt_coeff, j));
                        clipped = 1;
                    }
                }
            }
        }
        rw_index++;
        if(rw_index >= n_order)
            rw_index -= n_order;
    }

    x->x_rw_index = rw_index; // back to start

    if(clipped)
        clock_delay(x->x_clock, 0);
    return(w+3);

NLMSCC_tildeperfzero:

    while(n--)
    {
        *filt_out++ = 0.0f;
        *err_out++ = 0.0f;
    }
    return(w+3);
}

static void NLMSCC_tilde_dsp(t_NLMSCC_tilde *x, t_signal **sp)
{
    t_int n = sp[0]->s_n;
    int i;

    for(i=0; i<4; i++) // store io_vec
        x->x_io_ptr_beg[i] = sp[i]->s_vec;

    x->x_w_array_mem_beg = iemarray_check(x, "NLMSCC~", x->x_w_array_sym_name, x->x_n_order);
    x->x_wmin_array_mem_beg = iemarray_check(x, "NLMSCC~", x->x_wmin_array_sym_name, x->x_n_order);
    x->x_wmax_array_mem_beg = iemarray_check(x, "NLMSCC~", x->x_wmax_array_sym_name, x->x_n_order);

    if(!(x->x_w_array_mem_beg && x->x_wmin_array_mem_beg && x->x_wmax_array_mem_beg))
        dsp_add(NLMSCC_tilde_perform_zero, 2, x, n);
    else
        dsp_add(NLMSCC_tilde_perform, 2, x, n);
}


/* setup/setdown things */

static void NLMSCC_tilde_free(t_NLMSCC_tilde *x)
{

    freebytes(x->x_in_hist, 2*x->x_n_order*sizeof(t_float));

    clock_free(x->x_clock);
}

static void *NLMSCC_tilde_new(t_symbol *s, int argc, t_atom *argv)
{
    t_NLMSCC_tilde *x = (t_NLMSCC_tilde *)pd_new(NLMSCC_tilde_class);
    int n_order=39;
    t_symbol    *w_name;
    t_symbol    *wmin_name;
    t_symbol    *wmax_name;
    t_float beta=0.1f;
    t_float gammax=0.00001f;

    if((argc >= 6) &&
        IS_A_FLOAT(argv,0) &&   //IS_A_FLOAT/SYMBOL from iemlib.h
        IS_A_FLOAT(argv,1) &&
        IS_A_FLOAT(argv,2) &&
        IS_A_SYMBOL(argv,3) &&
        IS_A_SYMBOL(argv,4) &&
        IS_A_SYMBOL(argv,5))
    {
        n_order = (int)atom_getfloatarg(0, argc, argv);
        beta    = (t_float)atom_getfloatarg(1, argc, argv);
        gammax  = (t_float)atom_getfloatarg(2, argc, argv);
        w_name  = (t_symbol *)atom_getsymbolarg(3, argc, argv);
        wmin_name  = (t_symbol *)atom_getsymbolarg(4, argc, argv);
        wmax_name  = (t_symbol *)atom_getsymbolarg(5, argc, argv);

        if(beta < 0.0f)
            beta = 0.0f;
        if(beta > 2.0f)
            beta = 2.0f;

        if(gammax < 0.0f)
            gammax = 0.0f;
        if(gammax > 1.0f)
            gammax = 1.0f;

        if(n_order < 2)
            n_order = 2;
        if(n_order > 11111)
            n_order = 11111;

        inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal);
        outlet_new(&x->x_obj, &s_signal);
        outlet_new(&x->x_obj, &s_signal);
        x->x_out_clipping_bang = outlet_new(&x->x_obj, &s_bang);

        x->x_msi = 0;
        x->x_n_order = n_order;
        x->x_update = 0;
        x->x_beta = beta;
        x->x_gamma = gammax;
        // 2 times in and one time desired_in memory allocation (history)
        x->x_in_hist = (t_float *)getbytes(2*x->x_n_order*sizeof(t_float));

        // table-symbols will be linked to their memory in future (dsp_routine)
        x->x_w_array_sym_name = gensym(w_name->s_name);
        x->x_w_array_mem_beg = (iemarray_t *)0;
        x->x_wmin_array_sym_name = gensym(wmin_name->s_name);
        x->x_wmin_array_mem_beg = (iemarray_t *)0;
        x->x_wmax_array_sym_name = gensym(wmax_name->s_name);
        x->x_wmax_array_mem_beg = (iemarray_t *)0;

        x->x_clock = clock_new(x, (t_method)NLMSCC_tilde_tick);

        return(x);
    }
    else
    {
        pd_error(0, "%s: need 3 float- + 3 symbol-arguments:", s->s_name);
        pd_error(0, "  order_of_filter + learnrate_beta + security_value + array_name_taps + array_name_tap_min + array_name_tap_max");
        return(0);
    }
}

void NLMSCC_tilde_setup(void)
{
    NLMSCC_tilde_class = class_new(gensym("NLMSCC~"), (t_newmethod)NLMSCC_tilde_new, (t_method)NLMSCC_tilde_free,
        sizeof(t_NLMSCC_tilde), 0, A_GIMME, 0);
    CLASS_MAINSIGNALIN(NLMSCC_tilde_class, t_NLMSCC_tilde, x_msi);
    class_addmethod(NLMSCC_tilde_class, (t_method)NLMSCC_tilde_dsp, gensym("dsp"), A_CANT, 0);
    class_addmethod(NLMSCC_tilde_class, (t_method)NLMSCC_tilde_update, gensym("update"), A_FLOAT, 0); // method: downsampling factor of learning (multiple of 2^N)
    class_addmethod(NLMSCC_tilde_class, (t_method)NLMSCC_tilde_beta, gensym("beta"), A_FLOAT, 0); //method: normalized learning rate
    class_addmethod(NLMSCC_tilde_class, (t_method)NLMSCC_tilde_gamma, gensym("gamma"), A_FLOAT, 0);   // method: dithering noise related to signal

    //class_sethelpsymbol(NLMSCC_tilde_class, gensym("iemhelp2/NLMSCC~"));
}
