/* 29/01/12	version ver 2													*/
/* 02 Jan 2025 updates for aDAU1467                                         */
/* Changes:																	*/
/*	Hack ad1940 code base												    */
/* 																			*/
/* known Issues:            												*/
/* 				  use of arrays is massively code inefficient - fix needed  */



#include <xc.h>
#include <sys/attribs.h>
#include "definitions.h"
#include "Digital_Preamp.h"
#include "ADAU_DRV.h"
#include "Digital_Preamp_IC_1.h"
#include "Digital_Preamp_IC_1_PARAM.h"
#include "Digital_Preamp_IC_1_REG.h"
#include "Digital_Preamp_Parm_Map.h"
#include <math.h>


/* Make this global to this function to save RAM */
int Data_Counter = 0;
double Alpha;
double W0;
double A0, A0_2, IIR_B0_B1_B2_A1_A2[5], IIR2_B0_B1_B2_A1_A2[5];
double TempB0, TempB1;
long int Fixed_Point;
extern Digital_Preamp_DATA PreampData;

//static unsigned char	Read_Data_Array[6];						/* Array used to hold data read during debug process */



/* make this a function, as we need to use a single safeload write function...*/
long int FloatToFixed (double Float_No)
{
long int Fixed_No;

if (Float_No < 0)  				
	{									/* Integer part is float *1^ 23  */
	Fixed_No = -1*(long int)(Float_No * 0x1000000);
	Fixed_No = 0x100000000 - Fixed_No;
	}
else
	{									/* Integer part is float *1^ 23  */
	Fixed_No = (long int)(Float_No *  0x1000000);
	}
Fixed_No = Fixed_No &0xFFFFFFFF;
return Fixed_No;
}



/****************************************************************************************/
/* this will be done a few times - less RAM to make this a function call				*/
/* This assumes that CSEL is already active 											*/
/****************************************************************************************/
void WriteSPI(long int Data_Word, char bits)
{
    unsigned long int Write_Buff;

    do
    {
        Write_Buff = (Data_Word >> (bits-8)) & 0xff;
        SPI1_Write(&Write_Buff,1); //writes 1 bytes
        bits -= 8;     /* decrement bits by one bytes worth */
    } while (bits > 0);
    CORETIMER_DelayUs(Block_Write_Delay_Between_Words_uS);    
}


/****************************************************************************************/
/* Input																				*/
/*		devAddress: 		the SPI Address to be read from								*/
/*		Address_to_Read: 	the Address in RAM to be read (16 bits) 					*/
/*		Number_of_Words: 	the number of words											*/
/*		*ptr_to_Data: 		pointer to an array of CHAR in RAM for read data			*/
/*	Output																				*/
/*		Changes data pointed to by *prt_to_Data											*/
/*	Returns																				*/
/*		none																			*/
/****************************************************************************************/
void ReadSPI(char devAddress, int Address_to_Read, int Number_of_Words, unsigned char *ptr_to_Data)
{
int Temp_Addr = ADAU_SPI_Read_Addr;
int Data_Counter, Temp_Data;

ADAU_SS_Enable;  											/* Set CSEL Low */

    SPI1_Write(&Temp_Addr, 1);          //write the address with read bit set
    Write_16_Bits_Addr(Address_to_Read);
            
	for (Data_Counter = 0; Data_Counter < Number_of_Words; Data_Counter++)
		{ 
		Temp_Data = 0;          		/* clear temp data holder */
        SPI1_Read(&Temp_Data, 1);          // read byte into the temp store
        CORETIMER_DelayUs(Block_Write_Delay_Between_Words_uS);
		*ptr_to_Data = Temp_Data;
		ptr_to_Data++;
		}
ADAU_SS_Disable;  											/* Set CSEL High */
}



/* Writes data to multiple Device registers    */
/* Note This function is for ROM data only!!!  */
void SIGMA_WRITE_REGISTER_BLOCK(char devAddress, unsigned int Address_to_Write, unsigned int length, const unsigned char *ptr_to_Data) 
{
unsigned int counter;
const unsigned char *cPointer;

cPointer = ptr_to_Data;     /* point local variable to the array of data to be written */

ADAU_SS_Enable;  											/* Set CSEL Low */
	Write_SPI_Addr((char)devAddress, Write_To_SPI); /* Address the DSP */
	Write_16_Bits_Addr(Address_to_Write);          	/* Provide the address of the base of program data */
											  		/* And now do a burst transfer for all program data */
	for(counter = 0; counter < length; counter++)
		{
        Write_8_Bits(*cPointer);
		cPointer++;
		}
ADAU_SS_Disable;         									/* Set CSel bit High  */
CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);
} 


/* This function writes zeroes in for the large null data arrays */
SIGMA_WRITE_REGISTER_BLOCK_NULL(char devAddress, unsigned int Address_to_Write, unsigned int length, const unsigned char Data) 
{
unsigned int counter;

ADAU_SS_Enable;  											/* Set CSEL Low */
	Write_SPI_Addr((char)devAddress, Write_To_SPI); /* Address the DSP */
	Write_16_Bits_Addr(Address_to_Write);          	/* Provide the address of the base of program data */
											  		/* And now do a burst transfer for all program data */
	for(counter = 0; counter < length; counter++)
		{
        Write_8_Bits(0);
		}
ADAU_SS_Disable;         									/* Set CSel bit High  */
CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);
    
}





/* Writes data to a Single Device register */
void SIGMA_WRITE_SINGLE_REGISTER(char devAddress, unsigned int address, int length, long int Data) 
{

ADAU_SS_Enable;  										/* Set CSEL Low */
	Write_SPI_Addr((char)devAddress, Write_To_SPI);     /* Address the DSP */
	Write_16_Bits_Addr(address);          				/* Provide the address of the base of program data */
	Write_32_Bits(Data);					  		    /* And now write the data */
ADAU_SS_Disable;         								/* Set CSel bit High  */

CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);

} 


/* Writes data to a Single Device register */
void SIGMA_WRITE_DOUBLE_REGISTER(char devAddress, unsigned int address, int length, long int Data_0, long int Data_1) 
{

ADAU_SS_Enable;  										/* Set CSEL Low */
	Write_SPI_Addr((char)devAddress, Write_To_SPI);     /* Address the DSP */
	Write_16_Bits_Addr(address);          				/* Provide the address of the base of program data */
	Write_32_Bits(Data_0);					  		    /* And now write the first word of data */
	Write_32_Bits(Data_1);					  		    /* And now write the second word on data */
ADAU_SS_Disable;         								/* Set CSel bit High  */

CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);

} 




