// ******************************************************************
//
//    File: App_SPDC100_SPP_Demo_Client.c
//    Date: 2025-07-29
//    Version: 1.00
//    Mod: SLS
//    Case: General
//	  Notes:
//		Requires reference to both BGAPI v3.3 documentation (https://docs.silabs.com/bluetooth/3.3/bluetooth-api-reference-bgapi-headers/)
//		and BGAPI v2.13 (https://www.silabs.com/documents/public/reference-manuals/bluetooth-api-reference.pdf)
//		BLEGetEvent and BLETxRxModuleCommand both depend on knowledge of these documents.
//
// 	  V1.00:
//    ------
//    - Initial release, based on App_SPPD101_SPP_Demo_Client.c, name changed
//		to disambiguate from server configuration, adding bidirectional comms
//
// ******************************************************************

/*
TODO - Need all BLETxRx conditionals to check 2-byte response for error code
*/

#include "twn4.sys.h"
#include "apptools.h"

#define LOG_CHANNEL CHANNEL_USB
//#define HOST_CHANNEL CHANNEL_HOST //Commented out to remove obsoleted functionality

// Toggle this to enable/disable debugging
#define DEBUG_ENABLED 1 // set to 1-enable, 0-disable debug output

#if DEBUG_ENABLED
#define DEBUG_HEX(String, Data, ByteCnt)           \
	do                                             \
	{                                              \
		WriteString(LOG_CHANNEL, String);          \
		WriteBytesHex(LOG_CHANNEL, Data, ByteCnt); \
		WriteString(LOG_CHANNEL, "\r");            \
	} while (false)

#define DEBUG_STRING(String)              \
	do                                    \
	{                                     \
		WriteString(LOG_CHANNEL, String); \
		WriteString(LOG_CHANNEL, "\r");   \
	} while (false)

#define DEBUGUINT16HEX(String, UInt16)       \
	do                                       \
	{                                        \
		WriteString(LOG_CHANNEL, String);    \
		WriteUInt16Hex(LOG_CHANNEL, UInt16); \
		WriteString(LOG_CHANNEL, "\r");      \
	} while (false)

#else
#define DEBUG_HEX(String, Data, ByteCnt)
#define DEBUG_STRING(String)
#define DEBUGUINT16HEX(UInt16)

#endif


#define ADVERTISE_PACKETS 0
#define SCAN_PACKETS 1

#define MAXIDBYTES 10
#define MAXIDBITS (MAXIDBYTES * 8)

byte ID[MAXIDBYTES];
int IDBitCnt;
int TagType;

byte LastID[MAXIDBYTES];
int LastIDBitCnt;
int LastTagType;

bool bgm220 = false;
uint32_t ble2ServiceHandle;
const byte bgm220VersionString[] = {0x42, 0x54, 0x32, 0x2E, 0x30, 0x31, 0x45, 0x4C, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x00};
//const byte bgm1xxVersionString[] = {0x42, 0x54, 0x31, 0x2E, 0x30, 0x37, 0x45, 0x4C, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x00};

// ******************************************************************
// ****** Section containing configurable behaviour *****************
// ******************************************************************

// If necessary adjust default parameters
#ifndef __APP_CONFIG_H__
#define LFTAGTYPES 0
#define HFTAGTYPES 0
#define ICLASS_READMODE_VALUE ICLASS_READMODE_UID
#endif

byte rxBuffer[256];
int rxDataBytes;
bool connected = false;
int connClosedEvent;
int scanResponseEvent;
int openConnCommand;
int openConnEvent;
int stopScanCommand;

const unsigned char AppManifest[] =
	{
		USB_KEYBOARDREPEATRATE, 1, 1,
		EXECUTE_APP, 1, EXECUTE_APP_ALWAYS,
		TLV_END};

// Adjust parameters as configured in AppBlaster
const byte TLVBytes[] = {ICLASS_READMODE, 1, ICLASS_READMODE_VALUE, TLV_END};
void ConfigSetParameters(void)
{
	SetParameters(TLVBytes, sizeof(TLVBytes));
}

// Setup tag types as configured in AppBlaster
void ConfigSetTagTypes(void)
{
	if (LFTAGTYPES || HFTAGTYPES)
		SetTagTypes(LFTAGTYPES, HFTAGTYPES);
}
// ****************************************************************************
// BLE Services and Characteristics
// ****************************************************************************


