/*********************************************************************
 *               Flowcode PIC16 Internals Header File
 *
 * File: internals.h
 *
 * (c) 2010 Matrix Multimedia Ltd.
 * http://www.matrixmultimedia.com
 *
 * Software License Agreement
 *
 * The software supplied herewith by Matrix Multimedia Ltd (the
 * “Company”) for its Flowcode graphical programming language is
 * intended and supplied to you, the Company’s customer, for use
 * solely and exclusively on the Company's products. The software
 * is owned by the Company, and is protected under applicable
 * copyright laws. All rights are reserved. Any use in violation
 * of the foregoing restrictions may subject the user to criminal
 * sanctions under applicable laws, as well as to civil liability
 * for the breach of the terms and conditions of this licence.
 *
 * THIS SOFTWARE IS PROVIDED IN AN “AS IS” CONDITION. NO WARRANTIES,
 * WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT,
 * IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR
 * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
 *
 * Changelog:
 *
 *  date  | by | description
 * -------+----+-----------------------------------------------------
 * 140710 | BR | Created
 * 230710 | BR | Removed warnings and unused variables / converted ICD functions
 * 150910 | BR | Fixed a bug with ICD and EEPROM and also updated the reset and interrupt functions
 * 200910 | BR | Fixed bugs in the string manipulation functions to allow them to work correctly.
 * 210910 | BR | Updated the EEPROM functions for the ICD
 * 210910 | BR | Added the PIC compatible float manipulation functions
 * 101210 | BR | Moved float functions to defines and fixed a few bugs in functions
 * 221210 | BR | Modified isinf and float defines to allow them to work in a few more circumstances
 ********************************************************************/

#ifndef internals_h
#define internals_h

extern void __delay32(unsigned long instruction_cycles);

//Calculate delays based on device MIPS per clock - minimum delay 11 instructions
#define delay_us(A)				__delay32((unsigned long)(A)*(MX_Fosc / (MX_CLKS_PER_INST * 1000000)))
#define delay_10us(A)			__delay32((unsigned long)(A)*(MX_Fosc / (MX_CLKS_PER_INST * 100000)))
#define delay_ms(A)				__delay32((unsigned long)(A)*(MX_Fosc / (MX_CLKS_PER_INST * 1000)))
#define delay_s(A)				__delay32((unsigned long)(A)*(MX_Fosc / MX_CLKS_PER_INST))


//Common defines and functions
#define MX_CLEAR_WATCHDOG		asm("clrwdt")

#define set_bit(var,bitno) 		var |= (1 << bitno)
#define clear_bit(var,bitno) 	var &= ~(1 << bitno)

char test_bit(char reg, char bitno)
{
	if (reg & (1 << bitno))
		return (1 << bitno);
	return 0;
}

//EEPROM memory define
#ifdef MX_EE
	unsigned int __attribute__ ((space(eedata))) eeprom;        // eeprom storage base
#endif

//String function definitions
char FCI_GETCHAR(char* sStr1, char iStr1_len, char iPos);
char FCI_GETLENGTH(char* sStr1, char iStr1_len);
char FCI_MIDSTRING(char* sSrc, char iSrc_len, char* sDst, char iDst_len, char iStart, char iCount);
char FCI_LEFTSTRING(char* sSrc, char iSrc_len, char* sDst, char iDst_len, char iCount);
char FCI_RIGHTSTRING(char* sSrc, char iSrc_len, char* sDst, char iDst_len, char iCount);
//char FCI_CONCATENATE(char* sSrc1, char iSrc1_len, char* sSrc2, char iSrc2_len, char* sDst, char iDst_len);
char FCI_CONCATENATE(char* sSrc1, char iSrc1_len, volatile char* sSrc2, char iSrc2_len, volatile char* sDst, char iDst_len);
char FCI_TOSTRING(int iSrc1, char* sDst, char iDst_len);
void FCI_TOLOWER(char* sSrc, char iSrc_len, char* sDst, char iDst_len);
void FCI_TOUPPER(char* sSrc, char iSrc_len, char* sDst, char iDst_len);
//char FCI_COMPARE(char* sSrc1, char iSrc1_len, char* sSrc2, char iSrc2_len, char iNoCase);
char FCI_COMPARE(char* sSrc1, char iSrc1_len, volatile char* sSrc2, char iSrc2_len, char iNoCase);
char FCI_FLOAT_TO_STRING(float Number, char Precision, char* String, char MSZ_String);
char FCI_NUMBER_TO_HEX(short Number, char* String, char MSZ_String);
long FCI_STRING_TO_INT(char* String, char MSZ_String);
float FCI_STRING_TO_FLOAT(char* String, char MSZ_String);
void Wdt_msDelay(void);
void Wdt_Delay_S(short delay);
void Wdt_Delay_Ms(short delay);
unsigned int float32_is_signaling_nan(float f);