/* this will be done a few times - less RAM to make this a function call        */
/* This writes to page zero (first page of memory)                              */
/* format for ADAU1467 is                                                       */
/*                                                                              */
/*  Base addr (nominally 0x6000 but not guaranteed )                            */
/*      0X6000  Data_SafeLoad[0]        Slot 0 for safeload                     */
/*      0X6001  Data_SafeLoad[1]        Slot 1 for safeload                     */
/*      0X6002  Data_SafeLoad[2]        Slot 2 for safeload                     */
/*      0X6003  Data_SafeLoad[3]        Slot 3 for safeload                     */
/*      0X6004  Data_SafeLoad[4]        Slot 4 for safeload                     */
/*      0X6005  address_SafeLoad        Target Addr for slot 0                  */
/*      0X6006  num_SafeLoad_Lower     Num of items to safeload in page 1 = 0   */
/*      0X6006  num_SafeLoad_Upper     Num of items to safeload in page 2 = 1   */
/*                                                                              */
/*  IOn this cvase num = 1 as it is a single parameter                          */
void SafeLoad_Write_Single_Parameter(unsigned int Address, long int Data,int Bank)
{
/* write the data */
ADAU_SS_Enable;  					 			/* Set CSEL Low */
	Write_SPI_Addr((char)ADAU1467_Address, Write_To_SPI);   		 	/* Address the ADAU1467 */
	Write_16_Bits_Addr((unsigned int)ADAU1467_Safeload_Base_Parameter);		/* Provide the address of the indirect safeload parameter register */
	Write_32_Bits(Data);											/* Write data to the ADAU1467 */
	Write_32_Bits((long int)0);										/* Write data to the ADAU1467 */
	Write_32_Bits((long int)0);										/* Write data to the ADAU1467 */
	Write_32_Bits((long int)0);										/* Write data to the ADAU1467 */
	Write_32_Bits((long int)0);										/* Write data to the ADAU1467 */
	Write_32_Bits((long int) (Address));                            /* Provide the destination address for the data*/
    if (Bank == 0){
        Write_32_Bits((long int)1);										/* Triggers safeload of one paramater */
        Write_32_Bits((long int)0);										/* Triggers safeload of one paramater */
    }
    else{
        Write_32_Bits((long int)0);										/* Triggers safeload of one paramater */
        Write_32_Bits((long int)1);										/* Triggers safeload of one paramater */
    }
    
CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);
ADAU_SS_Disable;								/* Set CSEL High */
CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);
}


/* this will be done a few times - less RAM to make this a function call                    */
/* refer above to safeload single for data format                                           */
/* this assumes the data is in Page 1 which is true for the app we have here                */
void SafeLoad_Write_IIR_Parms(unsigned int Base_Address, long int W_B2,long int W_B1,long int W_B0,long int W_A2,long int W_A1)
{

/* write the data */
ADAU_SS_Enable;  					 			/* Set CSEL Low */
	Write_SPI_Addr((char)ADAU1467_Address, Write_To_SPI);   		 	/* Address the ADAU1442 */
	Write_16_Bits((unsigned int)ADAU1467_Safeload_Base_Parameter);		/* Provide the address of the indirect safeload parameter register */
	Write_32_Bits(W_B2);										/* Write data to the DSP */
	Write_32_Bits(W_B1);										/* Write data to the DSP */
	Write_32_Bits(W_B0);										/* Write data to the DSP */
	Write_32_Bits(W_A2);										/* Write data to the DSP */
	Write_32_Bits(W_A1);										/* Write data to the DSP */
	Write_32_Bits((long int) (Base_Address));                   /* Provide the destination address for the data */
	Write_32_Bits((long int)(5));								/* Triggers safeload of five paramaters */
	Write_32_Bits((long int)(0));								/* Triggers safeload of five paramaters */
    CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);

ADAU_SS_Disable;								/* Set CSEL High */

/* the AUAD1467 needs a minimum 1 cycle delay between updates */
CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);
}


/*  Base addr (nominally 0x6000 but not guaranteed )                            */
/*      0X6000  Data_SafeLoad[0]        Slot 0 for safeload                     */
/*      0X6001  Data_SafeLoad[1]        Slot 1 for safeload                     */
/*      0X6002  Data_SafeLoad[2]        Slot 2 for safeload                     */
/*      0X6003  Data_SafeLoad[3]        Slot 3 for safeload                     */
/*      0X6004  Data_SafeLoad[4]        Slot 4 for safeload                     */
/*      0X6005  address_SafeLoad        Target Addr for slot 0                  */
/*      0X6006  num_SafeLoad_Lower     Num of items to safeload in page 1 = 0   */
/*      0X6006  num_SafeLoad_Upper     Num of items to safeload in page 2 = 1   */
/*      Bank 1 or Bank 2 determins which bank written to                        */
/* refer above to safeload single for data format                                           */
/* this assumes the data is in Page 1 which is true for the app we have here                */
void SafeLoad_Write_General_Parms(unsigned int Base_Address, long int D0,long int D1,long int D2,long int D3,long int D4, int Num_Data, int Bank)
{

/* write the data */
ADAU_SS_Enable;  					 			/* Set CSEL Low */
	Write_SPI_Addr((char)ADAU1467_Address, Write_To_SPI);   		 	/* Address the ADAU1442 */
	Write_16_Bits((unsigned int)ADAU1467_Safeload_Base_Parameter);		/* Provide the address of the indirect safeload parameter register */
	Write_32_Bits(D0);										/* Write data to the DSP */
	Write_32_Bits(D1);										/* Write data to the DSP */
	Write_32_Bits(D2);										/* Write data to the DSP */
	Write_32_Bits(D3);										/* Write data to the DSP */
	Write_32_Bits(D4);										/* Write data to the DSP */
	Write_32_Bits((long int) (Base_Address));               /* Provide the destination address for the data */
    if (Bank == 0)  {
        Write_32_Bits((long int)(Num_Data));				/* Triggers safeload of Num_Data paramaters */
        Write_32_Bits((long int)(0));						/* Triggers safeload write */
    }
    else  {
        Write_32_Bits((long int)(0));						/* Triggers nmo data to bank 1 */
        Write_32_Bits((long int)(Num_Data));				/* Triggers safeload of Num_Data paramaters to B2 */
    }
CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);
ADAU_SS_Disable;								/* Set CSEL High */

/* the AUAD1467 needs a minimum 1 cycle delay between updates */
CORETIMER_DelayUs(More_Than_1_Audio_Frame_uS);
}