// SPP Service
TBLEGATTDBSERVICE BLE2Service = {
	.uuid.UUIDLength = 16,
	.uuid.UUID = {0x8e, 0xdf, 0xae, 0x3d, 0x9b, 0xcd, 0x0e, 0x88, 0x74, 0x42, 0x12, 0x41, 0x04, 0xc0, 0x44, 0x5a},
	.type = 0,	  // Primary service
	.property = 1 // Advertised Service
};

TBLEGATTDBCHARACTERISTIC BLEReadCharacteristic = {
	.uuid.UUIDLength = 16,
	.uuid.UUID = {0x52, 0xc7, 0x9e, 0x16, 0x9d, 0x48, 0x22, 0xaa, 0x43, 0x4c, 0x0a, 0x2f, 0xdf, 0x9e, 0xc2, 0x43},
	.property = SL_BT_GATTDB_CHARACTERISTIC_NOTIFY | SL_BT_GATTDB_CHARACTERISTIC_READ,
	.valueType = BLE_GATTDB_VALUETYPE_VARIABLELENGTH,
	.flag = 0,
	.security = 0,
	.maxValueLength = 256};

TBLEGATTDBCHARACTERISTIC BLEWriteCharacteristic = {
	.uuid.UUIDLength = 16,
	.uuid.UUID = {0xb6, 0x6e, 0x2e, 0x51, 0x57, 0x4a, 0xef, 0xa2, 0x2b, 0x4a, 0xf0, 0xad, 0x9f, 0x33, 0x97, 0xa8},
	.property = SL_BT_GATTDB_CHARACTERISTIC_WRITE_NO_RESPONSE | SL_BT_GATTDB_CHARACTERISTIC_WRITE,
	.valueType = BLE_GATTDB_VALUETYPE_VARIABLELENGTH,
	.flag = 0,
	.security = 0,
	.maxValueLength = 256};

byte connHandle = 0;
void OnStartup(void)
{
	// SetHostChannel(HOST_CHANNEL);//Commented out to remove obsoleted functionality

	// Adjust parameters
	ConfigSetParameters();
	// Set tag types
	ConfigSetTagTypes();

	// Init LEDs
	LEDInit(REDLED | GREENLED);
	// Turn on green LED
	LEDOn(GREENLED);
	// Turn off red LED
	LEDOff(REDLED);
	// Make some noise at startup at low volume
	SetVolume(30);
	BeepLow();
	BeepHigh();
	// Continue with maximum  volume
	SetVolume(100);

	// No transponder found up to now
	LastTagType = NOTAG;
}


//Must be edited to match Server file's "UserData" array, unless algorithm for identifying
//remote device from advertisement is changed
const byte AdvertisingArray[] =
	{
		// Flags - See Bluetooth 4.0 Core Specification , Volume 3, Appendix C, 18.1.
		0x02, // Length of the Flags field - 2 bytes-
		0x01, // Type of the Flags field.
		0x06, // Flags field. Device in LE General discoverable mode

		// Manufacturer data
		0x08, // Length of the Manufacturer Data field - 6+1 bytes -
		0x09, // Name
		'T',  // Local name "TWN4BLE"
		'W',
		'N',
		'4',
		'B',
		'L',
		'E',

		// TX power field advertisement, iOS apps may need this
		0x02, // ad field length = 2 bytes
		0x0A, // ad field type = 0x0A [TX Power)
		0x03, // TX power in dBm

		0x03,
		0xFF, // Manufacturer Specific data
		0x52, // Company Identifier Code - ELATEC 0x0752
		0x07, // Company Identifier Code

		0x05,
		0x05, // Complete List of 32-bit Service Class UUIDs
		0x3A,
		0x4B,
		0x5C,
		0x6D};

const TBLEConfig DefaultBleConfig = {

	.ConnectTimeout = 10000,
	.Power = 50,
	.BondableMode = 0x81,
	.AdvInterval = 0x00a0,
	.ChannelMap = 0x07,
	.DiscoverMode = 0x04,
	.ConnectMode = 0x02,
	.SecurityFlags = 0x00,
	.IOCapabilities = 0x00,
	.Passkey = 0x00000000,
};
static TBLEConfig BleConfig;

