/*---------------------------------------------------------------------------*\

    FILE....: v4logagc.cpp
    TYPE....: CPP Functions
    AUTHOR..: David Rowe
    DATE....: 16/10/99

    Fixed point AGC functions.  This version of AGc was developed in response
    to Voxtron bug007 (see \ivr\voxtron\bugs for more info).  A Matlab
    simulation exists: \ivr\voxtron\bugs\bug007\agc.m

    18/4/02    IR    Modified for use with V4LOG card.

\*---------------------------------------------------------------------------*/

#include <assert.h>
#include <cstdlib>
#include <cmath>

#include "v4logagc.h"


//#define __AGCDBG__
#ifdef __AGCDBG__
FILE *f;
#endif

#define	GMAX	10
#define	GMIN	1
#define	MAXV	20000
#define	TH	2000
#define	SLOPE   -524		/* (1<<20)*(GMAX-GMIN)/(TH-MAXV)	*/
#define	HANG	800		/* 100ms hangover at 8 kHz		*/

/* state variables for each instance */

typedef struct {
    long   v;     		/* current AGC "voltage" in Q15		*/
    short  py;			/* previous output sample in Q0		*/
    short  hang;
} AGCBOX;


/*--------------------------------------------------------------------------*\

	FUNCTION.: v4log_agc_open
	AUTHOR...: David Rowe
	DATE.....: 16/10/99

	Creates and initialises the AGC state variables.
	Call at the start of each recording.

\*--------------------------------------------------------------------------*/

void v4log_agc_open(void **states)
/*  void   **states;	AGC states		*/
{
    AGCBOX  *agc;	/* ptr to AGC state variables structure	*/

    assert(states != NULL);

    *states = malloc(sizeof(AGCBOX));
    assert(*states != NULL);

    agc = (AGCBOX*)*states;
    agc->v        = 0;
    agc->py	  = 0;
    agc->hang	  = HANG;

    #ifdef __AGCDBG__
    f = fopen("mal.txt","wt");
    assert(f != NULL);
    #endif
}

/*--------------------------------------------------------------------------*\

	FUNCTION.: v4log_agc_close
	AUTHOR...: David Rowe
	DATE.....: 16/10/99

	Frees memory allocated to AGC state variables.
	Call at the end of a record session.

\*--------------------------------------------------------------------------*/

void v4log_agc_close(void *states)
/*  void   *states;	AGC states		*/
{
    assert(states != NULL);
    free(states);
    #ifdef __AGCDBG__
    fclose(f);
    #endif
}


/*--------------------------------------------------------------------------*\

	FUNCTION.: v4log_agc_apply
	AUTHOR...: David Rowe
	DATE.....: 16/10/99

	Applies AGC to a frame of speech samples.  Time constants (attack,
	decay) specify how quickly this occurs.

\*--------------------------------------------------------------------------*/

void v4log_agc_apply(void *states, short Y[], short X[], USHORT n)
/*  void   *states;	AGC states					*/
/*  short  Y[];		vector of n linear output samples		*/
/*  short  X[];		vector of n linear input samples		*/
/*  USHORT n;		length of codes[] and X[]			*/
{
    AGCBOX	*agc;	/* ptr to AGC state variables structure		*/
    short	x;	/* rectified input sample			*/
    long	v;	/* current AGC "voltage" in Q15			*/
    short	vshort;	/* Q0 version of v				*/
    long    	acc;    /* 32-bit accumulator				*/
    short	g;	/* current gain	in Q4				*/
    short	py;	/* previous output sample			*/
    short	hang;	/* hangover counter				*/
    int		i;

    /* argument validation */

    assert(states != NULL);
    assert(Y != NULL);
    assert(X != NULL);
    assert(n != 0);

    /* Initialise */

    agc = (AGCBOX*)states;
    v = agc->v;
    vshort = (short)(v >> 15);
    py = agc->py;
    hang = agc->hang;

    for(i=0; i<n; i++) {
	x = (short)fabs(py);
	if (x > vshort) {
	    /* v = 0.9*v + 0.1*x; */

	    acc = v >> 16;
	    v -= acc << 13;	/* C5x efficient way of getting 2^-3 */
	    v += ((long)x) << 12;
	    vshort = (short)(v >> 15);

	    hang = HANG;
	}
	else {
	    if (hang)
		hang--;
	    else {
		/* v = 0.999*v; */

		acc = v >> 16;
		v -= acc << 6;	/* C5x efficient way of getting 2^-10 */
		vshort = (short)(v >> 15);
	    }
	}

	if (vshort > TH) {
	    /* g = GMAX + ((float)(GMAX-GMIN)/(TH-MAXV))*(vshort-TH); */

	    acc = (long)SLOPE*(vshort-TH);	/* Q20 	*/
	    g = (short)((GMAX<<4) + (acc>>16));		/* Q4	*/
	}
	else
	    g = GMAX<<4;

	/* limit gain */

	if (g < (GMIN<<4)) g = GMIN<<4;
	if (g > (GMAX<<4)) g = GMAX<<4;

	/* apply gain to input signal */

	acc = (long)g * (long)X[i];
	acc >>= 4;

	/* limit output signal */

	if (acc > 32767)
	    acc = 32767;
	if (acc < -32767)
	    acc = -32767;
	py = (short)acc;
	Y[i] = py;

	#ifdef __AGCDBG__
	fprintf(f,"%d %d %d\n",vshort,g,Y[i]);
	#endif
    }

    /* store state variables for next iteration */

    agc->v = v;
    agc->py = py;
    agc->hang = hang;
}