/* 
this function is to do the following:
-0- Init registers for configuration of DSP board as built
	- Check source of A/D input (this might in fact be from elsewhere!)
*/
/*	- ADAU1442
		- Hold RESET line active (low) until things settle down
		- WAIT for 256 * MCLK after reset before doing anything! This is about 20uS.
		- Toggle CSEL three times
		- Run through loading registers and RAM and Parameters as per datasheet - if you can make sense of it!!!
*/
void Init_DSP(void)
{
//long int Temp_Data;
int counter, Data_RX[4];

/* OK, OK, the DSP should already be in reset - but this is to catch errors if this function is called out of context */
RESET_DSP_Subsys;
CORETIMER_DelayMs(Delay_For_Reset_Ms);
CLR_RESET_DSP_Subsys;

/* Wait about 100mS for things to settle down in the DSP - RESET is low at this point - refer init I/O */
/* Minimum allowable time is 10ms */
CORETIMER_DelayMs(Delay_For_Reset_Settle_Ms); 
/* toggle CSEL three times minimum - this puts the ADAU1442 into SPI mode*/
for (counter = 0; counter < 3; counter++)
	{
        CORETIMER_DelayMs(ADAU_SS_Delay_Boot);
        ADAU_SS_Enable;
        CORETIMER_DelayMs(ADAU_SS_Delay_Boot);
        ADAU_SS_Disable;
    }

/* Wait about 100mS for things to settle down in the DSP - RESET is low at this point - refer init I/O */
CORETIMER_DelayMs(Delay_For_Reset_Settle_Ms); 
/* this lot is straight from sigma studio and can probably be called in their function - once I have it working;) */
    /****************************   This is a hack to avoid loading all the large array of zeroes into ROM ***********************************/
    /* This fills the range with zeroes - which is what the proper array is mostly*/
    /* Then we rewrite the start with the actual data that is not zeroes          */
    /* If you recompile and load the ADUA application you NEED to check and fix this*/

	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOFT_RESET_IC_1_ADDR, REG_SOFT_RESET_IC_1_BYTE, R0_SOFT_RESET_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOFT_RESET_IC_1_ADDR, REG_SOFT_RESET_IC_1_BYTE, R1_SOFT_RESET_IC_1_Default );
	SIGMA_WRITE_DELAY( DEVICE_ADDR_IC_1, R2_RESET_DELAY_IC_1_SIZE, R2_RESET_DELAY_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_HIBERNATE_IC_1_ADDR, REG_HIBERNATE_IC_1_BYTE, R3_HIBERNATE_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_HIBERNATE_IC_1_ADDR, REG_HIBERNATE_IC_1_BYTE, R4_HIBERNATE_IC_1_Default );
	SIGMA_WRITE_DELAY( DEVICE_ADDR_IC_1, R5_HIBERNATE_DELAY_IC_1_SIZE, R5_HIBERNATE_DELAY_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_KILL_CORE_IC_1_ADDR, REG_KILL_CORE_IC_1_BYTE, R6_KILL_CORE_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_KILL_CORE_IC_1_ADDR, REG_KILL_CORE_IC_1_BYTE, R7_KILL_CORE_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_PLL_ENABLE_IC_1_ADDR, REG_PLL_ENABLE_IC_1_BYTE, R8_PLL_ENABLE_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_PLL_CTRL1_IC_1_ADDR, REG_PLL_CTRL1_IC_1_BYTE, R9_PLL_CTRL1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_PLL_CLK_SRC_IC_1_ADDR, REG_PLL_CLK_SRC_IC_1_BYTE, R10_PLL_CLK_SRC_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_MCLK_OUT_IC_1_ADDR, REG_MCLK_OUT_IC_1_BYTE, R11_MCLK_OUT_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_PLL_ENABLE_IC_1_ADDR, REG_PLL_ENABLE_IC_1_BYTE, R12_PLL_ENABLE_IC_1_Default );
	SIGMA_WRITE_DELAY( DEVICE_ADDR_IC_1, R13_PLL_LOCK_DELAY_IC_1_SIZE, R13_PLL_LOCK_DELAY_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_POWER_ENABLE0_IC_1_ADDR, REG_POWER_ENABLE0_IC_1_BYTE, R14_POWER_ENABLE0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_POWER_ENABLE1_IC_1_ADDR, REG_POWER_ENABLE1_IC_1_BYTE, R15_POWER_ENABLE1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_CLK_GEN1_M_IC_1_ADDR, REG_CLK_GEN1_M_IC_1_BYTE, R16_CLK_GEN1_M_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_CLK_GEN1_N_IC_1_ADDR, REG_CLK_GEN1_N_IC_1_BYTE, R17_CLK_GEN1_N_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_CLK_GEN2_M_IC_1_ADDR, REG_CLK_GEN2_M_IC_1_BYTE, R18_CLK_GEN2_M_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_CLK_GEN2_N_IC_1_ADDR, REG_CLK_GEN2_N_IC_1_BYTE, R19_CLK_GEN2_N_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_CLK_GEN3_M_IC_1_ADDR, REG_CLK_GEN3_M_IC_1_BYTE, R20_CLK_GEN3_M_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_CLK_GEN3_N_IC_1_ADDR, REG_CLK_GEN3_N_IC_1_BYTE, R21_CLK_GEN3_N_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_BCLK_IN0_PIN_IC_1_ADDR, REG_BCLK_IN0_PIN_IC_1_BYTE, R22_BCLK_IN0_PIN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_BCLK_OUT0_PIN_IC_1_ADDR, REG_BCLK_OUT0_PIN_IC_1_BYTE, R23_BCLK_OUT0_PIN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_LRCLK_IN0_PIN_IC_1_ADDR, REG_LRCLK_IN0_PIN_IC_1_BYTE, R24_LRCLK_IN0_PIN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_LRCLK_OUT0_PIN_IC_1_ADDR, REG_LRCLK_OUT0_PIN_IC_1_BYTE, R25_LRCLK_OUT0_PIN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SDATA_OUT0_PIN_IC_1_ADDR, REG_SDATA_OUT0_PIN_IC_1_BYTE, R26_SDATA_OUT0_PIN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SDATA_OUT1_PIN_IC_1_ADDR, REG_SDATA_OUT1_PIN_IC_1_BYTE, R27_SDATA_OUT1_PIN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SDATA_OUT2_PIN_IC_1_ADDR, REG_SDATA_OUT2_PIN_IC_1_BYTE, R28_SDATA_OUT2_PIN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SDATA_OUT3_PIN_IC_1_ADDR, REG_SDATA_OUT3_PIN_IC_1_BYTE, R29_SDATA_OUT3_PIN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_ASRC_INPUT0_IC_1_ADDR, REG_ASRC_INPUT0_IC_1_BYTE, R30_ASRC_INPUT0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_ASRC_INPUT1_IC_1_ADDR, REG_ASRC_INPUT1_IC_1_BYTE, R31_ASRC_INPUT1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_ASRC_INPUT2_IC_1_ADDR, REG_ASRC_INPUT2_IC_1_BYTE, R32_ASRC_INPUT2_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_ASRC_INPUT3_IC_1_ADDR, REG_ASRC_INPUT3_IC_1_BYTE, R33_ASRC_INPUT3_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_ASRC_INPUT4_IC_1_ADDR, REG_ASRC_INPUT4_IC_1_BYTE, R34_ASRC_INPUT4_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_ASRC_OUT_RATE0_IC_1_ADDR, REG_ASRC_OUT_RATE0_IC_1_BYTE, R35_ASRC_OUT_RATE0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_ASRC_OUT_RATE1_IC_1_ADDR, REG_ASRC_OUT_RATE1_IC_1_BYTE, R36_ASRC_OUT_RATE1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_ASRC_OUT_RATE3_IC_1_ADDR, REG_ASRC_OUT_RATE3_IC_1_BYTE, R37_ASRC_OUT_RATE3_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_ASRC_OUT_RATE4_IC_1_ADDR, REG_ASRC_OUT_RATE4_IC_1_BYTE, R38_ASRC_OUT_RATE4_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE0_IC_1_ADDR, REG_SOUT_SOURCE0_IC_1_BYTE, R39_SOUT_SOURCE0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE1_IC_1_ADDR, REG_SOUT_SOURCE1_IC_1_BYTE, R40_SOUT_SOURCE1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE2_IC_1_ADDR, REG_SOUT_SOURCE2_IC_1_BYTE, R41_SOUT_SOURCE2_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE3_IC_1_ADDR, REG_SOUT_SOURCE3_IC_1_BYTE, R42_SOUT_SOURCE3_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE4_IC_1_ADDR, REG_SOUT_SOURCE4_IC_1_BYTE, R43_SOUT_SOURCE4_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE5_IC_1_ADDR, REG_SOUT_SOURCE5_IC_1_BYTE, R44_SOUT_SOURCE5_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE6_IC_1_ADDR, REG_SOUT_SOURCE6_IC_1_BYTE, R45_SOUT_SOURCE6_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE7_IC_1_ADDR, REG_SOUT_SOURCE7_IC_1_BYTE, R46_SOUT_SOURCE7_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE8_IC_1_ADDR, REG_SOUT_SOURCE8_IC_1_BYTE, R47_SOUT_SOURCE8_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE9_IC_1_ADDR, REG_SOUT_SOURCE9_IC_1_BYTE, R48_SOUT_SOURCE9_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE10_IC_1_ADDR, REG_SOUT_SOURCE10_IC_1_BYTE, R49_SOUT_SOURCE10_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE11_IC_1_ADDR, REG_SOUT_SOURCE11_IC_1_BYTE, R50_SOUT_SOURCE11_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE12_IC_1_ADDR, REG_SOUT_SOURCE12_IC_1_BYTE, R51_SOUT_SOURCE12_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE13_IC_1_ADDR, REG_SOUT_SOURCE13_IC_1_BYTE, R52_SOUT_SOURCE13_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE14_IC_1_ADDR, REG_SOUT_SOURCE14_IC_1_BYTE, R53_SOUT_SOURCE14_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE15_IC_1_ADDR, REG_SOUT_SOURCE15_IC_1_BYTE, R54_SOUT_SOURCE15_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE16_IC_1_ADDR, REG_SOUT_SOURCE16_IC_1_BYTE, R55_SOUT_SOURCE16_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE17_IC_1_ADDR, REG_SOUT_SOURCE17_IC_1_BYTE, R56_SOUT_SOURCE17_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE18_IC_1_ADDR, REG_SOUT_SOURCE18_IC_1_BYTE, R57_SOUT_SOURCE18_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE19_IC_1_ADDR, REG_SOUT_SOURCE19_IC_1_BYTE, R58_SOUT_SOURCE19_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE20_IC_1_ADDR, REG_SOUT_SOURCE20_IC_1_BYTE, R59_SOUT_SOURCE20_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE21_IC_1_ADDR, REG_SOUT_SOURCE21_IC_1_BYTE, R60_SOUT_SOURCE21_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE22_IC_1_ADDR, REG_SOUT_SOURCE22_IC_1_BYTE, R61_SOUT_SOURCE22_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SOUT_SOURCE23_IC_1_ADDR, REG_SOUT_SOURCE23_IC_1_BYTE, R62_SOUT_SOURCE23_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SPDIFTX_INPUT_IC_1_ADDR, REG_SPDIFTX_INPUT_IC_1_BYTE, R63_SPDIFTX_INPUT_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SERIAL_BYTE_0_0_IC_1_ADDR, REG_SERIAL_BYTE_0_0_IC_1_BYTE, R64_SERIAL_BYTE_0_0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SERIAL_BYTE_1_0_IC_1_ADDR, REG_SERIAL_BYTE_1_0_IC_1_BYTE, R65_SERIAL_BYTE_1_0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SERIAL_BYTE_2_0_IC_1_ADDR, REG_SERIAL_BYTE_2_0_IC_1_BYTE, R66_SERIAL_BYTE_2_0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SERIAL_BYTE_3_0_IC_1_ADDR, REG_SERIAL_BYTE_3_0_IC_1_BYTE, R67_SERIAL_BYTE_3_0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SERIAL_BYTE_4_0_IC_1_ADDR, REG_SERIAL_BYTE_4_0_IC_1_BYTE, R68_SERIAL_BYTE_4_0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SERIAL_BYTE_5_0_IC_1_ADDR, REG_SERIAL_BYTE_5_0_IC_1_BYTE, R69_SERIAL_BYTE_5_0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SERIAL_BYTE_6_0_IC_1_ADDR, REG_SERIAL_BYTE_6_0_IC_1_BYTE, R70_SERIAL_BYTE_6_0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SERIAL_BYTE_7_0_IC_1_ADDR, REG_SERIAL_BYTE_7_0_IC_1_BYTE, R71_SERIAL_BYTE_7_0_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SPDIF_RESTART_IC_1_ADDR, REG_SPDIF_RESTART_IC_1_BYTE, R72_SPDIF_RESTART_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SPDIF_TX_EN_IC_1_ADDR, REG_SPDIF_TX_EN_IC_1_BYTE, R73_SPDIF_TX_EN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_MP6_MODE1_IC_1_ADDR, REG_MP6_MODE1_IC_1_BYTE, R74_MP6_MODE1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_MP7_MODE1_IC_1_ADDR, REG_MP7_MODE1_IC_1_BYTE, R75_MP7_MODE1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_MP6_WRITE1_IC_1_ADDR, REG_MP6_WRITE1_IC_1_BYTE, R76_MP6_WRITE1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SECONDARY_I2C_IC_1_ADDR, REG_SECONDARY_I2C_IC_1_BYTE, R77_SECONDARY_I2C_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SDATA_IO3_PIN_IC_1_ADDR, REG_SDATA_IO3_PIN_IC_1_BYTE, R78_SDATA_IO3_PIN_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SDATA_4_ROUTE_IC_1_ADDR, REG_SDATA_4_ROUTE_IC_1_BYTE, R79_SDATA_4_ROUTE_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, PROGRAM_ADDR_IC_1, PROGRAM_SIZE_IC_1, Program_Data_IC_1 );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, PARAM_ADDR_IC_1, PARAM_SIZE_IC_1, Param_Data_IC_1 );

    SIGMA_WRITE_REGISTER_BLOCK_NULL( DEVICE_ADDR_IC_1, R82_IC_1_IC_1_ADDR, R82_IC_1_IC_1_SIZE, 0 );
    SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, R82_IC_1_IC_1_ADDR, R82_IC_1_IC_1_Hacked_SIZE, R82_IC_1_IC_1_Hacked_Default );  
    //	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, R82_IC_1_IC_1_ADDR, R82_IC_1_IC_1_SIZE, R82_IC_1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SECOND_PAGE_ENABLE_IC_1_ADDR, REG_SECOND_PAGE_ENABLE_IC_1_BYTE, R83_SECOND_PAGE_ENABLE_IC_1_Default );

    SIGMA_WRITE_REGISTER_BLOCK_NULL( DEVICE_ADDR_IC_1, R84_IC_1_IC_1_ADDR, R84_IC_1_IC_1_SIZE, 0);
	//  SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, R84_IC_1_IC_1_ADDR, R84_IC_1_IC_1_SIZE, R84_IC_1_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_SECOND_PAGE_ENABLE_IC_1_ADDR, REG_SECOND_PAGE_ENABLE_IC_1_BYTE, R85_SECOND_PAGE_ENABLE_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_KILL_CORE_IC_1_ADDR, REG_KILL_CORE_IC_1_BYTE, R86_KILL_CORE_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_START_ADDRESS_IC_1_ADDR, REG_START_ADDRESS_IC_1_BYTE, R87_START_ADDRESS_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_START_PULSE_IC_1_ADDR, REG_START_PULSE_IC_1_BYTE, R88_START_PULSE_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_START_CORE_IC_1_ADDR, REG_START_CORE_IC_1_BYTE, R89_START_CORE_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_START_CORE_IC_1_ADDR, REG_START_CORE_IC_1_BYTE, R90_START_CORE_IC_1_Default );
	SIGMA_WRITE_DELAY( DEVICE_ADDR_IC_1, R91_START_DELAY_IC_1_SIZE, R91_START_DELAY_IC_1_Default );
	SIGMA_WRITE_REGISTER_BLOCK( DEVICE_ADDR_IC_1, REG_HIBERNATE_IC_1_ADDR, REG_HIBERNATE_IC_1_BYTE, R92_HIBERNATE_IC_1_Default );