//Float functions to allow PIC imports to work correctly
#define float32_add(A, B)				((A) + (B))
#define float32_sub(A, B)				((A) - (B))
#define float32_mul(A, B)				((A) * (B))
#define float32_div(A, B)				((A) / (B))
#define float32_rem(A, B)				(fmod((A), (B)))
#define float32_eq_signaling(A, B)		((A) == (B))
#define float32_ge(A, B)				((A) >= (B))
#define float32_gt(A, B)				((A) > (B))
#define float32_le(A, B)				((A) <= (B))
#define float32_lt(A, B)				((A) < (B))
#define float32_to_int(A)				((int)(A))
#define float32_from_int(A)				((float)(A))


char FCI_GETCHAR(char* sStr1, char iStr1_len, char iPos)
{
	char tmp;
	for (tmp = 0; tmp < iStr1_len; tmp++)
	{
		if (sStr1[(unsigned int)tmp] == 0)
			break;
	}

	if(iPos < tmp)
		tmp = sStr1[(unsigned int)iPos];
	else tmp = 0;
	return (tmp);
}

char FCI_GETLENGTH(char* sStr1, char iStr1_len)
{
	char tmp;
	for (tmp = 0; tmp < iStr1_len; tmp++)
	{
		if (sStr1[(unsigned int)tmp] == 0)
			break;
	}
	return (tmp);
}

char FCI_MIDSTRING(char* sSrc, char iSrc_len, char* sDst, char iDst_len, char iStart, char iCount)
{
	char idx;
	for (idx = 0; idx < iSrc_len; idx++)	//find the length of the source string
	{
		if (sSrc[(unsigned int)idx] == 0)
			break;
	}

	if (iStart >= idx)
	{
		sDst[0] = 0;						//starting position is beyond string length, so return a null string
	}
	else
	{
		if ((iStart + iCount) >= idx)		//Make sure the required length is not too big
			iCount = idx - iStart;

		for (idx = 0; idx < iCount; idx++)	//Copy the contents of the source string
		{
			if (idx < iDst_len)
			{
				sDst[(unsigned int)idx] = sSrc[(unsigned int)iStart];
				iStart++;
			}
			else break;
		}

		if (idx < iDst_len)					//add terminating null (if we can)
			sDst[(unsigned int)idx] = 0;
	}
	return (idx);
}

char FCI_LEFTSTRING(char* sSrc, char iSrc_len, char* sDst, char iDst_len, char iCount)
{
	char idx;
	for (idx = 0; idx < iSrc_len; idx++)	//find the length of the source string
	{
		if (sSrc[(unsigned int)idx] == 0)
			break;
	}

	if (iCount > idx)						//make sure the required length is not too big
		iCount = idx;

	for (idx = 0; idx < iCount; idx++)		//copy the required characters
	{
		if (idx < iDst_len)
		{
			sDst[(unsigned int)idx] = sSrc[(unsigned int)idx];
		}
		else break;
	}

	if (idx < iDst_len)						//add terminating null (if we can)
		sDst[(unsigned int)idx] = 0;

	return (idx);
}

char FCI_RIGHTSTRING(char* sSrc, char iSrc_len, char* sDst, char iDst_len, char iCount)
{
	char idx = 0;
	char delta;

	for (idx = 0; idx < iSrc_len; idx++)	//find the length of the source string
	{
		if (sSrc[(unsigned int)idx] == 0)
			break;
	}

	if (iCount > idx)						//make sure the required length is not too big
		iCount = idx;

	delta = idx - iCount;					//get the offset

	for(idx = 0; idx < iCount; idx++)		//copy the required characters
	{
		if (idx < iDst_len)
		{
			sDst[(unsigned int)idx] = sSrc[(unsigned int)delta];
			delta++;
		}
		else break;
	}

	if (idx < iDst_len)						//add terminating null (if we can)
		sDst[(unsigned int)idx] = 0;

	return (idx);
}

//char FCI_CONCATENATE(char* sSrc1, char iSrc1_len, char* sSrc2, char iSrc2_len, char* sDst, char iDst_len)
char FCI_CONCATENATE(char* sSrc1, char iSrc1_len, volatile char* sSrc2, char iSrc2_len, volatile char* sDst, char iDst_len)
{
	char tmp;
	char idx = 0;

	for (tmp=0; tmp<iSrc1_len; tmp++)		//copy source 1 into destination
	{
		if (idx < iDst_len)
		{
			sDst[(unsigned int)idx] = sSrc1[(unsigned int)idx];

			if (sSrc1[(unsigned int)idx] != 0)
				idx++;
			else break;
		}
	}

	for (tmp=0; tmp<iSrc2_len; tmp++)		//copy source 2 into destination
	{
		if (idx < iDst_len)
		{
			sDst[(unsigned int)idx] = sSrc2[(unsigned int)tmp];

			if (sSrc2[(unsigned int)tmp] != 0)
				idx++;
			else break;
		}
	}

	if (idx < iDst_len)						//add terminating null (if we can)
		sDst[(unsigned int)idx] = 0;

	return (idx);
}

