apsRxState = APS_RXSTATE_IDLE;
			  break;
		  }

		  //parse this and see what to do with it.
		  if (APS_IS_CMD(a_aps_rx_data.aps_fcf)) {
			  //currently don't handle CMD packets. Discard.
			  DEBUG_STRING(DBG_INFO,"APS: Received APS CMD packet, discarding.\n");
			  //NWK,MAC resource already free; need to free the MEM resource
			  MemFree(a_aps_rx_data.orgpkt.data);
			  apsRxState = APS_RXSTATE_IDLE;
			  break;
		  }
		  //have a DATA or ACK packet, lets do something.
		  //parse it to figure out what it is.
		  apsParseHdr(a_aps_rx_data.orgpkt.data + a_aps_rx_data.apsOffset);
		  if (APS_IS_ACK(a_aps_rx_data.aps_fcf)) {
			  if (!aps_pib.flags.bits.ackPending) {
				  //not currently expecting an ACK so discard
				  DEBUG_STRING(DBG_INFO,"APS: Received unexpected ACK, discarding.\n");
			  }else {
				  //lets see if this is our ack.
				  if (apsCheckAck()) {
					  DEBUG_STRING(DBG_INFO,"APS: Received APS ack\n");
					  //this is our ACK, clear the ackPending bit
					  aps_pib.flags.bits.ackPending = 0;
				  } else {
					  DEBUG_STRING(DBG_INFO,"APS: Received ACK, did not match expected.\n");
				  }

			  }
			  //NWK,MAC resource already free; need to free the MEM resource
			  MemFree(a_aps_rx_data.orgpkt.data);
			  apsRxState = APS_RXSTATE_IDLE;
			  break;
		  }

		  //check for indirect message
		  if (!a_aps_rx_data.flags.bits.dstEP){
			  //no dest endpoint, must be an indirect message.
#ifdef LRWPAN_COORDINATOR
			  //see if we have room to buffer this packet
			  if (apsRxBuffFull()) {
				  //no room, discard this buffer
				  DEBUG_STRING(DBG_INFO,"APS: No room for indirect packet storage, discarding.\n");
				  MemFree(a_aps_rx_data.orgpkt.data);
				  apsRxState = APS_RXSTATE_IDLE;
				  break;
			  }
			  //copy this packet to the APS RX buffer
			  apsRxBuffAdd (&a_aps_rx_data);
			  //first, check to see if an ACK is requested.
			  if (APS_GET_FRM_ACKREQ(a_aps_rx_data.aps_fcf)){
				  //ack request send has to be done in the main FSM
				  aps_pib.flags.bits.ackSendPending = 1;
				  //will have both ackPending and indirectPending set,
				  //the ack pending will be handled first.
			  }

			  //I am the coordinator, must resolve binding
			  aps_pib.flags.bits.indirectPending = 1;
			  apsRxState = APS_RXSTATE_RESOLVE_INDIRECT;
			  goto apsRxFSM_start;
#else
			  //this is an ERROR. Non-coordinator has an unresolved indirect packet.
			  DEBUG_STRING(DBG_INFO,"APS: Non-Coord Received indirect packet, should not happen, check code.\n");
			  MemFree(a_aps_rx_data.orgpkt.data);
			  apsRxState = APS_RXSTATE_IDLE;
			  break;
#endif
		  }


		  //check for DST endpoint of 0, special endpoint
		  if (a_aps_rx_data.dstEP == 0) {
			  //not a user endpoint, handle this
			  DEBUG_STRING(DBG_INFO,"APS: Received ZEP Request.\n");
			  callback_status = zepHandleRxPacket();
			  goto apsRxFSM_freepkt;
		  }

		  //at this point, we have a user endpoint. Check profile
		  if (a_aps_rx_data.profile != LRWPAN_APP_PROFILE){
			  //reject, not our profile
			  DEBUG_STRING(DBG_INFO,"APS: Received data packet with wrong profile, rejecting.\n");
			  MemFree(a_aps_rx_data.orgpkt.data);
			  apsRxState = APS_RXSTATE_IDLE;
			  break;
		  }

		  //accept this packet, do Usr Callback
		  callback_status = usrRxPacketCallback();