CORETIMER_DelayMs(30);										/* then wait 30ms */ 
}



/*
Write_Filter_Parm() takes a given Band, Frequency, XO type, Sample Rate and programs the DSP for this
	- For safeload leave bit 9 and 6 as is
	- Program Parameters using SAFELOAD registers
	- Set Bit 4 of Core_Ctrl high - clears itself - initialtes transfer of safeload parameters to parameter RAM

Band:
	- 0X00 bands 0 Upper Frequency
	- 0X01 bands 1 Upper Frequency
	- 0X02 bands 2 Upper Frequency
	- 0X03 bands 3 Upper Frequency
	- 0X10 bands 0 Lower Frequency
	- 0X11 bands 1 Lower Frequency
	- 0X12 bands 2 Lower Frequency
	- 0X13 bands 3 Lower Frequency
Frequency 
	- long int, value in Hz
XO Type
	- 0X00 = No filter	
	- 0X01 = 6dB Buttterworth	
	- 0X02 = 12dB Butterworth	
	- 0X03 = 24dB LR
	- 0X10 = No Filter
	- 0X11 = Peaking (Para EQ)
	- 0X12 = High Shelf
	
Q
	- Q for peaking and High Shelf

Filter_Gain
	- Gain for Peaking and high Shelf filters

SR
	- unsigned int, value in samples per second
*/
void Write_Filter_Parm(unsigned char band, unsigned int Frequency, unsigned char XO_Type,  unsigned int SR, char Q, char Filter_Gain)
{
int Temp_Address1, Temp_Address2;
int counter;
double SinW0, CosW0;
double Gain_A, Alpha_on_A, Alpha_by_A, A_Plus_1, A_Minus_1, SQRT_2_A;
  char Temp_String[20], Temp_String2[20];
        
	/* Filter_Gain is the peaking gain or high shelf gain parameter */
	/* this value of "A" - gain for these filters ia 10^(boos/40)   */
	/* go figure...  what a bastard of a value. 					*/
	/* for 4th order butterworth, need to cascade two filters		*/
	/*					1st Q = 0.54								*/
	/*					2nd Q = 1.31								*/
  
if (Filter_Gain <= 0)
	{
	Gain_A = ((float)(Atten_Shift_Para[-Filter_Gain]))/255.0;
	}
else
	{
	Gain_A = 255.0/((float)Atten_Shift_Para[Filter_Gain]);
	}

/* W0 = 2*pi*f0/Fs */
W0 = 2 * Pi * (double)Frequency / (double)SR;

/* Calculate SinW0 and CosW0  - this is cheaper on memory than the library routine in the compiler...*/

	A0 = W0*W0;   /* borrow A0 */
	SinW0 = -2.39e-8;
	SinW0 = SinW0 * A0;
	SinW0 = SinW0 + 2.7526e-6;
    SinW0 = SinW0 * A0;
    SinW0 = SinW0 - 1.98409e-4;
    SinW0 = SinW0 * A0;
    SinW0 = SinW0 + 8.3333315e-3;
    SinW0 = SinW0 * A0;
    SinW0 = SinW0 - 1.666666664e-1;
    SinW0 = SinW0 * A0;
    SinW0 = SinW0 + 1.0;
    SinW0 = SinW0 * W0;

	CosW0 = (float)-2.605e-7;
	CosW0 *= A0;
	CosW0 += (float) 2.47609e-5;
	CosW0 *= A0;
	CosW0 -= (float) 1.3888397e-3;
	CosW0 *= A0;
	CosW0 += (float) 4.16666418e-2;
	CosW0 *= A0;
	CosW0 -= (float) 4.999999963e-1;
	CosW0 *= A0;
	CosW0 += (float) 1.0;

Alpha = 0;
/****************************************************************/
/* Case of NO filter "none"                                     */
/* Crossover filter high pass or low pass - no cutoff           */
/****************************************************************/



if ((XO_Type == (unsigned char)0) || (XO_Type == (unsigned char)0X10))
	{
	/* If want no filter set B0 = 1, rest = 0 */
	A0 = 0;
	A1 = 0;
	A2 = 0;
	B0 = 1;
	B1 = 0;
	B2 = 0;

	/* If want no filter set B0 = 1, rest = 0 */
	A0_2 = 0;
	A1_2 = 0;
	A2_2 = 0;
	B0_2 = 1;
	B1_2 = 0;
	B2_2 = 0;

	}
/****************************************************************/
/* Case of 6dB filter                                           */
/****************************************************************/
else if (XO_Type == (unsigned char)1)
	{
	/* Low Pass
		a1 =  2.7^-?0
		b0 =  gainLinear * (1.0 - a1)
		b1 =  0   */
	/* High Pass
		a1 =  2.7^-?0
		b0 =  gainLinear * a1
		b1 = -a1 * gainLinear  */	
	A0 = 1;
	/* Algorithm to calculate e power w0) .... 	A1 = pow(2.7, -W0); */
	counter = 1;
	B0 = 1;							/* borrow B0 for a while - use this to calculate X^n - real value stuck in later*/
	B1 = 1;							/* borrow B1 for a while - use this to calculate n! - real value stuck in later*/
	A1 = 1;  						/* first term */
	do								/* This is essentially a taylor series expansion of e ^ -W0 */
		{
		B0 = -B0 * W0;
		B1 = B1 * counter;
		A1 = A1 + B0/B1;
		counter++;
		}
	while (counter < 10);
	A2 = 0;							/* B0 and B1 are always initialised in the following if -- else */
	B2 = 0;	
    /**********************************************/
    /*   Low pass if band < 8                     */
    /**********************************************/
	if (band < (unsigned char)8)
		{
		B0 = 1 - A1;
		B1 = 0;
		}
    /**********************************************/
    /*   Else the filter is a high pass           */
    /**********************************************/
	else
		{
		B0 = A1;
		B1 = -A1;
		}

		/* for 6dB filter, the second filter set need to be set to flat */
		/* If want no filter set B0 = 1, rest = 0 */
		A0_2 = 0;
		A1_2 = 0;
		A2_2 = 0;
		B0_2 = 1;
		B1_2 = 0;
		B2_2 = 0;

	}
/****************************************************************/
/*   12dB per octave, reused for 24dB                           */
/****************************************************************/
else if ((XO_Type == (unsigned char)2) || (XO_Type == (unsigned char)3))
	{
	/*Butterworth LP and HP  alpha = sin(?0) / 2.0 * 1/sqrt(2) */	
	Alpha =  SinW0/sqrt2;
	/* these are the same for both LP and HP */
	A0 = 1 + Alpha;
	A1 = (2*CosW0)/A0;
	A2 = (Alpha-1)/A0;
    /**********************************************/
    /*   Low pass if band < 8                     */
    /**********************************************/
	if (band < (unsigned char)8)   /* The crossover is a low pass if band < 8 */
		{
		/*	a0 =   1 + alpha
			a1 =  -2*cos(?0)
			a2 =   1 - alpha
			b0 =  (1 - cos(w0)) * gainLinear / 2
			b1 =   1 - cos(w0)  * gainLinear
			b2 =  (1 - cos(?0)) * gainLinear / 2  */
		B1 = (1 - CosW0)/(A0);
		B0 = B1/2;
		B2 = B0;
		}
    /**********************************************/
    /*   Else the filter is a high pass           */
    /**********************************************/
	else  /* The crossover is a high pass if band > 8 */
		{
		B1 = -(1 + CosW0)/(A0);
		B0 = -B1/2;
		B2 = B0;
		}

		/* for 12 and 24dB filters, the second filter set need to be set to flat */
		/* If want no filter set B0 = 1, rest = 0 */
		A0_2 = 0;
		A1_2 = 0;
		A2_2 = 0;
		B0_2 = 1;
		B1_2 = 0;
		B2_2 = 0;
	}

/************************************************************************************************/
/* This is only used for 8th order LR filters													*/
/************************************************************************************************/
else if (XO_Type == (unsigned char)4)
	{
	/* 4th order Butterworth LP and HP  alpha = sin(w0) / 2.0 * Q */	
	/* do the first set of filters, Q = 0.54 */
	Alpha =  SinW0/(2.0*0.54);
	/* these are the same for both LP and HP */
	A0 = 1 + Alpha;
	A1 = (2*CosW0)/A0;
	A2 = (Alpha-1)/A0;
    /**********************************************/
    /*   Low pass if band < 8                     */
    /**********************************************/
	if (band < (unsigned char)8)   /* The crossover is a low pass if band < 8 */
		{
		/*	a0 =   1 + alpha
			a1 =  -2*cos(?0)
			a2 =   1 - alpha
			b0 =  (1 - cos(w0)) * gainLinear / 2
			b1 =   1 - cos(w0)  * gainLinear
			b2 =  (1 - cos(?0)) * gainLinear / 2  */
		B1 = (1 - CosW0)/(A0);
		B0 = B1/2;
		B2 = B0;
		}
    /**********************************************/
    /*   Else the filter is a high pass           */
    /**********************************************/
	else  /* The crossover is a high pass if band > 8 */
		{
		B1 = -(1 + CosW0)/(A0);
		B0 = -B1/2;
		B2 = B0;
		}

	/* 4th order Butterworth LP and HP  alpha = sin(w0) / 2.0 * Q */	
	/* do the second set of filters, Q = 1.31 */
	Alpha =  SinW0/(2.0*1.31);
	/* these are the same for both LP and HP */
	A0_2 = 1 + Alpha;
	A1_2 = (2*CosW0)/A0_2;
	A2_2 = (Alpha-1)/A0_2;
    /**********************************************/
    /*   Low pass if band < 8                     */
    /**********************************************/
	if (band < (unsigned char)8)   /* The crossover is a low pass if band < 8 */
		{
		B1_2 = (1 - CosW0)/(A0_2);
		B0_2 = B1_2/2;
		B2_2 = B0_2;
		}
    /**********************************************/
    /*   Else the filter is a high pass           */
    /**********************************************/
	else  /* The crossover is a high pass if band > 8 */
		{
		B1_2 = -(1 + CosW0)/(A0_2);
		B0_2 = -B1_2/2;
		B2_2 = B0_2;
		}
	}


/************************************************************************************************/
/* This defines the parameters for Parametric EQ												*/
/************************************************************************************************/
else if (XO_Type == (unsigned char)0x11)
	{
	/*Peaking - Parametric EQ Filter*/	
	/* Remember that Q is in units of Q*10 */
	Alpha =  SinW0/(2.0 * (double)Q / 10.0);

	Alpha_on_A = Alpha / Gain_A;  
	Alpha_by_A = Alpha * Gain_A;

	/* these are the same for both LP and HP */
	A0 = 1 + Alpha_on_A;
	A1 = (2*CosW0)/A0;
	A2 = (Alpha_on_A - 1)/A0;

	B0 = (1 + Alpha_by_A)/A0;
	B1 = -(2* CosW0)/(A0);
	B2 = (1 - Alpha_by_A)/A0;;
	}

else if (XO_Type == (unsigned char)0x12)
	{
	/* High Shelf - CD Horn Correction*/	
	/* Q of 0.7 and S = 1 degenerates to the following */
	Alpha =  SinW0/sqrt2;

	/* These values for A+1 and A-1 are for A = 15dB */
	A_Plus_1 = 3.371373706;
	Gain_A = 2.371373706;
	A_Minus_1 = 1.371373706;
	SQRT_2_A = 2.177785;
	
	/* Coeficients for the High Shelf Filter */
	A0 = A_Plus_1 - A_Minus_1*CosW0 + SQRT_2_A*SinW0;
	A1 = -2*(A_Minus_1 - A_Plus_1*CosW0)/A0;
	A2 = -(A_Plus_1 - A_Minus_1*CosW0 - SQRT_2_A*SinW0)/A0;

	B0 = Gain_A * (A_Plus_1 + A_Minus_1*CosW0 + SQRT_2_A*SinW0)/A0;
	B1 = -2*Gain_A *(A_Minus_1 + A_Plus_1*CosW0)/A0;
	B2 = Gain_A * (A_Plus_1 + A_Minus_1*CosW0 - SQRT_2_A*SinW0)/A0;
	}

else;

if (XO_Type < (unsigned char)0x10)   /* Then we are programming the XO filters */
	{
										/* char Filter_Map[4][2][2][5] */ 
										/* [Band] [Fh, Fl] [Filter1, Filter2, Filter3, Filter4] [b0, b1, b2, a1, a2] */
										/* void(int Address, double Data, char offset)  */
										/* Safeload Parameters */
										/* Write the 5 words to the safeload registers*/
	Temp_Address1 = band & 0x03;		/* do this to save instructions */
	Temp_Address2 = (band & 0x10) >> 4;	/* do this to save instructions */

	/* this function loads and initiates safeload write of IIR filter parameters to DSP 			*/
	/* Need to provide address of first parameter - B2 for this DSP, then data B2, B1, B0, A2, A1	*/
	SafeLoad_Write_IIR_Parms((unsigned int) (Filter_Map[Temp_Address1][Temp_Address2][0][2]), FloatToFixed(IIR_B0_B1_B2_A1_A2[2]), FloatToFixed(IIR_B0_B1_B2_A1_A2[1]), FloatToFixed(IIR_B0_B1_B2_A1_A2[0]), FloatToFixed(IIR_B0_B1_B2_A1_A2[4]),FloatToFixed(IIR_B0_B1_B2_A1_A2[3]));

	if (XO_Type <= (unsigned char)2)					/* Write second lot of parameters.... */
		{
			B0 = 1;
			B1 = 0;
			B2 = 0;
			A0 = 0;
			A1 = 0;
			A2 = 0;
		}

	/* this function loads and initiates safeload write of IIR filter parameters to DSP */
	SafeLoad_Write_IIR_Parms((unsigned int) (Filter_Map[Temp_Address1][Temp_Address2][1][2]), FloatToFixed(IIR_B0_B1_B2_A1_A2[2]), FloatToFixed(IIR_B0_B1_B2_A1_A2[1]), FloatToFixed(IIR_B0_B1_B2_A1_A2[0]), FloatToFixed(IIR_B0_B1_B2_A1_A2[4]),FloatToFixed(IIR_B0_B1_B2_A1_A2[3]));

    /************************************************************************************/
	/* this function loads and initiates safeload write of IIR filter parameters to DSP */
    /*  NOTE this uses IIR2 parameters - which are for 48dB per octave else set to flat */
    /************************************************************************************/    
	SafeLoad_Write_IIR_Parms((unsigned int) (Filter_Map[Temp_Address1][Temp_Address2][2][2]), FloatToFixed(IIR2_B0_B1_B2_A1_A2[2]), FloatToFixed(IIR2_B0_B1_B2_A1_A2[1]), FloatToFixed(IIR2_B0_B1_B2_A1_A2[0]), FloatToFixed(IIR2_B0_B1_B2_A1_A2[4]),FloatToFixed(IIR2_B0_B1_B2_A1_A2[3]));

    /************************************************************************************/
	/* this function loads and initiates safeload write of IIR filter parameters to DSP */
    /*  NOTE this uses IIR2 parameters - which are for 48dB per octave else set to flat */
    /************************************************************************************/    
	SafeLoad_Write_IIR_Parms((unsigned int) (Filter_Map[Temp_Address1][Temp_Address2][3][2]), FloatToFixed(IIR2_B0_B1_B2_A1_A2[2]), FloatToFixed(IIR2_B0_B1_B2_A1_A2[1]), FloatToFixed(IIR2_B0_B1_B2_A1_A2[0]), FloatToFixed(IIR2_B0_B1_B2_A1_A2[4]),FloatToFixed(IIR2_B0_B1_B2_A1_A2[3]));

	}
else     /* Else we are programming the parametric filters */
	{
	/* this function loads and initiates safeload write of IIR filter parameters to DSP */
	SafeLoad_Write_IIR_Parms((unsigned int) Para_Filter_Map[band][2], FloatToFixed(IIR_B0_B1_B2_A1_A2[2]), FloatToFixed(IIR_B0_B1_B2_A1_A2[1]), FloatToFixed(IIR_B0_B1_B2_A1_A2[0]), FloatToFixed(IIR_B0_B1_B2_A1_A2[4]),FloatToFixed(IIR_B0_B1_B2_A1_A2[3]));
	}
}