char FCI_TOSTRING(int iSrc1, char* sDst, char iDst_len)
{
	char tmp1 = 0;
	signed int tmp2;
	char idx = 0;

	//copy source 1 into destination:
	//minus sign
	if (iSrc1 < 0)
	{
		sDst[(unsigned int)0] = '-';
		idx++;
		iSrc1 = 0 - iSrc1;
	}

	tmp2 = iSrc1;
	if (iSrc1 >= 10000)
	{
		if (idx < iDst_len)
		{
			tmp1 = tmp2 / 10000;
			sDst[(unsigned int)idx] = '0' + tmp1;
			idx++;
		}

		while (tmp1 > 0)
		{
			tmp2 = tmp2 - 10000;
			tmp1--;
		}
	}

	if (iSrc1 >= 1000)
	{
		if (idx < iDst_len)
		{
			tmp1 = tmp2 / 1000;
			sDst[(unsigned int)idx] = '0' + tmp1;
			idx++;
		}

		while (tmp1 > 0)
		{
			tmp2 = tmp2 - 1000;
			tmp1--;
		}
	}

	if (iSrc1 >= 100)
	{
		if (idx < iDst_len)
		{
			tmp1 = tmp2 / 100;
			sDst[(unsigned int)idx] = '0' + tmp1;
			idx++;
		}

		while (tmp1 > 0)
		{
			tmp2 = tmp2 - 100;
			tmp1--;
		}
	}

	if (iSrc1 >= 10)
	{
		if (idx < iDst_len)
		{
			tmp1 = tmp2 / 10;
			sDst[(unsigned int)idx] = '0' + tmp1;
			idx++;
		}

		while (tmp1 > 0)
		{
			tmp2 = tmp2 - 10;
			tmp1--;
		}
	}

	if (idx < iDst_len)
	{
		sDst[(unsigned int)idx] = '0' + tmp2;
		idx++;
	}

	//add terminating null (if we can)
	if (idx < iDst_len)
	{
		sDst[(unsigned int)idx] = 0;
	}

	return (idx);
}

void FCI_TOLOWER(char* sSrc, char iSrc_len, char* sDst, char iDst_len)
{
	char idx;
	char ch;

	//copy source into destination and change to lower case
	for (idx=0; idx<iSrc_len; idx++)
	{
		if (idx < iDst_len)
		{
			ch = sSrc[(unsigned int)idx];
			sDst[(unsigned int)idx] = ch;
			if (ch != 0)
			{
				if ((ch >= 'A') && (ch <= 'Z'))
				{
					sDst[(unsigned int)idx] = (ch | 0x20);
				}
			} else {
				break;
			}
		}
	}
}

void FCI_TOUPPER(char* sSrc, char iSrc_len, char* sDst, char iDst_len)
{
	char idx;
	char ch;

	//copy source into destination and change to upper case
	for (idx=0; idx<iSrc_len; idx++)
	{
		if (idx < iDst_len)
		{
			ch = sSrc[(unsigned int)idx];
			sDst[(unsigned int)idx] = ch;
			if (ch != 0)
			{
				if ((ch >= 'a') && (ch <= 'z'))
				{
					sDst[(unsigned int)idx] = (ch & 0xDF);
				}
			} else {
				break;
			}
		}
	}
}

//char FCI_COMPARE(char* sSrc1, char iSrc1_len, char* sSrc2, char iSrc2_len, char iNoCase)
char FCI_COMPARE(char* sSrc1, char iSrc1_len, volatile char* sSrc2, char iSrc2_len, char iNoCase)
{
	char idx;
	char ch1, ch2;

	for (idx=0; idx < iSrc1_len; idx++)	//compare a byte from the strings
	{
		if (idx < iSrc2_len)
		{
			ch1 = sSrc1[(unsigned int)idx];
			ch2 = sSrc2[(unsigned int)idx];

			if (iNoCase)
			{
				if ((ch1 >= 'a') && (ch1 <= 'z'))
					ch1 = (ch1 & 0xDF);

				if ((ch2 >= 'a') && (ch2 <= 'z'))
					ch2 = (ch2 & 0xDF);
			}

			if (ch1 < ch2)
			{
				return(255);
			}
			else if (ch1 > ch2)
			{
				return(1);
			}
		}
		else
		{
			if (sSrc1[(unsigned int)idx] == 0)
				return (0);				//end of source1 as well, so ok
			else
				return (1);				//reached the end of iSrc2, but still more of iSrc1
		}
	}

	if (iSrc1_len == iSrc2_len)			//reached the end of iSrc1.  If we're also at the end of iSrc2, then return 0
		return (0);
	else
	{
		if (sSrc2[(unsigned int)idx] == 0)
			return (0);					//end of source1 as well, so ok
		else
			return (255);				//not at end of source2, so return -1
	}
}



