#include "profiler.h"

// System max and min values
int Tmax = 1000,	Tmin = 10,
	Vmax = 5,		Vmin = 0,
	Amax = 2,		Amin = 1;
	
//Simulated user input
int Vdes = 2;

void Accel_Init( void )
{
	char 	outString[10];

	sprintf(outString, "%c%c%c%c\r",0xC4, 0xC1, 0x29, 0xC2);		//Enable continuous accel and angular rate data mode
	UART_SendString((unsigned char *) outString);

	return;
	
}

void PDucer_Init( void )
{
	char	outString[20];

	sprintf(outString, "%s\r","*01WE=RAM");			//Enable sequential paramter writes to RAM
	UART_SendString((unsigned char *) outString);

	sprintf(outString, "%s\r","*01DU=PSI");			//Set units to PSI
	UART_SendString((unsigned char *) outString);
	
	sprintf(outString, "%s\r","*01I=R120");			//Set read rate to 120 reads/sec
	UART_SendString((unsigned char *) outString);

	sprintf(outString, "%s\r","*01D$=0");			//No deadband
	UART_SendString((unsigned char *) outString);

	sprintf(outString, "%s\r","*01MO=X2");			//Power up mode = no continuous readings
	UART_SendString((unsigned char *) outString);

	sprintf(outString, "%s\r","*01NE=OFF");			//Disable analog output
	UART_SendString((unsigned char *) outString);

	sprintf(outString, "%s\r","*01OP=A");			//Power up mode = no continuous readings
	UART_SendString((unsigned char *) outString);
	sprintf(outString, "%s\r","*01OP=N");			//Power up mode = no checksum
	UART_SendString((unsigned char *) outString);
	sprintf(outString, "%s\r","*01OP=E");			//Power up mode = extended form
	UART_SendString((unsigned char *) outString);
	sprintf(outString, "%s\r","*01OP=W");			//Power up mode = enable watchdog timer
	UART_SendString((unsigned char *) outString);
	
	sprintf(outString, "%s\r","*01WE");				//Enable parameter writes to EEPROM
	UART_SendString((unsigned char *) outString);
	sprintf(outString, "%s\r","*01SP=ALL");			//Store parameters to EEPROM
	UART_SendString((unsigned char *) outString);
			
	sprintf(outString, "%s\r","*01CK");				//Check EEPROM
	UART_SendString((unsigned char *) outString);
	//NEED TO DO SOMETHING HERE IS RETURN STRING IS NOT "OK"
	
//	sprintf(outString, "%s\r","*01P1");				//Start continuous ASCII messages
//	UART_SendString((unsigned char *) outString);

}
	
void Profiler_Init(void)
{
	u32 i;

	// initialize EZ-Kit
	ezInit(1);
	for(i=0; i<EZ_NUM_BUTTONS; i++)
	  ezInitButton(i);
	
}

//main.c
void main(void) {
	char	outString[20];
	int i = 0,
		userCmd = 0,
		sigIncr,
		dacOffset,
		sigMax, sigMin,
		sigValue,
		dacValue;

	Profiler_Init();
	UART_Initialize();/*
	PDucer_Init();
//	Accel_Init();
			
	//Init ADC & DAC
	Init_Flags();
	Audio_Reset();
	Init_Sport0();
	Init_DMA();
	Init_Interrupts();
	Enable_DMA_Sport0();

	
//Setup Elmo Whistle
	sprintf(outString, "%s\r\n","MO=0");			//Disable motor
	UART_SendString((unsigned char *) outString);
	sprintf(outString, "%s\r\n","UM=2");			//Set velocity mode
	UART_SendString((unsigned char *) outString);
	sprintf(outString, "%s\r\n","AG[2]=10000");		//Set ADC gain (cts/sec per volt)
	UART_SendString((unsigned char *) outString);
	sprintf(outString, "%s\r\n","AS[1]=2.13");		//Set ADC offset
	UART_SendString((unsigned char *) outString);
	sprintf(outString, "%s\r\n","RM=1");			//Set analog modulation of velocity
	UART_SendString((unsigned char *) outString);
	sprintf(outString, "%s\r\n","MO=1");			//Enable motor
	UART_SendString((unsigned char *) outString);

	sigIncr = 1;
	sigMax = 5000000;
	sigMin = -1 * sigMax;
	dacOffset = 000000;
	sigValue = 0;
*/	
	sprintf(outString, "%c%c%c\r",0x1B, 0x2, 0x42);		//esc stx A to connect master to port B
	printf("Sending switch command B\r\n");
	UART_SendString((unsigned char *) outString);
	ezDelay(500);

	while( !ezIsButtonPushed(EZ_LAST_BUTTON) )
	{
		sprintf(outString, "%s\r","*01P1");				
		printf("Sending P1\r\n");
		UART_SendString((unsigned char *) outString);
		ezDelay(500);

/*		sigValue = sigValue + sigIncr;
		if ((sigValue > sigMax) || (sigValue < sigMin))
		{
			sigValue = sigValue - (2 * sigIncr);
			sigIncr = -1 * sigIncr;
			for(i=0; i<1000000; i++)
			{
				dacValue = sigValue - dacOffset;
//				printf("dacValue= %d\n", dacValue);
				DAC_setValue(dacValue);	
			}
				
//			sprintf(outString, "%s\r\n","IQ");
//			UART_SendString((unsigned char *) outString); 
		}
		dacValue = sigValue - dacOffset;
//		printf("dacValue= %d\n", dacValue);
		DAC_setValue(dacValue);  */
	}
	
	printf("Button was pushed \r\n");
	sprintf(outString, "%s\r\n","MO=0");
	UART_SendString((unsigned char *) outString);

	dacValue = 0;
	printf("dacValue= %d\n", dacValue);
	DAC_setValue(dacValue);

 //	UART_Finalize();
 	
 	printf("Exiting\r\n");

	// return
}