/* Call this to completely update all filter settings etc */
void Update_Filter_Set(unsigned int SR)
{
char local_loop;	

for (local_loop = 0; local_loop < 4; local_loop++)
	{
	    Write_Filter_Parm(0x10 + local_loop, PreampData.Data_Values[local_loop].Fl,     PreampData.Data_Values[local_loop].Sl,              ADAU_SR,0                                       ,0); 
	    Write_Filter_Parm(0x00 + local_loop, PreampData.Data_Values[local_loop].Fu,     PreampData.Data_Values[local_loop].Su,              ADAU_SR,0                                       ,0); 
	    Write_Filter_Parm(3+   local_loop*3, PreampData.EQ_Values[local_loop*3 +3].CF, (PreampData.EQ_Values[local_loop*3 +3].Type), ADAU_SR,PreampData.EQ_Values[local_loop*3 +3].Q ,PreampData.EQ_Values[local_loop*3 +3].Gain); 
	    Write_Filter_Parm(4 +  local_loop*3, PreampData.EQ_Values[local_loop*3 +4].CF, (PreampData.EQ_Values[local_loop*3 +4].Type), ADAU_SR,PreampData.EQ_Values[local_loop*3 +4].Q ,PreampData.EQ_Values[local_loop*3 +4].Gain); 
	    Write_Filter_Parm(5 +  local_loop*3, PreampData.EQ_Values[local_loop*3 +5].CF, (PreampData.EQ_Values[local_loop*3 +5].Type), ADAU_SR,PreampData.EQ_Values[local_loop*3 +5].Q ,PreampData.EQ_Values[local_loop*3 +5].Gain); 
	}
    Write_Filter_Parm(       Gen1_Para_data, PreampData.EQ_Values[Gen1_Para_data].CF,  (PreampData.EQ_Values[Gen1_Para_data].Type), ADAU_SR,PreampData.EQ_Values[Gen1_Para_data].Q ,PreampData.EQ_Values[Gen1_Para_data].Gain); 
    Write_Filter_Parm(       Gen2_Para_data, PreampData.EQ_Values[Gen2_Para_data].CF,  (PreampData.EQ_Values[Gen2_Para_data].Type), ADAU_SR,PreampData.EQ_Values[Gen2_Para_data].Q ,PreampData.EQ_Values[Gen2_Para_data].Gain); 
    Write_Filter_Parm(       Gen3_Para_data, PreampData.EQ_Values[Gen3_Para_data].CF,  (PreampData.EQ_Values[Gen3_Para_data].Type), ADAU_SR,PreampData.EQ_Values[Gen3_Para_data].Q ,PreampData.EQ_Values[Gen3_Para_data].Gain); 
}




