/*********************************************************************
 *                  Flowcode Internals Header File
 *
 * File: internals.h
 *
 * (c) 2009 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
 * -------+----+-----------------------------------------------------
 * 010107 | ST | Created
 * 010108 | SK | Edited for ICD Operation
 * 031108 | BR | Edited for integration with PPP
 * 130309 | BR | Added Floating point, Hex and Decimal String Coversion Functions
 * 160309 | BR | Added ICD Port Definitions
 * 160309 | BR | Added Watchdog Delay routines
 * 090409 | BR | Fixed minor definitions bug
 * 170409 | BR | Optimised Floating / String conversions
 * 210509 | BR | Fixed ICD Stop and wait functionality
 * 110809 | BR | Changed the amount of delay in the watchdog ms delay routine
 * 160909 | BR | Added BoostC and HiTECH definitions
 * 290909 | BR | Altered the Test_bit function and ICD arrays for HITECH
 * 161009 | BR | Added a missing semi colon and added the math.h library for 16F devices
 * 221009 | BR | Removed the FCI_TMP_STR and FCI_TMP_INT variables
 * 101209 | BR | Altered timeout val and in <dbg_hold> routine got rid of initial delay and comms now wait forever
 * 181209 | BR | Edited reset function for use with the HITECH compiler.
 ********************************************************************/

#ifndef internals_h
#define internals_h

//BR 22/10/09 - Are these needed? 10F devices could not allocate enough memory to create array
//unsigned char FCI_TMP_STR[20];
//int FCI_TMP_INT;

//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_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_FLOAT_TO_STRING(float Number, char Precision, char* String, char MSZ_String);
char FCI_NUMBER_TO_HEX(short Number, char* String, char MSZ_String);
short 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);
char isinf(float f);

/******************** BOOST C SPECIFICS *****************/
#ifdef _BOOSTC

	#include <float.h>

	#pragma CLOCK_FREQ 				MX_CLK_SPEED
	#define MX_INTERRUPT_MACRO		interrupt
	#define MX_CLEAR_WATCHDOG		clear_wdt()

	#define st_bit(var,bitno) 		set_bit(var,bitno)
	#define cr_bit(var,bitno) 		clear_bit(var,bitno)
	#define ts_bit(var,bitno)		test_bit(var,bitno)

#endif

