#include "chuck_dl.h"
#include "chuck_def.h"

#include <stdio.h>
#include <limits.h>
#include <math.h>

CK_DLL_CTOR(LMSLPC_ctor);
CK_DLL_DTOR(LMSLPC_dtor);

CK_DLL_MFUN(LMSLPC_setMuAlpha);
CK_DLL_MFUN(LMSLPC_getMuAlpha);
CK_DLL_MFUN(LMSLPC_setAdapt);
CK_DLL_MFUN(LMSLPC_getAdapt);
CK_DLL_MFUN(LMSLPC_setOrder);  // set order, allocate memory (default 12)
CK_DLL_MFUN(LMSLPC_getOrder);
CK_DLL_MFUN(LMSLPC_setDecimate); 
CK_DLL_MFUN(LMSLPC_getDecimate);
CK_DLL_MFUN(LMSLPC_getCoeff);
CK_DLL_MFUN(LMSLPC_getResidue);

CK_DLL_TICK(LMSLPC_tick);

t_CKINT LMSLPC_data_offset = 0;

struct LMSLPCData
{
    int adapt;       	// 1 = rate power adaptive, 0 = fixed
    float mualpha;  	// adaption rate coefficient
    
    int order;		// order, number of delays and coeffs

    int decimate;   // these variables can be used for decimation
    int deccount;   // to run at a lower sample rate, if you care to

    float *coeff;		// predictor filter coefficient buffer
    float *buffer; 		// holds our delayed sample buffer

    float power;		// used for adaptive lpc (mu)
    float error;		// residue (prediction error)

};

CK_DLL_QUERY(LMSLPC)
{
    QUERY->setname(QUERY, "LMSLPC");
    
    QUERY->begin_class(QUERY, "LMSLPC", "UGen");
    
    QUERY->add_ctor(QUERY, LMSLPC_ctor);
    QUERY->add_dtor(QUERY, LMSLPC_dtor);
    
    QUERY->add_ugen_func(QUERY, LMSLPC_tick, NULL, 1, 1);
    
    QUERY->add_mfun(QUERY, LMSLPC_setMuAlpha, "float", "mualpha");
    QUERY->add_arg(QUERY, "float", "arg");
    
    QUERY->add_mfun(QUERY, LMSLPC_getMuAlpha, "float", "mualpha");

    QUERY->add_mfun(QUERY, LMSLPC_setAdapt, "int", "adapt");
    QUERY->add_arg(QUERY, "int", "arg");

    QUERY->add_mfun(QUERY, LMSLPC_getAdapt, "int", "adapt");

    QUERY->add_mfun(QUERY, LMSLPC_setOrder, "int", "order");
    QUERY->add_arg(QUERY, "int", "arg");

    QUERY->add_mfun(QUERY, LMSLPC_getOrder, "int", "order");

    QUERY->add_mfun(QUERY, LMSLPC_setDecimate, "int", "decimate");
    QUERY->add_arg(QUERY, "int", "arg");

    QUERY->add_mfun(QUERY, LMSLPC_getDecimate, "int", "decimate");

    QUERY->add_mfun(QUERY, LMSLPC_getCoeff, "float", "coeff");
    QUERY->add_arg(QUERY, "int", "arg");

    QUERY->add_mfun(QUERY, LMSLPC_getResidue, "float", "residue");

    LMSLPC_data_offset = QUERY->add_mvar(QUERY, "int", "@lpc_data", false);
    
    QUERY->end_class(QUERY);

    return TRUE;
}

CK_DLL_CTOR(LMSLPC_ctor)
{
    OBJ_MEMBER_INT(SELF, LMSLPC_data_offset) = 0;
    
    LMSLPCData * lpcdata = new LMSLPCData;
    lpcdata->mualpha = 0.1;
    lpcdata->adapt = 0;

    lpcdata->order = 12;  // default, can reset after construct
    lpcdata->decimate = 1;
    lpcdata->deccount = 0;
    
    lpcdata->coeff = (float *) malloc(sizeof(float) * lpcdata->order);
    lpcdata->buffer = (float *) malloc(sizeof(float) * lpcdata->order);

    float temp = 1.0 / lpcdata->order;
    for (int i = 0; i < lpcdata->order; i++)  {
        lpcdata->coeff[i] = temp; // fill with moving average
        lpcdata->buffer[i] = 0.0;
    }

    OBJ_MEMBER_INT(SELF, LMSLPC_data_offset) = (t_CKINT) lpcdata;
}