/*
Write_Input() takes a given input select:
 * 0 = MCHStreamer 
 * 1 = SPDIF
 * 2 = ADC
*/
//void Write_Input(char Input_Selected)
//{
//    long int Data_RX[4];
//
//if (Input_Selected == (char) 0)  /* the input is the MCHStreamer */
//	{
//		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, MOD_INPUT_SEL_MCH_SPDIF_ADC_STEREOMUXSIGMA300NS4INDEX_ADDR, NumBytes_Word_Parms_SW, MOD_INPUT_SEL_MCH_SPDIF_ADC_to_MCH_STREAMER );
//	}
//else if (Input_Selected == (char) 1)  /* the input is the SPDIF */
//	{
//		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, MOD_INPUT_SEL_MCH_SPDIF_ADC_STEREOMUXSIGMA300NS4INDEX_ADDR, NumBytes_Word_Parms_SW, MOD_INPUT_SEL_MCH_SPDIF_ADC_to_SPDIF );
//	}
//else   /* the input is the ADC */
//	{
//		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, MOD_INPUT_SEL_MCH_SPDIF_ADC_STEREOMUXSIGMA300NS4INDEX_ADDR, NumBytes_Word_Parms_SW, MOD_INPUT_SEL_MCH_SPDIF_ADC_to_ADC );
//	}
//
//
//}