apsRxFSM_freepkt:
		  //finished, free the space
		  MemFree(a_aps_rx_data.orgpkt.data);

		  //see if an ACK is requested. Only send back the ack if the callback status
		  //returned as success!
		  if (APS_GET_FRM_ACKREQ(a_aps_rx_data.aps_fcf) && callback_status == LRWPAN_STATUS_SUCCESS) {
			  //ack request send has to be done in the main FSM
			  aps_pib.flags.bits.ackSendPending = 1;
			  apsRxState = APS_RXSTATE_ACK_SEND_WAIT;
			  break;
		  }

		  apsRxState = APS_RXSTATE_IDLE;
		  break;


#ifdef LRWPAN_COORDINATOR
  case APS_RXSTATE_RESOLVE_INDIRECT:
	  if (aps_pib.flags.bits.indirectPending) break;
	  //the main FSM will free up the memory associated with the RX packet

	  apsRxState = APS_RXSTATE_IDLE;
	  break;
#endif

  case APS_RXSTATE_ACK_SEND_WAIT:
	  if (aps_pib.flags.bits.ackSendPending) break;  //waiting for ACK to be sent
	  //main FSM is finished, can now proceed with new RX
	  apsRxState = APS_RXSTATE_IDLE;
	  break;

  default:
	  break;
	}



}

//see if this matches expected ack.
static BOOL apsCheckAck(void){
	if (a_aps_rx_data.flags.bits.dstEP && a_aps_rx_data.flags.bits.srcEP) {
		//both are present.
		if ((a_aps_rx_data.srcEP == a_aps_tx_data.dstEP) &&
			(a_aps_rx_data.dstEP == a_aps_tx_data.srcEP)) {
				//this is our ack
				return TRUE;
			}
	} else if (a_aps_rx_data.flags.bits.dstEP) {
		//indirect, check just the EP
		if ((a_aps_rx_data.dstEP == a_aps_tx_data.srcEP) ) {
			return TRUE;
		}
	} else if (a_aps_rx_data.flags.bits.srcEP) {
		//indirect, check just the EP
		if ((a_aps_rx_data.srcEP == a_aps_tx_data.dstEP) ) {
			return TRUE;
		}

	}
	//if we reach here, must not be our expected ack
	return(FALSE);
}

//Callback from NWK Layer
//Returns TRUE if aps is still busy with last RX packet.

BOOL apsRxBusy(void){
	return(apsRxState != APS_RXSTATE_IDLE);
}

void apsRxHandoff(void){
	a_aps_rx_data.orgpkt.data = a_nwk_rx_data.orgpkt.data;
	a_aps_rx_data.orgpkt.rssi = a_nwk_rx_data.orgpkt.rssi;
	a_aps_rx_data.apsOffset = a_nwk_rx_data.nwkOffset+8;
	a_aps_rx_data.srcSADDR = a_nwk_rx_data.srcSADDR;
	a_aps_rx_data.aps_fcf = *(a_aps_rx_data.orgpkt.data + a_aps_rx_data.apsOffset);//get first byte
	a_aps_rx_data.flags.val = 0;
	apsRxState = APS_RXSTATE_START;

}