bool BLEWaitForEvent(int event, byte* eventData, int* eventDataBytes, int maxEventDataBytes, int timeout){
	byte currentEventData[128];
	int currentEventDataBytes;
	unsigned long timer = GetSysTicks();
	while((GetSysTicks() - timer) < timeout){
		int receivedEvent = BLEGetEvent(currentEventData, sizeof(currentEventData), &currentEventDataBytes);
		if((receivedEvent != BLE_NO_MESSAGE)&(receivedEvent != scanResponseEvent)){
            DEBUG_HEX( "Received Event Data", currentEventData, currentEventDataBytes);
            DEBUG_STRING(BLEGetMessageString(receivedEvent));
        }
        if(receivedEvent == event){
        	int bytesToCopy = MIN(maxEventDataBytes, currentEventDataBytes);
        	CopyBytes(eventData, currentEventData, bytesToCopy);
        	*eventDataBytes = bytesToCopy;
        	return true;
        }
	}
	//Timeout
	DEBUG_STRING("Timeout Waiting For Event ");
	DEBUG_STRING(BLEGetMessageString(event));
	return false;
}

void BLE_WaitForConnection(void){
    byte ScanDataBuffer[256];
    int ScanDataBufferBytes;
    //Set up scanning command
    byte scanCommand[] = {0x01, 0x01};		//scan on 1M phy for limited and general discoverable devices
    bool foundServer = false;
    //Start discovery based on HW module
    if(bgm220){
	    DEBUG_STRING("Start Discovery Scanning");
	    if(!BLETxRxModuleCommand(CMD_SCANNER_START, scanCommand, sizeof(scanCommand), ScanDataBuffer, &ScanDataBufferBytes, sizeof(ScanDataBuffer))){
			DEBUG_STRING("Failed Initiating scan command");
		}
	}else{
		DEBUG_STRING("Start Discovery Command");
		if(!BLETxRxModuleCommand(CMD_LE_GAP_START_DISCOVERY, scanCommand, sizeof(scanCommand), ScanDataBuffer, &ScanDataBufferBytes, sizeof(ScanDataBuffer))){
			DEBUG_STRING("Failed Initiating Discovery command");
		}
	}
    while(!foundServer){
    	if(bgm220){
	        if(BLEWaitForEvent(BLE_EVENT_SCANNER_SCAN_REPORT, ScanDataBuffer, &ScanDataBufferBytes, sizeof(ScanDataBuffer), 1000)){
	        	//Very crude method of identifying remote device
	        	if(CompBytes(&ScanDataBuffer[18], AdvertisingArray, sizeof(AdvertisingArray))){
	            	DEBUG_HEX("SUCCESS!", ScanDataBuffer, ScanDataBufferBytes);
	               	foundServer = true;
	        	}
	
	        }
	    }else{
        	if(BLEWaitForEvent(BLE_EVENT_LE_GAP_SCAN_RESPONSE, ScanDataBuffer, &ScanDataBufferBytes, sizeof(ScanDataBuffer), 1000)){
	            //Very crude method of identifying remote device
	            if(CompBytes(&ScanDataBuffer[11], AdvertisingArray, sizeof(AdvertisingArray))){
	            	DEBUG_HEX("SUCCESS!", ScanDataBuffer, ScanDataBufferBytes);
	               	foundServer = true;
	        	}
	        }
	    }
    }
    if(!BLETxRxModuleCommand(stopScanCommand, scanCommand, 0, ScanDataBuffer, &ScanDataBufferBytes, sizeof(ScanDataBuffer))){
		DEBUG_STRING("Failed Ending Discovery Process");
	}
    //Establish remote device contains correct service and characteristics and reetrieve handles
    byte remoteAddress[6];
    byte addressType;
    if(bgm220){
	    CopyBytes(remoteAddress, &ScanDataBuffer[1], 6);
    	addressType = ScanDataBuffer[7];
    }else{
	    CopyBytes(remoteAddress, &ScanDataBuffer[2], 6);
    	addressType = ScanDataBuffer[8];
    }
    
    byte connectCommand[16];
    CopyBytes(connectCommand, remoteAddress, 6);
    connectCommand[6] = addressType;
    connectCommand[7] = 1;	//1M PHY
    byte connectResponse[16];
    int connectResponseBytes;
    if(bgm220){
    	openConnCommand = CMD_CONNECTION_OPEN;
    }else{
    	openConnCommand = CMD_LE_GAP_CONNECT;
    }
    if(!BLETxRxModuleCommand(openConnCommand, connectCommand, 8, connectResponse, &connectResponseBytes, sizeof(connectResponse))){
    	DEBUG_STRING("Failed Connect Command");
    	return;
    }
    DEBUG_HEX("CMD_LE_GAP_CONNECT", connectResponse, connectResponseBytes);
    //Ensure return with no error, record connection handle
    if((connectResponse[0] == 0)&(connectResponse[1] == 0)){
    	connHandle = connectResponse[2];
    }else{
    	DEBUG_HEX("Connect Command Error", connectResponse, connectResponseBytes);
    	return;
    }

    if(!BLEWaitForEvent(openConnEvent, connectResponse, &connectResponseBytes, sizeof(connectResponse), 1000)){
    	DEBUG_STRING("Timeout waiting for connection event");
    	return;
    }
    DEBUG_STRING("Connection Established");



	int TimerStartTime;
	//Following procedures are necessary to retrieve the handles of remote device characteristics,
	//allowing interaction with remote device.
    DEBUG_STRING("------ Discover primary services by UUID ---------");
    byte discoverServicesCommand[18] = {[0]=connHandle, [1]=BLE2Service.uuid.UUIDLength};
    byte discoverServicesResponse[32];
    int discoverServicesResponseBytes;
    CopyBytes(&discoverServicesCommand[2], BLE2Service.uuid.UUID, BLE2Service.uuid.UUIDLength);
    if(!BLETxRxModuleCommand(CMD_GATT_DISCOVER_PRIMARY_SERVICES_BY_UUID, 
    											discoverServicesCommand, 
    											sizeof(discoverServicesCommand), 
    											discoverServicesResponse, 
    											&discoverServicesResponseBytes, 
    											sizeof(discoverServicesResponse))){
    	DEBUG_STRING("Failed Discover Primary Services command");
    }
    DEBUG_HEX("Discover Services Response", discoverServicesResponse, discoverServicesResponseBytes);

    TBLEUUID DummyUUID;
    TimerStartTime = GetSysTicks();
    bool foundService = false;
    while (!foundService)
    {
        if (GetSysTicks()-TimerStartTime >= 3000)
        {
            DEBUG_STRING("Timeout waiting for Priamry Service");
            return;
        }

        int event = BLEGetEvent(discoverServicesResponse, sizeof(discoverServicesResponse), &discoverServicesResponseBytes);
        if (event == BLE_EVENT_GATT_PROCEDURE_COMPLETED){
            DEBUG_STRING("GATT Procedure complete before receiving primary service");
            return;
        }
        else if (event == BLE_EVENT_GATT_SERVICE)
        {
        	DEBUG_HEX("GATT Service Event Data", discoverServicesResponse, discoverServicesResponseBytes);
          	DummyUUID.UUIDLength = discoverServicesResponse[5];
          	CopyBytes(DummyUUID.UUID, &discoverServicesResponse[6], DummyUUID.UUIDLength);
          	if(CompBytes(DummyUUID.UUID, BLE2Service.uuid.UUID, DummyUUID.UUIDLength)){
          		CopyBytes((byte*)&ble2ServiceHandle, &discoverServicesResponse[1], 4);
          		BLE2Service.ServiceHandle = (uint16_t)(discoverServicesResponse[3]<<8);//Little endian, value shouldn't ever be higer than 0xFF
          		foundService = true;
          		DEBUG_HEX("Found Service", &BLE2Service.ServiceHandle, 2);
          		DEBUG_HEX("Stored uint32 service handle", &ble2ServiceHandle, 4);
          	}else{
          		DEBUG_HEX("Rejected GATT Service Message", discoverServicesResponse, discoverServicesResponseBytes);
          	}
        }
	}

    DEBUG_STRING("------ Discover characteristics ------------------");
    byte discoverCharacteristicCommand[5] = {[0]=connHandle};
    CopyBytes(&discoverCharacteristicCommand[1], (byte*)&ble2ServiceHandle, 4);
    DEBUG_HEX("Discover Char Command", discoverCharacteristicCommand, 5);
    byte discoverCharacteristicResponse[32];
    int discoverCharacteristicResponseBytes;
    if(!BLETxRxModuleCommand(CMD_GATT_DISCOVER_CHARACTERISTICS, 
    											discoverCharacteristicCommand, 
    											sizeof(discoverCharacteristicCommand), 
    											discoverCharacteristicResponse, 
    											&discoverCharacteristicResponseBytes, 
    											sizeof(discoverCharacteristicResponse))){
    	DEBUG_STRING("Failed Discover Primary Services command");
    }
    DEBUG_HEX("Discover Characteristics response", discoverCharacteristicResponse, discoverCharacteristicResponseBytes);

    TimerStartTime = GetSysTicks();
   	bool foundReadCharacteristic = false;
   	bool foundWriteCharacteristic = false;
   	bool procedureComplete = false;
    while (!(foundReadCharacteristic&foundWriteCharacteristic&procedureComplete))
    {
        if (GetSysTicks()-TimerStartTime >= 3000)
        {
            DEBUG_STRING("Timeout waiting for Characteristics");
            return;
        }

        int event = BLEGetEvent(discoverCharacteristicResponse, sizeof(discoverCharacteristicResponse), &discoverCharacteristicResponseBytes);
        if (event == BLE_EVENT_GATT_PROCEDURE_COMPLETED){
            procedureComplete = true;
        }
        else if (event == BLE_EVENT_GATT_CHARACTERISTIC)
        {
          	DummyUUID.UUIDLength = discoverCharacteristicResponse[4];
          	CopyBytes(DummyUUID.UUID, &discoverCharacteristicResponse[5], DummyUUID.UUIDLength);
          	if(CompBytes(DummyUUID.UUID, BLEReadCharacteristic.uuid.UUID, DummyUUID.UUIDLength)){
          		BLEReadCharacteristic.CharacteristicHandle = discoverCharacteristicResponse[1];//Little endian, value shouldn't ever be higer than 0xFF
          		foundReadCharacteristic = true;
          		DEBUG_HEX("Found Read Char", &BLEReadCharacteristic.CharacteristicHandle, 2);
          	}else if(CompBytes(DummyUUID.UUID, BLEWriteCharacteristic.uuid.UUID, DummyUUID.UUIDLength)){
          		BLEWriteCharacteristic.CharacteristicHandle = discoverCharacteristicResponse[1];
          		foundWriteCharacteristic = true;
          		DEBUG_HEX("Found Write Char", &BLEWriteCharacteristic.CharacteristicHandle, 2);
          	}else{
          		DEBUG_HEX("Rejected GATT Service Message", discoverCharacteristicResponse, discoverCharacteristicResponseBytes);
          	}
        }
	}

    DEBUG_STRING("------ Set characteristic notification -----------");
    //Enable notifications on read characteristic
    byte setNotificationCmd[] = {connHandle, (byte)BLEReadCharacteristic.CharacteristicHandle, 0x00, 0x01};
    byte setNotificationRsp[32];
    int setNotificationRspBytes;
    if(!BLETxRxModuleCommand(CMD_GATT_SET_CHARACTERISTIC_NOTIFICATION,
    												setNotificationCmd, 
    												sizeof(setNotificationCmd), 
    												setNotificationRsp, 
    												&setNotificationRspBytes, 
    												sizeof(setNotificationRsp))){
    	DEBUG_STRING("Failed Set Characteristic Notification Command");
    	return;
    }
    if(!BLEWaitForEvent(BLE_EVENT_GATT_PROCEDURE_COMPLETED, setNotificationRsp, &setNotificationRspBytes, sizeof(setNotificationRsp), 1000)){
    	DEBUG_STRING("Timeout Waiting for GATT Procedure Complete on Set Notification");
    	return;
    }
    DEBUG_HEX("EVENT_GATT_PROCEDURE_COMPLETE Error Code", setNotificationRsp, setNotificationRspBytes);

    connected = true;
	
    return;
}