/*
Write_Input() takes a given input select:
 * 1 = SPDIF
 * 2 = ADC
*/
//void Write_Monitor(char Input_Selected)
//{
//
//if (Input_Selected == (char) 1)  /* the input is the SPDIF */
//	{
//		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, MOD_MONITOR_SEL_SPDIF_ADC_STEREOMUXSIGMA300NS2INDEX_ADDR, NumBytes_Word_Parms_SW, MON_INPUT_SEL_MCH_SPDIF_ADC_to_SPDIF );
//	}
//else   /* the input is the ADC */
//	{
//		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, MOD_MONITOR_SEL_SPDIF_ADC_STEREOMUXSIGMA300NS2INDEX_ADDR, NumBytes_Word_Parms_SW, MON_INPUT_SEL_MCH_SPDIF_ADC_to_ADC );
//	}
//}


/******************************************************************/
/* Call this to update all Invert settings etc                     */
/******************************************************************/
void Update_Invert_Set()
{
    int loop;
    
	for (loop = 0; loop < 4; loop++)
	{
		Write_Invert(loop, PreampData.Data_Values[loop].Invert);	
	}
}



/*
Write_Invert() Inverts selected band
*/
void Write_Invert(char band, char Invert)
{

if (Invert == Invert_True)  /* User wants band inverted */
	{
		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, Invert_Map[band*2], NumBytes_Word_Parms_INV, Output_Inverted );
		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, Invert_Map[band*2 + 1], NumBytes_Word_Parms_INV, Output_Inverted );	
	}