CK_DLL_DTOR(LMSLPC_dtor)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    if(lpcdata)
    {
        free(lpcdata->coeff);
        free(lpcdata->buffer);
        delete lpcdata;
        OBJ_MEMBER_INT(SELF, LMSLPC_data_offset) = 0;
        lpcdata = NULL;
    }
}

CK_DLL_TICK(LMSLPC_tick)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    
    int i;
    float clip = lpcdata->order; // attempt to keep things reasonable

    lpcdata->deccount = lpcdata->deccount - 1;
    if (lpcdata->deccount <= 0)  {
    	*out = 0.0;
    	lpcdata->power = 0.0;

    	for (i = 0; i < 12; i++)  {                         // run filter
            *out += lpcdata->coeff[i]*lpcdata->buffer[i];   // calculate our prediction
            if (lpcdata->adapt) {  // and collect power too if we're gonna use it
		lpcdata->power += lpcdata->buffer[i]*lpcdata->buffer[i]; 
	    }
    	}
	lpcdata->power = lpcdata->power / lpcdata->order; // do we need to do this?
    	lpcdata->error = in - *out;             // calculate prediction error
	if (lpcdata->adapt & (lpcdata->power > 0.001)) { // arbitrary floor for normalize
	    lpcdata->error = lpcdata->error / lpcdata->power;
	}
    	for (i = 0; i < lpcdata->order; i++)  {             // adapt our coefficients
	    lpcdata->coeff[i] += lpcdata->error * lpcdata->mualpha * lpcdata->buffer[i];
            if (lpcdata->coeff[i] > clip)  lpcdata->coeff[i] = clip;  // explosion proofing
            if (lpcdata->coeff[i] < -clip) lpcdata->coeff[i] = -clip;  // on coeffs
    	}
    	for (i = lpcdata->order-1; i > 0; i--)  {
            lpcdata->buffer[i] = lpcdata->buffer[i-1];
    	}
    	lpcdata->buffer[0] = in;
    	if (*out < -1.0) *out = -1.0;  // explosion proofing on output
    	if (*out > 1.0) *out = 1.0;    // (at least it doesn't wedge our output)
	
	lpcdata->deccount = lpcdata->decimate;	// reset our decimation counter
    }

    return TRUE;
}

CK_DLL_MFUN(LMSLPC_setAdapt)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    // TODO: sanity check
    lpcdata->adapt = GET_NEXT_INT(ARGS);
    RETURN->v_int = lpcdata->adapt;
}

CK_DLL_MFUN(LMSLPC_getAdapt)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    RETURN->v_int = lpcdata->adapt;
}

CK_DLL_MFUN(LMSLPC_setOrder)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    // TODO: sanity check
    lpcdata->order = GET_NEXT_INT(ARGS);

    free(lpcdata->coeff);
    free(lpcdata->buffer);

    int i;

    lpcdata->coeff = (float *) malloc(sizeof(float) * lpcdata->order);
    lpcdata->buffer = (float *) malloc(sizeof(float) * lpcdata->order);

    float temp = 1.0 / lpcdata->order;
    for (i = 0; i < lpcdata->order; i++)  {
        lpcdata->coeff[i] = temp; // fill with moving average
        lpcdata->buffer[i] = 0.0;
    }

    RETURN->v_int = lpcdata->order;
}

CK_DLL_MFUN(LMSLPC_getOrder)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    RETURN->v_int = lpcdata->order;
}

CK_DLL_MFUN(LMSLPC_setDecimate)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    // TODO: sanity check
    lpcdata->decimate = GET_NEXT_INT(ARGS);
    if (lpcdata->decimate < 1) {
	lpcdata->decimate = 1;
	printf("Hey, can't decimate less than 1!  Setting to 1.\n");
    }
    RETURN->v_int = lpcdata->decimate;
}

CK_DLL_MFUN(LMSLPC_getDecimate)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    RETURN->v_int = lpcdata->decimate;
}

CK_DLL_MFUN(LMSLPC_setMuAlpha)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    // TODO: sanity check
    lpcdata->mualpha = GET_NEXT_FLOAT(ARGS);
    RETURN->v_float = lpcdata->mualpha;
}

CK_DLL_MFUN(LMSLPC_getMuAlpha)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    RETURN->v_float = lpcdata->mualpha;
}


CK_DLL_MFUN(LMSLPC_getCoeff)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    RETURN->v_float = lpcdata->coeff[GET_NEXT_INT(ARGS)];
}

CK_DLL_MFUN(LMSLPC_getResidue)
{
    LMSLPCData * lpcdata = (LMSLPCData *) OBJ_MEMBER_INT(SELF, LMSLPC_data_offset);
    RETURN->v_float = lpcdata->error;
}