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

#include "ADDS_21161_EzKit.h"
#include "audiomodule_spt.h"
#include "AudioModule - Delay_ST.h"
#include "AudioModule - Amplitude_Env.h"
#include <def21161.h>

/* Guitar Distortion */

#define SAMPLING_FREQ	48000.0



#define	INPUT_GAIN		(4.0)
#define	PREFILTER_GAIN	(2.0)

#define	POS_CLIP		(0.08)
#define	NEG_CLIP		(-0.08)


/* 	These delay values must be less than 1000 as that is the amount of memory allocated for each */

#define	DELAY_LINE_0_LEN	(200)
#define	DELAY_LINE_1_LEN	(743)
#define	DELAY_LINE_2_LEN	(431)

#define	DELAY_LINE_0_FB		(0.4)
#define	DELAY_LINE_1_FB		(-0.7)
#define	DELAY_LINE_2_FB		(-0.6)


/* 	Extremely low center frequencies and small widths can cause the filters to become unstable.  If there is no sound coming out, one of the filters
	has probably become unstable - increase the filter widths to correct */
#define	FILTER_CF_0			(1600.0)
#define	FILTER_CF_1			(350.0)
#define	FILTER_CF_2			(500.0)

#define FITLER_WIDTH_0		(150.0)
#define FITLER_WIDTH_1		(150.0)
#define FITLER_WIDTH_2		(150.0)



float iir(float sample, float __pm a_coeffs[], float __pm b_coeffs[], float __dm state[], int taps);
float	pm A_Coeffs_0[2], A_Coeffs_1[2], A_Coeffs_2[2];
float	pm B_Coeffs_0[3], B_Coeffs_1[3], B_Coeffs_2[3];
float	state0[3], state1[3], state2[3];

struct CDelay_ST				cFeedback1, cFeedback2, cFeedback3;
struct CAmplitude_Measure_Env	cAmplitude;

void	Process_Samples( int sig_int)
{
	float out_l, out_r, out_c;
	Receive_Samples();

	Left_Channel0 *= INPUT_GAIN;
	if (Left_Channel0 > POS_CLIP) Left_Channel0 = POS_CLIP;
	else if (Left_Channel0 < NEG_CLIP) Left_Channel0 = NEG_CLIP;
	Left_Channel0 *= PREFILTER_GAIN;


	/* Filter clipped signal and break up into 3 channels */
	out_c = 	iir(Left_Channel0, A_Coeffs_0, B_Coeffs_0, state0, 2);
	out_c += 	iir(Left_Channel0, A_Coeffs_1, B_Coeffs_1, state1, 2);
	out_c += 	iir(Left_Channel0, A_Coeffs_2, B_Coeffs_2, state2, 2);

	/* Delay each channel */
	out_l = xDelay_ST( &cFeedback1,	out_c );
	out_r = xDelay_ST( &cFeedback2,	out_c );
	out_c = xDelay_ST( &cFeedback3,	out_c );

	/* Merge signals and send out */
	Left_Channel0 = 	(out_l + out_c 	+ 0.1 * out_r) * 0.5;
	Right_Channel0 = 	(out_r + out_c	+ 0.1 * out_l) * 0.5;

	Transmit_Samples();
}

void	Calculate_Coefficients (	float pm * A_Coeffs,
									float pm * B_Coeffs,
									float	Freq,
									float	BW )
{
	float C,D;

	C = 1.0/tan(3.14159 * BW / SAMPLING_FREQ);
	D = 2.0 * cos(2.0 * 3.14159 * Freq);

	B_Coeffs[2] = 1.0 / (1.0 + C);
	B_Coeffs[1] = 0.0;
	B_Coeffs[0] = -B_Coeffs_0[2];

	A_Coeffs[0] = -(C-1.0) * B_Coeffs_0[2];
	A_Coeffs[1] = C * D * B_Coeffs_0[2];
}


void	main()
{
	int err;

	/* Setup Filters */
	err = Setup_Delay_ST( &cFeedback1  , 1000, 	DELAY_LINE_0_LEN, DELAY_LINE_0_FB, 1.0, "seg_heap");
	err |= Setup_Delay_ST( &cFeedback2 , 1000, 	DELAY_LINE_1_LEN, DELAY_LINE_1_FB, 1.0, "seg_heap");
	err |= Setup_Delay_ST( &cFeedback3 , 1000,	DELAY_LINE_2_LEN, DELAY_LINE_2_FB, 1.0, "seg_heap");

	/* 	if an error condition was generated, halt.  An error will be returned by Setup_Delay_ST if the specified heap does not exist
		or there is not enough room in the heap */
	if (err) while(1){}

	/* Calculate filter coefficients based on user parameters */
	Calculate_Coefficients( A_Coeffs_0, B_Coeffs_0, FILTER_CF_0/SAMPLING_FREQ, FITLER_WIDTH_0 );
	Calculate_Coefficients( A_Coeffs_1, B_Coeffs_1, FILTER_CF_1/SAMPLING_FREQ, FITLER_WIDTH_1 );
	Calculate_Coefficients( A_Coeffs_2, B_Coeffs_2, FILTER_CF_2/SAMPLING_FREQ, FITLER_WIDTH_2 );


	/* Setup Interrupt edges and flag I/O directions */
	Setup_ADSP21161N();

	/* Setup SDRAM Controller */
	Setup_SDRAM();

	Setup_AD1836();
	Init_AD1852_DACs();

	Program_SPORT02_TDM_Registers();
	Program_SPORT02_DMA_Channels();

	interruptf(	SIG_SP0I,	Process_Samples);

	*(int *) SP02MCTL |= MCE;

	for (;;)
		asm("idle;");

}