else    /* Don't invert band... */
	{
		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, Invert_Map[band*2], NumBytes_Word_Parms_INV, Output_Normal );
		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, Invert_Map[band*2 + 1], NumBytes_Word_Parms_INV, Output_Normal );	
	}
}


void Write_Mono_Sub(char On)
{
 if (On == 1)  /* User wants mono dub */
	{
     /* this will be done a few times - less RAM to make this a function call*/
        SafeLoad_Write_General_Parms(MOD_BRIDGE_SW_ALG0_STEREOMUXSIGMA3001VOL00_ADDR, Mono_Sub_Data_0, Mono_Sub_Data_1, 0, 0, 0, 2, MOD_SAFELOADMODULE_DATA_SAFELOAD0_MEMORYPAGE);
		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, MOD_BRIDGE_SW_ALG0_SLEW_MODE_ADDR, 1, Mono_Sub_Slew );	
	}
 else    /* Leave sub stereo */
	{
        SafeLoad_Write_General_Parms(MOD_BRIDGE_SW_ALG0_STEREOMUXSIGMA3001VOL00_ADDR, Stereo_Sub_Data_0, Stereo_Sub_Data_1, 0, 0, 0, 2, MOD_SAFELOADMODULE_DATA_SAFELOAD0_MEMORYPAGE);
		SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, MOD_BRIDGE_SW_ALG0_SLEW_MODE_ADDR, 1, Mono_Sub_Slew );	
	}
   
}


/*
Write_DC_Block() Uses Sigma Studio  Values to Set DC Block
*/
void Write_DCBlock(void)
{
	SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, MOD_DCB1_DCBLOCKALGDBLPRECS3001POLE_ADDR, NumBytes_Word_Parms_DCB, DCB_Value );
	SIGMA_WRITE_SINGLE_REGISTER( DEVICE_ADDR_IC_1, MOD_DCB2_DCBLOCKALGDBLPRECS3002POLE_ADDR, NumBytes_Word_Parms_DCB, DCB_Value );
}

/********************************************************************************/
/* Call this to update all Atten settings etc 									*/
/*	inputs																		*/
/*		DAC_Attenuation - attenution sought in 0.5dB steps							*/
/*		Data_Values  with band attenuation in 0.5dB steps		*/
/*	Outputs																		*/
/*		None																	*/
/*	Calls Write Atten Loop with required attentaion in o.5dB steps				*/
/********************************************************************************/
void Update_Atten_Set()
{
int temp_atten;
int loop;

	/* first handle the gain deltas for each channel - do this in the ADAU1667 */
	for (loop = 0; loop < 4; loop++)
	{
		temp_atten = PreampData.Data_Values[loop].Atten;
		if (temp_atten <  Gain_Atten_Min)
			temp_atten = Gain_Atten_Min;
		else if (temp_atten > Gain_Atten_Max)
			temp_atten = Gain_Atten_Max;
        else
        /* now add overall volume */    
        
        temp_atten = temp_atten + PreampData.Atten_Set;
        if (PreampData.Output_Mute == 1) {
            Write_Atten(loop, Atten_Max);
        }
        else {
            Write_Atten(loop, temp_atten);	
        }
	}
}


/*
Write_Atten() takes a given bands required attenuation and programs the DSP for this
	- For safeload leave bit 9 and 6 as is
	- Program Parameters using SAFELOAD registers
	- Set Bit 4 of Core_Ctrl high then low (initialte safeload transfer to parameter RAM)
Band:
	- 0X00 bands 0 
	- 0X01 bands 1
	- 0X02 bands 2
	- 0X03 bands 3
Attenuation:
	- unsigned char 
How the hell does this work?
	- The DSP simply implements the attenuation by multiplying the signal with a constant
	- The value (unsurprisingly) is a 5.24 fixed point value
	- Attenuation is value < 1, or in fixed point 0x080000
	- This function simply implements this by - well an approximation....
		- The attenuation in dB is divided by 6, which is a right shift of the value by one place
		- So the "6db steps" of attenuation are first set up by 0x800000 >> Aten/6dB
		- Then the in between bits (0-5dB) need to be added back in....
*/
void Write_Atten(char band, int atten)
{
float Atten_Working;
long int Atten_Calc;
long int Interim_Atten_Calc;

/* apply a reasonable limit to the attenuation */
if (atten > Atten_Max)
	atten = Atten_Max;
else if (atten < Atten_Min)
	atten = Atten_Min;
else;

Atten_Working = pow(10,((float)-atten/40));
Atten_Calc = FloatToFixed(Atten_Working);
SafeLoad_Write_Single_Parameter(Atten_Map[2*band], Atten_Calc, 0);
							/* Current code uses one parameter for both channels */

}




/* this is used by the sigma stuff */
void SIGMA_WRITE_DELAY( char devAddress, int length, const unsigned char * pData )
{
    CORETIMER_DelayMs(ADAU_Loading_Delay_ms);
}


/******************************************************************/
/* Call this to update all Atten settings etc                     */
/******************************************************************/
void Update_Delay_Set(unsigned int SR)
{
    int loop;
    
	for (loop = 0; loop < 4; loop++)
	{
		Write_Delay(loop, PreampData.Data_Values[loop].Delay_mm, SR);	
	}
}


/*
Write_Delay() takes a bands required delay and programs the DSP for this
	- set bit 9 of Core_Ctrl register set HIGH
	- Program Attenuation Parameter using SAFELOAD
	- Clear Bit 9

Band:
	- 0X00 bands 0 
	- 0X01 bands 1
	- 0X02 bands 2
	- 0X03 bands 3
Delay:
	- long int 
*/
void Write_Delay(unsigned char band, int Delay_mm, unsigned int SR)
{
float Delay_Clocks;

Delay_Clocks = (float)SR * (float)Delay_mm;
Delay_Clocks = Delay_Clocks / SpeedOfSound;

/* Dont let this be zero though */
if (Delay_Clocks < 1)
	Delay_Clocks = 1;

/* safeload write parameters to AD1940 */
SafeLoad_Write_Single_Parameter(Delay_Map[band],(int)Delay_Clocks, 0);
}



/*
 * Default Download
 */
//#define DEFAULT_DOWNLOAD_SIZE_IC_1 86