/********************* ICD SPECIFICS *******************/
#ifdef USE_FLOWCODE_ICD

	//ICD Clock Port Defines
	#if (ICD_CLK_PORT == 0)
		#define ICD_CLK_PIN_TRIS TRISA
		#define ICD_CLK_PIN_PORT PORTA
	#endif
	#if (ICD_CLK_PORT == 1)
		#define ICD_CLK_PIN_TRIS TRISB
		#define ICD_CLK_PIN_PORT PORTB
	#endif
	#if (ICD_CLK_PORT == 2)
		#define ICD_CLK_PIN_TRIS TRISC
		#define ICD_CLK_PIN_PORT PORTC
	#endif
	#if (ICD_CLK_PORT == 3)
		#define ICD_CLK_PIN_TRIS TRISD
		#define ICD_CLK_PIN_PORT PORTD
	#endif
	#if (ICD_CLK_PORT == 4)
		#define ICD_CLK_PIN_TRIS TRISE
		#define ICD_CLK_PIN_PORT PORTE
	#endif
	#if (ICD_CLK_PORT == 5)
		#define ICD_CLK_PIN_TRIS TRISF
		#define ICD_CLK_PIN_PORT PORTF
	#endif
	#if (ICD_CLK_PORT == 6)
		#define ICD_CLK_PIN_TRIS TRISG
		#define ICD_CLK_PIN_PORT PORTG
	#endif

	//ICD Data Port Defines
	#if (ICD_DATA_PORT == 0)
		#define ICD_DATA_PIN_TRIS TRISA
		#define ICD_DATA_PIN_PORT PORTA
	#endif
	#if (ICD_DATA_PORT == 1)
		#define ICD_DATA_PIN_TRIS TRISB
		#define ICD_DATA_PIN_PORT PORTB
	#endif
	#if (ICD_DATA_PORT == 2)
		#define ICD_DATA_PIN_TRIS TRISC
		#define ICD_DATA_PIN_PORT PORTC
	#endif
	#if (ICD_DATA_PORT == 3)
		#define ICD_DATA_PIN_TRIS TRISD
		#define ICD_DATA_PIN_PORT PORTD
	#endif
	#if (ICD_DATA_PORT == 4)
		#define ICD_DATA_PIN_TRIS TRISE
		#define ICD_DATA_PIN_PORT PORTE
	#endif
	#if (ICD_DATA_PORT == 5)
		#define ICD_DATA_PIN_TRIS TRISF
		#define ICD_DATA_PIN_PORT PORTF
	#endif
	#if (ICD_DATA_PORT == 6)
		#define ICD_DATA_PIN_TRIS TRISG
		#define ICD_DATA_PIN_PORT PORTG
	#endif

	#ifndef ICD_CLK_PIN_TRIS
	  #warning "Error: no definition for the ICD clock pin"
	#endif

	#ifndef ICD_DATA_PIN_TRIS
	  #warning "Error: no definition for the ICD data pin"
	#endif

	//ICD Global Defines
	#define ICD_START	  0x80
	#define ICD_SETBP	  0x81
	#define ICD_CLRBP	  0x82
	#define ICD_EN_IBP	  0x83
	#define ICD_DIS_IBP	  0x84
	#define ICD_EN_SBP	  0x85
	#define ICD_DIS_SBP	  0x86
	#define ICD_RD_RAM	  0x87
	#define ICD_WR_RAM	  0x88
	#define ICD_GET_ID    0x8C
	#define ICD_GET_STACK 0x8D
	#define ICD_DIS_BPS	  0x8B
	#define ICD_RESET	  0x8E

	#ifdef MX_EE					//If EEPROM
		#define ICD_RD_EE	  0x89
		#define ICD_WR_EE	  0x8A
		unsigned int tempdata = 0;
	#endif

	//ICD Global Variables
	unsigned char dbg_mode = 0x00;
	unsigned char icd_stack_ptr = 0x00;
	unsigned int ICD_TIMEOUT = 1000;

	//ICD Functions
	void FC_MARKER(signed int block_id);
	void ICD_PUSH(char macro_id);
	void ICD_POP();
	char icd_byte_tx(char data);
	char icd_byte_rx(void);
	char wait_clock(char state, char timeout_inc);
	char wait_data(char state);
	char read_register(unsigned int address);
	void write_register(unsigned int address, char data);
	void rst_dev(void);

	//ICD Marker Functions - Called at the start of every Flowcode block when ICD is enabled.
	void FC_MARKER(signed int block_id)
	{
		unsigned char dbg_hold = 0;
		unsigned char icd_cmd;
		unsigned int address;
		unsigned char idx;

		set_bit(ICD_CLK_PIN_TRIS, ICD_CLK_PIN);					//Clock is an input
		clear_bit(ICD_DATA_PIN_TRIS, ICD_DATA_PIN);				//Data is an output
		set_bit(ICD_DATA_PIN_PORT, ICD_DATA_PIN);				//Data is default low	****** inverted

		if(test_bit(ICD_CLK_PIN_PORT, ICD_CLK_PIN))				//Test to see if clock input is 1
			dbg_mode = dbg_mode | 0x04;					//Enable ICD Pause
		else
			dbg_mode = dbg_mode & 0xFB;					//Disable ICD Pause

		if(dbg_mode)									//Is ICD Active?
		{
			__asm__ volatile("disi #0x3FFF");			//Disable interrupts

			if(dbg_mode & 0x01) dbg_hold = 0x01;		//Test for Single Step operation
			if(dbg_mode & 0x02)							//Test for Breakpoint operation and match
			{
				for(idx=0; idx<ICD_NUM_BPS; idx++)
				{
					if(block_id == *(brk_pnt + idx)) dbg_hold = dbg_hold | 0x02;		//if(block_id == brk_pnt[idx]) dbg_hold = dbg_hold | 0x02;
				}
			}

			if(dbg_mode & 0x04) 								//Test for Flowcode triggered operation
				dbg_hold = dbg_hold | 0x04;

			if(dbg_mode & 0x08) 								//Test for initial stop condition
			{
				dbg_hold = dbg_hold | 0x08;						//Place target into infinite wait mode
				dbg_mode = dbg_mode & 0xF7;						//Disable infinite wait mode for next iteration.
			}

			if(dbg_hold)										//Enter Debug mode
			{
				set_bit(ICD_DATA_PIN_TRIS, ICD_DATA_PIN);		//Data is an input target stopped

				if (dbg_hold & 0x08)
				{
					//delay_us(250);								//Wait for pins to settle after reset
					delay_us(250);								//Wait for pins to settle after reset
					wait_clock(1<<ICD_CLK_PIN, 0);				//Wait indefinitely for the next clock
					wait_clock(0, 0);							//Wait indefinitely for the next clock
					wait_clock(1<<ICD_CLK_PIN, 0);				//Wait indefinitely for the next clock
					icd_byte_tx(dbg_mode);						//Send current ICD mode
					icd_byte_tx(block_id);						//Send block ID
					icd_byte_tx(block_id>>8);

				}
				else
				{
					if (wait_clock(1<<ICD_CLK_PIN, 1))			//Wait for clock to go high
						return;
					if (wait_clock(0, 0))						//Wait for clock to go low
						return;
					if (wait_clock(1<<ICD_CLK_PIN, 0))			//Wait for clock to go high
						return;

					if (icd_byte_tx(dbg_mode)) dbg_hold = 0;	//Send current ICD mode
					if (icd_byte_tx(block_id)) dbg_hold = 0;	//Send block ID
					if (icd_byte_tx(block_id>>8)) dbg_hold = 0;
				}

				while(dbg_hold)
				{
					icd_cmd = icd_byte_rx();							//Receive command byte

					__asm__ volatile("disi #0x3FFF");					//Ensure interrupts remain disabled while waiting

					switch(icd_cmd)
					{
						case ICD_START:									//Start Command

							dbg_hold = 0;								//Start the target running
							clear_bit(ICD_DATA_PIN_TRIS, ICD_DATA_PIN);				//Data is an output
							set_bit(ICD_DATA_PIN_PORT, ICD_DATA_PIN);				//Data is default low	**** inverted
							wait_clock(0, 1);
							break;

						case ICD_SETBP:						//Set Breakpoint Command

							address = icd_byte_rx();					//Receive BP ID
							address = address | (icd_byte_rx() << 8);
							idx=0;										//Initialise BP counter
							while(idx < ICD_NUM_BPS && *(brk_pnt + idx) != -1)	//Scan for empty BP slot           //while(idx < ICD_NUM_BPS && brk_pnt[idx] != -1)	//Scan for empty BP slot
								idx = idx + 1;

							if (idx == ICD_NUM_BPS)						//If breakpoint command list full
								icd_byte_tx(255);						//Let the ICD know
							else
							{
								*(brk_pnt + idx) = address;					//Else assign the BP ID to the BP list				//brk_pnt[idx] = address;
								icd_byte_tx(0);							//Transmit an acknowledge
							}
							break;

						case ICD_CLRBP:						//Clear Breakpoint Command

							address = icd_byte_rx();
							address = address | (icd_byte_rx() << 8);
							idx=0;										//Initialise BP counter
							while(idx < ICD_NUM_BPS && *(brk_pnt + idx) != address)	//Scan for slot containing BP				//while(idx < ICD_NUM_BPS && brk_pnt[idx] != address)
								idx = idx + 1;

							if (idx == ICD_NUM_BPS)						//If breakpoint not in the list
								icd_byte_tx(255);						//Let the ICD know
							else
							{
								*(brk_pnt + idx) = -1;						//Else clear the BP ID from the BP list					//brk_pnt[idx] = -1;
								icd_byte_tx(0);							//Transmit an acknowledge
							}
							break;

						case ICD_EN_IBP:					//Enable indexed breakpoints Command

							dbg_mode = dbg_mode | 0x02;					//Enable indexed breakpoints
							break;

						case ICD_DIS_IBP:					//Disable indexed breakpoints Command

							dbg_mode = dbg_mode & 0xFD;					//Disable indexed breakpoints
							break;

						case ICD_EN_SBP:					//Enable step breakpoint Command

							dbg_mode = dbg_mode | 0x01;					//Enable step breakpoint
							break;

						case ICD_DIS_SBP:					//Disable step breakpoint Command

							dbg_mode = dbg_mode & 0xFE;					//Disable step breakpoint
							break;

						case ICD_RD_RAM:					//Read Byte Of RAM Command

							address = icd_byte_rx();
							address = address | (icd_byte_rx() << 8);
							icd_byte_tx(read_register(address));		//&address//Transmit the register value
							break;

						case ICD_WR_RAM:					//Write Byte Of RAM Command

							address = icd_byte_rx();
							address = address | (icd_byte_rx() << 8);
							write_register(address, icd_byte_rx());
							break;

						case ICD_DIS_BPS:					//Clear all breakpoints

							idx=0;	//init BP counter
							while (idx < ICD_NUM_BPS)
							{
								*(brk_pnt + idx) = -1;											//brk_pnt[idx] = -1;
								idx = idx + 1;
							}
							break;

						#ifdef MX_EE						//Is EEPROM Available?

							case ICD_RD_EE:					//Read Contents Of EEPROM Command

								address = icd_byte_rx();
								address = address | (icd_byte_rx() << 8);
								if (address < (MX_EE_SIZE / 2))
								{
									TBLPAG = __builtin_tblpage(&eeprom);      // eeprom page
									address = __builtin_tbloffset(&eeprom) + address;
									icd_byte_tx(__builtin_tblrdl(address));
								}
								else
								{
									icd_byte_tx(0);
								}
								break;

							case ICD_WR_EE:					//Write Byte to EEPROM Command

								address = icd_byte_rx();
								address = address | (icd_byte_rx() << 8);
								tempdata = icd_byte_rx();

								if (address < (MX_EE_SIZE / 2))
								{
									NVMCON = 0x4004;            // set up to write 1 word

									TBLPAG = __builtin_tblpage(&eeprom);
									address = __builtin_tbloffset(&eeprom) + address;

									__builtin_tblwtl(address,tempdata);    // prepare data in write latch

									 //asm volatile ("disi #6");    // disable irqs for 6 instructions
									 //Interrupts already temporarily disabled.

									 __builtin_write_NVM();        // unlock and write

									while(NVMCONbits.WR);        // wait while busy
								}
								break;

						#endif								//End of EEPROM code

						case ICD_GET_ID:
							icd_byte_tx(ICD_ID0);
							icd_byte_tx(ICD_ID1);
							icd_byte_tx(ICD_ID2);
							icd_byte_tx(ICD_ID3);
							icd_byte_tx(ICD_ID4);
							icd_byte_tx(ICD_ID5);
							icd_byte_tx(ICD_ID6);
							icd_byte_tx(ICD_ID7);
							break;

						case ICD_GET_STACK:
							icd_byte_tx(ICD_STACK_SIZE+1);		//+1 because the PPP firmware does not expect the stack pointer return as well
							icd_byte_tx(icd_stack_ptr);
							for (idx=0; idx<ICD_STACK_SIZE; idx++)
							{
								icd_byte_tx(*(icd_stack + idx));											//icd_byte_tx(icd_stack[idx]);
							}
							break;

						case ICD_RESET:
							rst_dev();
							break;

						default:
							break;
					}
				}
			}
			clear_bit(ICD_DATA_PIN_TRIS,ICD_DATA_PIN);				//Data line as output
			set_bit(ICD_DATA_PIN_PORT,ICD_DATA_PIN);				//Data output low	**** inverted
			__asm__ volatile("disi #0x0001");						//Restore all interrupts
		}
	}


	void ICD_PUSH(char macro_id)
	{
		//note: there is no specific stack overflow test here (there will be at the PC end though)
		if (icd_stack_ptr < ICD_STACK_SIZE)
		{
			*(icd_stack + icd_stack_ptr) = macro_id;			//Assign macro ID				//icd_stack[icd_stack_ptr] = macro_id;
		}
		icd_stack_ptr++;									//Move to next layer of stack
	}


	void ICD_POP()
	{
		//note: there is no specific stack underflow test here (there will be at the PC end though)
		icd_stack_ptr--;
	}


	//ICD transmit byte Function - Bit bangs out a data byte.
	char icd_byte_tx(char data)
	{
		char count, mask;

		set_bit(ICD_CLK_PIN_TRIS,ICD_CLK_PIN);				//Clock line as input
		clear_bit(ICD_DATA_PIN_TRIS,ICD_DATA_PIN);			//Data line as output
		set_bit(ICD_DATA_PIN_PORT,ICD_DATA_PIN);			//Data is 0 RTS		**** inverted

		if (wait_clock(0, 1))								//Wait for clock to go low
			return 255;

		for(count=0; count<8; count++)
		{
			mask = data & 0x01;								//Prepare data
			if(mask)										//Write data onto data line
				clear_bit(ICD_DATA_PIN_PORT,ICD_DATA_PIN);			// **** inverted
			else
				set_bit(ICD_DATA_PIN_PORT,ICD_DATA_PIN);			// **** inverted
			data = data >> 1;
			if (wait_clock(1<<ICD_CLK_PIN, 1))				//Wait for clock to go high
				return 254;
			if (wait_clock(0, 1))							//Wait for clock to go low
				return 253;
		}

		set_bit(ICD_DATA_PIN_TRIS,ICD_DATA_PIN);			//Data line as input end of byte
		if (wait_data(0))									//Wait for byte acknowledge
				return 252;
		if (wait_clock(1<<ICD_CLK_PIN, 1))					//Wait for byte acknowledge end
				return 251;
		return 0;											//Success!
	}



	//ICD receive byte Function - Collects a bit banged serial data byte.
	char icd_byte_rx(void)
	{
		unsigned char data = 0;
		unsigned char count;

		set_bit(ICD_DATA_PIN_TRIS,ICD_DATA_PIN);			//Data line as input
		set_bit(ICD_CLK_PIN_TRIS,ICD_CLK_PIN);				//Clock line as input
		for(count=0; count<8; count++)						//Receive Byte
		{
			data = data >> 1;
			if (wait_clock(0, 1))							//Wait for clock to go low
				return 0;

			if (test_bit(ICD_DATA_PIN_PORT,ICD_DATA_PIN) == 0) 		//Read incoming data on data pin	**** inverted
				data = data | 0x80;

			if (wait_clock(1<<ICD_CLK_PIN, 1))				//Wait for clock to go high
				return 0;
		}

		if (wait_data(1<<ICD_DATA_PIN))						//Wait for data to go high
			return 0;

		if (wait_clock(0, 1))								//Wait for clock to go low
			return 0;

		clear_bit(ICD_DATA_PIN_TRIS, ICD_DATA_PIN);			//ICD data is an output
		set_bit(ICD_DATA_PIN_PORT, ICD_DATA_PIN);			//ICD data 0 send Ack		**** inverted

		if (wait_clock(1<<ICD_CLK_PIN, 1))					//Wait for clock to go high
			return 0;

		set_bit(ICD_DATA_PIN_TRIS, ICD_DATA_PIN);			//ICD data clear ack
		return(data);
	}


	char wait_clock(char state, char timeout_inc)
	{
		unsigned int timeout = 0;

		while ((test_bit(ICD_CLK_PIN_PORT,ICD_CLK_PIN) != state) && (timeout < ICD_TIMEOUT))	//Wait for clk
			timeout = timeout +  timeout_inc;				//Increment time out

		if (timeout == ICD_TIMEOUT)							//Has the routine timed out
			return 1;
		return 0;
	}


	char wait_data(char state)
	{
		unsigned int timeout = 0;
		while ((test_bit(ICD_DATA_PIN_PORT, ICD_DATA_PIN) == state) && (timeout < ICD_TIMEOUT))	//Wait for data		**** inverted
			timeout = timeout +  1;							//Increment time out

		if (timeout == ICD_TIMEOUT)							//Has the routine timed out
			return 1;
		return 0;
	}


	char read_register(unsigned int address)
	{
		char RetVal;

		volatile char* register_ptr = (char*)address;

		RetVal = *register_ptr;

		return RetVal;
	}


	void write_register(unsigned int address, char data)
	{
		volatile char* register_ptr = (char*)address;

		*register_ptr = data;
	}


	void rst_dev()
	{
		//Perform device reset
		__asm__ volatile("reset");

		//If that didnt work then we need a alt approach
		RCONbits.SWDTEN = 1;
		delay_ms(200);
	}