/******************* HITECH C SPECIFICS *****************/
#ifdef HI_TECH_C

	#include <htc.h> 				//Delay Params
	#include "definitions.h"
	#include <math.h>

	#define _XTAL_FREQ 				MX_CLK_SPEED
	#define MX_INTERRUPT_MACRO		interrupt myisr
	#define MX_CLEAR_WATCHDOG		CLRWDT()

	char ts_bit(char reg, char bt);
	char test_bit(char reg, char bt);
	void delay_10us(char del);
	void delay_us(char del);
	void delay_ms(char del);
	void delay_s(char del);
	void nop(void);
	float float32_add(float a, float b);
	float float32_sub(float a, float b);
	float float32_mul(float a, float b);
	float float32_div(float a, float b);
	float float32_rem(float a, float b);
	char float32_is_signaling_nan(float a);
	char float32_eq(float a, float b);
	char float32_ge(float a, float b);
	char float32_gt(float a, float b);
	char float32_le(float a, float b);
	char float32_lt(float a, float b);
	short float32_to_int32 (float a);
	float float32_from_int32 (short a);

	#define INTE					INT0IE
	#define INTF					INT0IF

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

	char ts_bit(char reg, char bt)
	{
		if (bt)
			return 1;
		return 0;
	}

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

	void delay_10us(char del)
	{
		char count;
		for(count=0; count<10; count++)
		{
			delay_us(del);
		}
	}

	void delay_us(char del)
	{
		while (del > 1)
		{
			__delay_us(1);
			del = del - 1;
		}
	}

	void delay_ms(char del)
	{
		while (del > 1)
		{
			__delay_ms(1);
			del = del - 1;
		}
	}

	void delay_s(char del)
	{
		char i;
		for(i=0; i<del; i++)
		{
			delay_ms(250);
			delay_ms(250);
			delay_ms(250);
			delay_ms(250);
		}
	}

	void nop()
	{
		asm("NOP");
	}

	float float32_add(float a, float b)
	{
		return (a + b);
	}

	float float32_sub(float a, float b)
	{
		return (a - b);
	}

	float float32_mul(float a, float b)
	{
		return (a * b);
	}

	float float32_div(float a, float b)
	{
		return (a / b);
	}

	float float32_rem(float a, float b)
	{
		return (fmod(a, b));
	}

	char float32_is_signaling_nan(float a)
	{
		return isinf(a);
	}

	char float32_eq(float a, float b)
	{
		return (a == b);
	}

	char float32_ge(float a, float b)
	{
		return (a >= b);
	}

	char float32_gt(float a, float b)
	{
		return (a > b);
	}

	char float32_le(float a, float b)
	{
		return (a <= b);
	}

	char float32_lt(float a, float b)
	{
		return (a < b);
	}

	short float32_to_int32 (float a)
	{
		return ((int)a);
	}

	float float32_from_int32 (short a)
	{
		return ((float)a);
	}

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

		if(iPos < tmp)
			tmp = *sStr1;
		else tmp = 0;
		return (tmp);
	}

	char FCI_GETLENGTH(char* sStr1, char iStr1_len)
	{
		char tmp;
		for (tmp = 0; tmp < iStr1_len; tmp++)
		{
			if (*sStr1 == 0)
				break;
			sStr1++;
		}
		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 == 0)
				break;
			sSrc++;
		}

		if (iStart >= idx)
		{
			*sDst = 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;

			sSrc = 0;

			for (idx = 0; idx < iStart; idx++)	//Move to the correct place in the source string
				sSrc++;

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

			if (idx < iDst_len)					//add terminating null (if we can)
				*sDst = 0;
		}
		return (idx);
	}

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

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

		sSrc = 0;

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

		if (idx < iDst_len)						//add terminating null (if we can)
			*sDst = 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 == 0)
				break;
			sSrc++;
		}

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

		delta = idx - iCount;					//get the offset

		sSrc = 0;
		for (idx = 0; idx < delta; idx++)		//Move to the correct place in the source string
			sSrc++;

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

		if (idx < iDst_len)						//add terminating null (if we can)
			*sDst = 0;

		return (idx);
	}

	char FCI_CONCATENATE(char* sSrc1, char iSrc1_len, char* sSrc2, char iSrc2_len, 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 = *sSrc1;
				sDst++;
				sSrc1++;

				if (*sSrc1 != 0)
					idx++;
				else break;
			}
		}

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

				if (*sSrc2 != 0)
					idx++;
				else break;
			}
		}

		if (idx < iDst_len)						//add terminating null (if we can)
			*sDst = 0;

		return (idx);
	}

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

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

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

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

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

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

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

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

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

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

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

		//add terminating null (if we can)
		if (idx < iDst_len)
		{
			sDst[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[idx];
				sDst[idx] = ch;
				if (ch != 0)
				{
					if ((ch >= 'A') && (ch <= 'Z'))
					{
						sDst[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[idx];
				sDst[idx] = ch;
				if (ch != 0)
				{
					if ((ch >= 'a') && (ch <= 'z'))
					{
						sDst[idx] = (ch & 0xDF);
					}
				} else {
					break;
				}
			}
		}
	}

	char FCI_COMPARE(char* sSrc1, char iSrc1_len, char* sSrc2, char iSrc2_len, char iNoCase)
	{
		char iRetVal = 0;
		char idx;
		char ch1, ch2;

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

				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);
				}

				sSrc1++;
				sSrc2++;
			}
			else
			{
				if (*sSrc1 == 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 == 0)
				return (0);					//end of source1 as well, so ok
			else
				return (255);				//not at end of source2, so return -1
		}
	}