//parse the APS header, also the AF header as well
static void apsParseHdr(BYTE *ptr){
	BYTE len;

	//get the aps fcf byte
	a_aps_rx_data.aps_fcf = *ptr;
	ptr++;

	len = 1;

	if (!((APS_GET_FRM_DLVRMODE(a_aps_rx_data.aps_fcf) == APS_FRM_DLVRMODE_INDIRECT) &&
		(APS_GET_FRM_INDIRECT_SUBMODE(a_aps_rx_data.aps_fcf)))) {
			//have a destination EP
			a_aps_rx_data.flags.bits.dstEP = 1;
			a_aps_rx_data.dstEP = *ptr;
			ptr++;
			len++;

		}
		//get the cluster ID
		if (APS_GET_FRM_TYPE(a_aps_rx_data.aps_fcf) == APS_FRM_TYPE_DATA){
			a_aps_rx_data.cluster = *ptr;
			ptr++;
		}

		if ((APS_GET_FRM_TYPE(a_aps_rx_data.aps_fcf) == APS_FRM_TYPE_DATA) ||
			(APS_GET_FRM_TYPE(a_aps_rx_data.aps_fcf) == APS_FRM_TYPE_ACK)
			){
				//get the profile ID
				a_aps_rx_data.profile = *ptr;
				ptr++;
				a_aps_rx_data.profile += (((UINT16)*ptr) << 8);
				ptr++;
			}

			len = len +3;

			//get the SRC EP
			if (!((APS_GET_FRM_DLVRMODE(a_aps_rx_data.aps_fcf) == APS_FRM_DLVRMODE_INDIRECT) &&
				(!APS_GET_FRM_INDIRECT_SUBMODE(a_aps_rx_data.aps_fcf)))) {
					//have a SRC EP
					a_aps_rx_data.flags.bits.srcEP = 1;
					a_aps_rx_data.srcEP = *ptr;
					ptr++;
					len++;
				}

				//parse AF frame, assume MSG type
				a_aps_rx_data.afOffset = len;

				//save the af_fcf byte
				a_aps_rx_data.af_fcf = *ptr;
				ptr++;

				//get the transaction number
				a_aps_rx_data.tsn = *ptr;
				ptr++;

				//get the length of the payload
				a_aps_rx_data.usrPlen = *ptr;
				ptr++;
				//save the pointer to the payload
				a_aps_rx_data.usrPload = ptr;

}

//this does not actually format the ACK, just sets
// the aps_tx fields correctly
static void apsFormatAck(void){

	a_aps_tx_data.usrPlen = 0;
	a_aps_tx_data.usrPload = NULL;
	a_aps_tx_data.aps_fcf = APS_FRM_TYPE_ACK;
	a_aps_tx_data.srcSADDR = macGetShortAddr();
	a_aps_tx_data.dstSADDR = a_aps_rx_data.srcSADDR;
	a_aps_tx_data.flags.val = 0;


	DEBUG_STRING(DBG_INFO,"APS:Formatted ack\n");
	if (a_aps_rx_data.flags.bits.dstEP && a_aps_rx_data.flags.bits.srcEP){
		//both are present, so must be direct delivery mode
		a_aps_tx_data.aps_fcf |= APS_FRM_DLVRMODE_NORMAL;
		a_aps_tx_data.dstEP = a_aps_rx_data.srcEP;
		a_aps_tx_data.srcEP = a_aps_rx_data.dstEP;
		a_aps_tx_data.cluster = a_aps_rx_data.cluster;
	} else if (a_aps_rx_data.flags.bits.dstEP){
		//indirect packet
		//only dstEP is present, so this ack must be going back to the COORD
		a_aps_tx_data.aps_fcf |= APS_FRM_DLVRMODE_INDIRECT;
		a_aps_tx_data.srcEP = a_aps_rx_data.dstEP;
		//set the submode bit indicating only src EP will be present
		a_aps_tx_data.aps_fcf |=APS_FRM_INDIRECT_SUBMODE_MASK;
	} else if (a_aps_rx_data.flags.bits.srcEP){
		//indirect packet
		//only srcEP is present, so this ack must be going from COORD to src
		a_aps_tx_data.aps_fcf |= APS_FRM_DLVRMODE_INDIRECT;
		a_aps_tx_data.dstEP = a_aps_rx_data.srcEP;
	}

}