//End of ICD functionality
#endif


void Wdt_msDelay(void)
{
	char i;
	for (i=0; i<75; i++)		//(99 x 10us) + Overhead = 1ms
	{
		MX_CLEAR_WATCHDOG;		//Clear Watchdog Timer
		delay_us(10);
	}
}


void Wdt_Delay_S(short delay)
{
	short i, j;
	for (i=0; i<delay; i++)
	{
		for (j=0; j<1000; j++)
			Wdt_msDelay();
	}
}


void Wdt_Delay_Ms(short delay)
{
	short i;
	for (i=0; i<delay; i++)
		Wdt_msDelay();
}


char FCI_FLOAT_TO_STRING(float Number, char Precision, char* String, char MSZ_String)
{
	int whole;
	char str_length;
	int idx;
	int stringidx = 0;
	float real, temp;
	char temp_string[10];

	whole = (int)Number;						//Convert to whole integer
	temp = whole;								//Convert whole number back to floating point

	if (Number >= 0)
	{
		real = Number - temp;					//Subtract whole number from floating point number
	}
	else
	{
		if (whole == 0)							//Fix -0 bug
		{
			String[stringidx] = '-';
			stringidx = stringidx + 1;
		}

		real = temp - Number;					//Subtract floating point number from whole number
	}

	str_length = FCI_TOSTRING(whole, temp_string, 5);		//Convert integer numbers to strings

	for (idx=0; idx<str_length; idx++)						//Copy whole part of number to string
	{
		if(stringidx < MSZ_String)
		{
			String[stringidx] = temp_string[idx];
			stringidx = stringidx + 1;
		}
	}

	if(stringidx < MSZ_String)						//Add decimal point to string
	{
		String[stringidx] = '.';
		stringidx = stringidx + 1;
	}

	for (idx = 0; idx < Precision; idx++)			//Output Real Part Of Number
	{
		if (stringidx >= MSZ_String)
			break;

		real = real * 10;
		whole = (int)real;							//Convert to whole integer
		temp = whole;								//Convert whole number back to floating point
		real = real - temp;							//Subtract whole number from floating point number

		String[stringidx] = '0' + whole;
		stringidx = stringidx + 1;
	}

	if(stringidx < MSZ_String)									//Add null termination to string
	{
		String[stringidx] = 0;
	}

	return stringidx;
}