#endif


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

	//ICD Clock Port Defines
	#if (ICD_CLK_PORT == 0)
		#ifdef porta
			#define ICD_CLK_PIN_TRIS trisa
			#define ICD_CLK_PIN_PORT porta
		#endif
		#ifdef gpio
			#define ICD_CLK_PIN_PORT gpio
			#ifdef trisgpio
				#define ICD_CLK_PIN_TRIS trisgpio
			#endif
			#ifdef trisio
				#define ICD_CLK_PIN_TRIS trisio
			#endif
			#ifdef tris
				#define ICD_CLK_PIN_TRIS tris
			#endif
		#endif
	#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)
		#ifdef PORTA
			#define ICD_DATA_PIN_TRIS trisa
			#define ICD_DATA_PIN_PORT porta
		#endif
		#ifdef GPIO
			#define ICD_DATA_PIN_PORT gpio
			#ifdef trisgpio
				#define ICD_DATA_PIN_TRIS trisgpio
			#endif
			#ifdef trisio
				#define ICD_DATA_PIN_TRIS trisio
			#endif
			#ifdef tris
				#define ICD_DATA_PIN_TRIS tris
			#endif
		#endif
	#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
	#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();
	unsigned char icd_byte_tx(char data);
	unsigned char icd_byte_rx(void);
	unsigned char wait_clock(char state, char timeout_inc);
	unsigned 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 int_store = 0;
		unsigned char idx;
		char tempdata = 0;

		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?
		{
			int_store = intcon;							//Store interrupt register
			intcon = 0;									//Disable all 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

					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?
							#ifdef _BOOSTC
								#ifndef MX_EEADR
									#ifdef EEADR
										#define MX_EEADR eeadr
									#endif
									#ifdef EEADRL
										#define MX_EEADR eeadrl
									#endif
								#endif
								#ifndef MX_EEDATA
									#ifdef EEDATA
										#define MX_EEDATA eedata
									#endif
									#ifdef EEDATAL
										#define MX_EEDATA eedatal
									#endif
								#endif
							#endif

							case ICD_RD_EE:					//Read Contents Of EEPROM Command

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

								#ifdef _BOOSTC
									MX_EEADR = address & 0xff;
									#if (MX_EE_SIZE > 256)
										eeadrh = (address & 0xff00) >> 8;
									#endif
									#ifdef MX_EE_TYPE2
										cr_bit(eecon1, EEPGD);
									#endif
									#ifdef MX_EE_TYPE3
										cr_bit(eecon1, EEPGD);
										cr_bit(eecon1, CFGS);
									#endif
										st_bit(eecon1, RD);

									icd_byte_tx(MX_EEDATA);
								#endif
								#ifdef HI_TECH_C
									icd_byte_tx(eeprom_read(address));
								#endif
								break;

							case ICD_WR_EE:					//Write Byte to EEPROM Command

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

								#ifdef _BOOSTC
									while (ts_bit(eecon1, WR));				//wait for previous EE writes to complete...

									#if (MX_EE_SIZE > 256)
										eeadrh = (address & 0xff00) >> 8;
									#endif

									MX_EEADR = address & 0xff;
									MX_EEDATA = tempdata;

									#ifdef MX_EE_TYPE2
										cr_bit(eecon1, EEPGD);
									#endif
									#ifdef MX_EE_TYPE3
										cr_bit(eecon1, EEPGD);
										cr_bit(eecon1, CFGS);
									#endif

									st_bit(eecon1, WREN);
									eecon2 = 0x55;
									eecon2 = 0xAA;
									st_bit(eecon1, WR);
									while (ts_bit(eecon1, WR));				//wait for EE write to complete...
								#endif
								#ifdef HI_TECH_C
									eeprom_write(address, tempdata);
								#endif

								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
			intcon = int_store;										//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()
	{
		//Disable all interrupts
		intcon = 0;
		#ifdef PIE1 | pie1
			pie1 = 0x00;
		#endif
		#ifdef PIE2 | pie2
			pie2 = 0x00;
		#endif

		//Reset all port pins back to inputs
		#ifdef PORTA | porta
			trisa = 0xff;
			porta = 0x00;
		#endif
		#ifdef GPIO | gpio
			gpio = 0x00;
			#ifdef TRISGPIO | trisgpio
				trisgpio = 0xff;
			#endif
			#ifdef TRISIO | trisio
				trisio = 0xff;
			#endif
			#ifdef TRIS | tris
				tris = 0xff;
			#endif
		#endif
		#ifdef PORTB | portb
			trisb = 0xff;
			portb = 0x00;
		#endif
		#ifdef PORTC | portc
			trisc = 0xff;
			portc = 0x00;
		#endif
		#ifdef PORTD | portd
			trisd = 0xff;
			portd = 0x00;
		#endif
		#ifdef PORTE | porte
			trise = 0xff;
			porte = 0x00;
		#endif
		#ifdef PORTF | portf
			trisf = 0xff;
			portf = 0x00;
		#endif

		//Software Reset
		#ifdef _BOOSTC
			reset();			//Supported in BoostC
		#endif
		#ifdef HI_TECH_C
			intcon=0;			//Disable Interrupts
			option_reg=8;		//Enable watchdog timer with 1:1 prescale
			while(1) ;   		//Wait forever for watchdog reset
		#endif
	}

//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;
	char idx;
	char stringidx = 0;
	float real, temp;
	char old;
	char temp_string[10];

	#ifdef _BOOSTC									//BoostC Floating Point

		float_rounding_mode = 3;					//Always round to zero
		whole = float32_to_int32(Number);			//Convert to whole integer
		temp = float32_from_int32(whole);			//Convert whole number back to floating point

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

			real = float32_sub(temp, Number);		//Subtract floating point number from whole number
		}
	#endif

	#ifdef HI_TECH_C								//HiTECH Floating Point

		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
		}
	#endif

	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;

		#ifdef _BOOSTC								//BoostC Floating Point
			real = float32_mul(real, 10);
			whole = float32_to_int32(real);			//Convert to whole integer
			temp = float32_from_int32(whole);		//Convert whole number back to floating point
			real = float32_sub(real, temp);			//Subtract whole number from floating point number
		#endif
		#ifdef HI_TECH_C							//HiTECH Floating Point
			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
		#endif

		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)
{
	char stringidx;
	char 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;
}