//handles retries for APS packets that require ACKs
static void apsTxFSM(void) {
	if(!apsTXIdle()) {
		//we are not idle
		if (nwkIdle()) {
			//cannot check anything until NWK is idle
			if (a_nwk_service.status != LRWPAN_STATUS_SUCCESS) {
				//don't bother waiting for ACK, TX did not start correctly
				aps_pib.flags.bits.ackPending = 0;
				apsSetTxIdle();  //mark TX as idle
				apsTxFSM_status = a_nwk_service.status; //return status
			} else if (!aps_pib.flags.bits.ackPending) {
				//either no ACK requested or ACK has been received
				apsSetTxIdle();  //finished successfully, mark as idle
				apsTxFSM_status = LRWPAN_STATUS_SUCCESS;
			}
			//check timeout
			else if (halMACTimerNowDelta(aps_pib.tx_start_time)> aps_pib.apscAckWaitDuration)
			{
				//first, check the apsAckWaitMultiplier
				if(aps_pib.apsAckWaitMultiplierCntr) aps_pib.apsAckWaitMultiplierCntr--;  //this is based on number of hops for the apsAck
				if (aps_pib.apsAckWaitMultiplierCntr) {
					//reset the timer.
					aps_pib.tx_start_time = halGetMACTimer();
				}else {
					DEBUG_STRING(1,"APS: TX retry\n");
					// ACK timeout
					aps_pib.currentAckRetries--;
					if (!aps_pib.currentAckRetries) {
						//retries are zero. We have failed.
						apsSetTxIdle();
						DEBUG_STRING(1,"APS TX Retry exceeded\n");
					} else {
						//retry...
						//must reset the len, frm pointers to the beginning of
						// the formatted aps header before retry
						phy_pib.currentTxFlen = a_aps_tx_data.aps_flen;
						phy_pib.currentTxFrm = a_aps_tx_data.aps_ptr;
						aps_pib.tx_start_time = halGetMACTimer();  //reset the timer
						aps_pib.apsAckWaitMultiplierCntr = aps_pib.apsAckWaitMultiplier;
						apsTxData(TRUE);  //reuse the last packet.
					}
				}
			}

		}

	}

}

#ifdef LRWPAN_COORDINATOR

void apsRxBuffInit(void);
BOOL apsRxBuffFull(void);
BOOL apsRxBuffEmpty(void);
APS_RX_DATA *apsGetRxPacket(void);
void apsFreeRxPacket(BOOL freemem);
void apsRxBuffAdd (APS_RX_DATA *ptr);

//copies data into RX buffer
void apsRxBuffAdd (APS_RX_DATA *ptr){
	if (aps_pib.rxCnt == APS_RXBUFF_SIZE) {
		DEBUG_STRING(DBG_ERR,"APS:Trying to add to full buffer in apsRxBuffAdd\n");
		return;
	}
	halUtilMemCopy((BYTE *)&aps_pib.rxBuff[aps_pib.rxHead], (BYTE *)ptr, sizeof(APS_RX_DATA));
	aps_pib.rxCnt++;
	aps_pib.rxHead++; //head points to next free location
	//wrap index
	if (aps_pib.rxHead == APS_RXBUFF_SIZE) aps_pib.rxHead = 0;
}



void apsRxBuffInit(void){
	aps_pib.rxCnt = 0;
	aps_pib.rxTail = 0;
	aps_pib.rxHead = 0;
}

BOOL apsRxBuffFull(void){
	return(aps_pib.rxCnt == APS_RXBUFF_SIZE);
}


BOOL apsRxBuffEmpty(void){
	return(aps_pib.rxCnt == 0);
}

//this does NOT remove the packet from the buffer
APS_RX_DATA *apsGetRxPacket(void) {
	return(&aps_pib.rxBuff[aps_pib.rxTail]);
}

//frees the first packet in the buffer.
void apsFreeRxPacket(BOOL freemem) {
	if (aps_pib.rxCnt == 0) {
		DEBUG_STRING(DBG_ERR,"APS:Trying to free empty buffer in apsFreeRxPacket\n");
		return;
	}
	if (freemem)MemFree(aps_pib.rxBuff[aps_pib.rxTail].orgpkt.data);
	aps_pib.rxCnt--;
	aps_pib.rxTail++;
	if (aps_pib.rxTail == APS_RXBUFF_SIZE) aps_pib.rxTail = 0;

}
#endif