char FCI_NUMBER_TO_HEX(short Number, char* String, char MSZ_String)
{
	int stringidx;
	int idx = 0;
	char temp;
	char digit[4] = {0,0,0,0};
	unsigned int num;

	//Check that output string is long enough to store result
	if(Number < 256 && MSZ_String < 4)
		return 0;

	if(Number > 255 && MSZ_String < 6)
		return 0;

	//Initialise start of string with hexadecimal indicator
	String[0] = '0';
	String[1] = 'x';

	//Number is positive
	if(Number < 0)
	{
		num = 0 - Number;
		num = 0xFFFF - (num - 1);
	}
	else
		num = Number;

	//Calculate hex values
	while (num > 0)
	{
		temp = num % 16;
		num = num / 16;

		if(temp > 9)
		{
			temp = temp - 10;
			digit[idx] = temp + 'A';
		}
		else
			digit[idx] = temp + '0';

		idx = idx + 1;
	}

	//Copy Hex values over to output string
	for (stringidx = 2; stringidx < (idx + 2); stringidx++)
		String[stringidx] = digit[idx - (stringidx - 1)];

	//Add null termination to string
	if(stringidx < MSZ_String)
	{
		String[stringidx] = 0;
	}

	return stringidx;
}


long FCI_STRING_TO_INT(char* String, char MSZ_String)
{
	char bNegative = 0;
	int idx = 0;
	long RetVal = 0;

	//Is number negative
	if(String[0] == '-')
	{
		bNegative = 1;
		idx = 1;
	}

	//While string contains a valid number
	while (idx < MSZ_String && String[idx] >= '0' && String[idx] <= '9')
	{
		//Pull character from string and add to running total
		RetVal = (long) RetVal * 10;
		RetVal = (long) RetVal + (String[idx] - '0');
		idx = idx + 1;
	}

	if (bNegative)
		RetVal = 0 - RetVal;

	return RetVal;
}