short FCI_STRING_TO_INT(char* String, char MSZ_String)
{
	char bNegative = 0;
	char idx = 0;
	short 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 = RetVal * 10;
		RetVal = 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;
	int whole, real;
	char idx = 0;
	char idx2 = 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)
	{
		#ifdef _BOOSTC
			RetVal = float32_from_int32(whole);
		#endif
		#ifdef HI_TECH_C
			RetVal = (int)whole;
		#endif
		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];
			#ifdef _BOOSTC
				real_divider = float32_mul(real_divider, 0.1);
			#endif
			#ifdef HI_TECH_C
				real_divider = real_divider * 0.1;
			#endif
		}
		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
	#ifdef _BOOSTC

		RetVal = float32_from_int32(real);
		RetVal = float32_mul(RetVal, real_divider);
		real_divider = float32_from_int32(whole);
		RetVal = float32_add(RetVal, real_divider);

		if (bNegative)
			RetVal = float32_sub(0, RetVal);

	#endif
	#ifdef HI_TECH_C

		RetVal = (float)real;
		RetVal = RetVal * real_divider;
		real_divider = (float)whole;
		RetVal = RetVal + real_divider;

		if (bNegative)
			RetVal = 0 - RetVal;
	#endif

	return RetVal;
}


char isinf(float f)
{
      char* b;
      b = (char*)&f;
      if ((b[0] == 0)   &&
          (b[1] == 0)   &&
          (b[2] == 128) &&
          ((b[3] & 127) == 127))
      {
            return (255);
      }
      return (0);
}

#endif