#include "mbed.h"
#include "main.h"
#include "Amp_Init_Lib.h"
extern I2C i2c;
extern Serial pc;
extern DigitalOut RESET_AMP;
extern DigitalOut DAC_RESET;
extern int addr8;

int addr = addr8;
char amp_reset_arr[][2]
{
    { 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x02, 0x11 },
    { 0x01, 0x11 },
    { 0x00, 0x00 },
    { 0x00, 0x00 },
    { 0x00, 0x00 },
    { 0x00, 0x00 },
    { 0x03, 0x11 },
    { 0x2a, 0x00 },
    { 0x25, 0x18 },
    { 0x0d, 0x00 },
    { 0x02, 0x80 },
};
//wait 5ms now

char amp_config_clocks_and_misc[][2]
{
	{ 0x00, 0x00 },
    { 0x7f, 0x00 },

// speed 03-48k 04-96k
//dynamically reading speed
    { 0x22, 0x03 },

    { 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x02, 0x00 },



//register tuning
    { 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x00, 0x00 },
    { 0x07, 0x00 }, //changed from 0x00 to change SDOUT
    { 0x08, 0x20 },
    { 0x55, 0x07 },
    { 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x00, 0x00 },
    { 0x3d, 0x30 }, //volume control
    { 0x3e, 0x30 }, //volume control
    { 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x00, 0x01 },
    { 0x02, 0x00 },

//things to keep
	{ 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x00, 0x00 },
	{ 0x28, 0x00 },
	{ 0x08, 0x20 },
	{ 0x55, 0x01 },


};

char amp_dsp_startup[][2]
{
	{0x02, 0x00 },
	//now dsp instructions
	
};

char amp_unmute_arr[] [2]
{
	//Unmute the device
    { 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x03, 0x00 },
    { 0x2a, 0x11 },

};

char amp_init_arr[][2]
{
	/*{0x00, 0x00},
	{0x7f, 0x00},
	{ 0x02, 0x11 | 0x80 }, //put DSP into reset, put dac in standby and put dac into powerdown
    { 0x01, 0x11 }, //reset all modules and registers
	{ 0x00, 0x00 },
    { 0x00, 0x00 },
    { 0x00, 0x00 },
    { 0x00, 0x00 },
	{ 0x2a, 0x00}, //dac data mute
	
	//sample rate update
	{ 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x02, 0x80 },

    { 0x00, 0x00 },
    { 0x7f, 0x00 },

// speed 03-48k 04-96k
//dynamically reading speed
    { 0x22, 0x03 },

    { 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x02, 0x00 },
	
	
    { 0x00, 0x00 },
    { 0x7f, 0x00 },
    { 0x03, 0x00 },
    { 0x2a, 0x22 },
    */
};

/*
* this function writes to a register value and reads back the result to pc serial
* data = array of size 2 with register in 0 and value in 1
* returns 1 when ack failed
*/



char RegisterSetAndRead(char data[]) //returns true when it fails
{
	char failed = i2c.write(addr, data, 2);
	if(failed)
	{
		pc.printf("i2c write failied\n");
		return failed; //NOTE failed = 1 when it failed
	}
	char temp;
	i2c.write(addr, data, 1);
	i2c.read(addr, &temp, 1);
	//pc.printf("Register %x set to %x\n", data[0], temp);
	return 0;
}

char RegisterRead(char reg)
{	
	char failed = i2c.write(addr, &reg, 1);
	if(failed)
	{
		pc.printf("i2c write failied\n");
		return failed; //NOTE failed = 1 when it failed
	}
	char temp;
	i2c.read(addr, &temp, 1);
	pc.printf("Register %x reads %x\n", reg, temp);
	return failed;
}

void amp_init(void)
{
	size_t n = sizeof(amp_reset_arr) / sizeof(amp_reset_arr[0]);
	
	DAC_RESET = 0;
	wait_us(100);
	DAC_RESET = 1;
	//config clocking interface
	RESET_AMP = 0;
	for( int i = 0; i<n; i++)
	{
		RegisterSetAndRead(amp_reset_arr[i]);
	}
	
	wait_ms(5);
	n = sizeof(amp_config_clocks_and_misc) / sizeof(amp_config_clocks_and_misc[0]);
	for( int i = 0; i<n; i++)
	{
		RegisterSetAndRead(amp_config_clocks_and_misc[i]);
	}
	
	wait_ms(5);
	n = sizeof(amp_dsp_startup) / sizeof(amp_dsp_startup[0]);
	for( int i = 0; i<n; i++)
	{
		RegisterSetAndRead(amp_dsp_startup[i]);
	}
	
	//last step keeping it muted
	RESET_AMP = 1;
	wait_ms(5);
	n = sizeof(amp_unmute_arr) / sizeof(amp_unmute_arr[0]);
	for( int i = 0; i<n; i++)
	{
		RegisterSetAndRead(amp_unmute_arr[i]);
	}
	

	RegisterRead(0x5e);
	RegisterRead(0x5f);
	RegisterRead(0x03);
	RegisterRead(0x77);
	RegisterRead(0x3d);
	RegisterRead(0x3e);
	RegisterRead(0x78);
	RegisterRead(0x5b);


}