float FCI_STRING_TO_FLOAT(char* String, char MSZ_String)
{
	float RetVal = 0;
	float real_divider = 1;
	unsigned int whole;
	unsigned long real;
	int idx = 0;
	char offset = 0;
	char bNegative = 0;
	char Comp_String[8];

	//Is number negative
	if(String[0] == '-')
	{
		bNegative = 1;
		offset = 1;
	}

	//Scan for decimal point
	for(idx=offset; idx<MSZ_String; idx++)
	{
		if(String[idx] == '.')
			break;
	}

	//Convert whole portion of number from string
	whole = FCI_STRING_TO_INT (String, idx);

	//No decimal point found
	if(idx == MSZ_String)
	{
		RetVal = (int)whole;
		return RetVal;
	}

	//Collect real portion of number into new string
	offset = idx + 1;
	for(idx=offset; idx<MSZ_String; idx++)
	{
		if(String[idx] >= '0' && String[idx] <= '9')
		{
			Comp_String[idx - offset] = String[idx];
			real_divider = real_divider * 0.1;
		}
		else
			break;
	}

	//Convert real portion of number from string
	real = FCI_STRING_TO_INT (Comp_String, (idx - offset));

	//Adjust weighting of values and combine into one float variable
	RetVal = (float)real;
	RetVal = RetVal * real_divider;
	real_divider = (float)whole;
	RetVal = RetVal + real_divider;

	if (bNegative)
		RetVal = 0 - RetVal;

	return RetVal;
}


unsigned int float32_is_signaling_nan(float f)
{
      char* b;
      b = (char*)&f;
      if ((b[0] == 0)   &&
          (b[1] == 0)   &&
          (b[2] == 128) &&
          ((b[3] & 127) == 127))
      {
            return (0xFFFF);
      }
      return (0);
}

#endif