int main(void)
{
	StartTimer(5000);
	while (!TestTimer())
		; // Starting delay 3 sec
	DEBUG_STRING("GATT Server running");

	// Init BLE
	BleConfig = DefaultBleConfig;
	if (BLEPresetConfig(&BleConfig))
		DEBUG_STRING("BLEPresetConfig: Ok");
	else
		DEBUG_STRING("BLEPresetConfig: Failed");

	if (BLEInit(BLE_MODE_TRANSPARENT))
		DEBUG_STRING("BLEInit: Ok");
	else
		DEBUG_STRING("BLEInit: Failed");
	byte HWVersion[16];
	byte BootString[12];
	if(!BLEGetVersion(HWVersion, BootString)){
		DEBUG_STRING("BLEGetVersion failed");
	}else{
		DEBUG_HEX("BLE HW Version", HWVersion, 16);
		DEBUG_HEX("BLE Boot String", BootString, 12);
	}
	//Check HW version of connected module
	//Depends on .gbl file name programmed on module
	//Can also check BootString[0], which should be '0x02'
	//for BGM1xx and '0x03' for BGM220
	int connClosedEvent;
	if(CompBytes(HWVersion, bgm220VersionString, 16)){
		bgm220 = true;
		connClosedEvent = BLE_EVENT_CONNECTION_CLOSED;
		scanResponseEvent = BLE_EVENT_SCANNER_SCAN_REPORT;
		openConnCommand = CMD_CONNECTION_OPEN;
		openConnEvent =	BLE_EVENT_CONNECTION_OPENED;
		stopScanCommand = CMD_SCANNER_STOP;
	}else{
		DEBUG_STRING("Comp HWVersion and VersionString");
		DEBUG_STRING((const char*)HWVersion);
		DEBUG_STRING((const char*)bgm220VersionString);
		connClosedEvent = BLE_EVENT_LE_CONNECTION_CLOSED;
		scanResponseEvent = BLE_EVENT_LE_GAP_SCAN_RESPONSE;	
		openConnCommand = CMD_LE_GAP_CONNECT;
		openConnEvent = BLE_EVENT_LE_CONNECTION_OPENED;
		stopScanCommand = CMD_LE_GAP_END_PROCEDURE;
	}

	OnStartup();

	while (true)
	{
		if(!connected){
			LEDOff(GREENLED);
			LEDBlink(REDLED, 100, 100);
		}else{
			LEDOff(REDLED);
			LEDOn(GREENLED);
		}
		while(!connected){
			BLE_WaitForConnection();
		}
		byte eventData[64] = {0};
		int eventDataBytes;
		int event = BLEGetEvent(eventData, sizeof(eventData), &eventDataBytes);
		if(event == BLE_EVENT_GATT_CHARACTERISTIC_VALUE){
			DEBUG_HEX("EVENT GATT Characteristic Value", eventData, eventDataBytes);
			HostWriteHex(&eventData[7], eventData[6]*8, eventData[6]*2);
			HostWriteChar('\r');
		}else if((event != BLE_NO_MESSAGE)&(event != scanResponseEvent)){
			DEBUG_HEX( "Received Event Data", eventData, eventDataBytes);
            DEBUG_STRING(BLEGetMessageString(event));
            DEBUG_HEX("EVENT Value", &event, 4);
		}
		if(event == connClosedEvent){
			connected = false;
		}
		//Can be edited to allow scanning and transmitting values back to server device
		// Search a transponder
		if (SearchTag(&TagType, &IDBitCnt, ID, sizeof(ID)))
		{
			// Is this transponder new to us?
			if (TagType != LastTagType || IDBitCnt != LastIDBitCnt || !CompBits(ID, 0, LastID, 0, MAXIDBITS))
			{
				// Save this as known ID, before modifying the ID for proper output format
				CopyBits(LastID, 0, ID, 0, MAXIDBITS);
				LastIDBitCnt = IDBitCnt;
				LastTagType = TagType;

				// Yes! Sound a beep
				BeepHigh();
				// Turn off the green LED
				LEDOff(GREENLED);
				// Let the red one blink, start with on-state
				LEDOn(REDLED);
				LEDBlink(REDLED, 500, 500);

				//Build a message that includes the byte string and a carriage return
				int messageBytes = ((IDBitCnt + 7) / 8);
				byte BLEMessage[messageBytes];
				CopyBytes(BLEMessage, ID, messageBytes);


				byte BGAPIPacket[128];
				byte rxData[128];
				int rxDataBytes;
				BGAPIPacket[0] = connHandle; 										//Connection number
				BGAPIPacket[1] = (byte)BLEWriteCharacteristic.CharacteristicHandle;	//First byte of handle
				BGAPIPacket[2] = 0x00;												//Second byte of handle
				BGAPIPacket[3] = (byte)messageBytes;								//Number of data bytes
				CopyBytes(&BGAPIPacket[4], BLEMessage, messageBytes);
				if(!BLETxRxModuleCommand(CMD_GATT_WRITE_CHARACTERISTIC_VALUE_WITHOUT_RESPONSE, BGAPIPacket, messageBytes+4, rxData, &rxDataBytes, sizeof(rxData))){
					DEBUG_STRING("Failed sending notification");
				}


			}
			// Start a timeout of two seconds
			StartTimer(2000);
		}
		if (TestTimer())
		{
			LEDOn(GREENLED);
			LEDOff(REDLED);
			LastTagType = NOTAG;
		}
	}
}
