/******************************************************************************
* ZigBeeLightReceiverEnd - Receiver end device code                           *
*                                                                             *
* based on the Freescale MyApp_Ex05b demo code                                *
*                                                                             *
* Jan Hrbacek, bachelor's thesis, 2008                                        *
******************************************************************************/

#include "802_15_4.h" /* Include everything related to the 802.15.4 interface*/
#include "ToolBox.h"  /* Defines for memcpy, memcmp. */


/* PREPROCESSOR MACROS *******************************************************/

/* Defines the channels to scan. Each bit represents one channel. Use
   0x07FFF800 to scan all 16 802.15.4 channels in the 2.4GHz band. */
#define SCAN_CHANNELS 0x07FFF800

/* Maximum number of outstanding packets */
#define MAX_PENDING_DATA_PACKETS 2

/* Default size of data payload in MCPS-Data.request.
   The length has no real meaning since MCPS-Data.requests
   are always sent in one-size buffers big enough to hold
   a maximum length data frame. */
#define DEFAULT_DATA_LENGTH 3
             
/* Transmit bufer length */
#define RX_BUFFER_LEN 16

/* Do transmit debug data over UART? */
//#define USE_UART   

#ifdef USE_UART
#include "Uart.h"     /* Defines the interface of the demo UART. */
#endif

/* LED settings */
#define LED_P       PTCD
#define LED_DD      PTCDD
#define LED_PE      PTCPE
#define LED_PIN     0x01  

#define LED_ON      LED_P &= ~LED_PIN
#define LED_OFF     LED_P |= LED_PIN
#define LED_TOGGLE  LED_P ^= LED_PIN

/* Output pin */
#define OUTPUT_P    PTDD
#define OUTPUT_DD   PTDDD
#define OUTPUT_PE   PTDPE
#define OUTPUT_PIN  0x04

#define OUTPUT_ON   OUTPUT_P &= ~OUTPUT_PIN
#define OUTPUT_OFF  OUTPUT_P |= OUTPUT_PIN

/* Zero-cross detection */
#define ZC_ENABLE   KBISC |= 0x02
#define ZC_DISABLE  KBISC &= ~0x02
#define ZC_CLEAR    KBISC |= 0x04
#define ZC_TOGGLE   KBISC ^= 0x80

#define TPM_ENABLE  TPM1C2SC |= 0x40
#define TPM_DISABLE TPM1C2SC &= ~0x40
#define TMP_CLEAR   TPM1CNTL = 0x00; TPM1CNTH = 0x00
#define TMP_CLEARF  TPM1C2SC; TPM1C2SC &= ~0x80


/* TYPE DEFINITIONS **********************************************************/
typedef struct appMessage_tag {
  uint8_t  channelId;
  uint8_t  command;
  uint8_t  data;
} appMessage_t;


/* FORWARD FUNCTION DECLARATIONS *********************************************/
void HW_init(void); // Device initialization function declaration
void Addr_init(void);
void handleData(void);
void handleMessage(appMessage_t *pMsg);
void setReceiveData(uint8_t *pData, uint8_t length);
appMessage_t *getReceiveData(void);

uint8_t App_StartScan(uint8_t scanType);
uint8_t App_HandleScanActiveConfirm(nwkMessage_t *pMsg);
uint8_t App_WaitMsg(nwkMessage_t *pMsg, uint8_t msgType);
uint8_t App_SendAssociateRequest(void);
void    App_HandleAssociateConfirm(nwkMessage_t *pMsg);
uint8_t App_HandleMlmeInput(nwkMessage_t *pMsg);
void    App_HandleMcpsInput(mcpsToNwkMessage_t *pMsgIn);


/* VARIABLE DECLARATIONS *****************************************************/

/* All states in the applications state machine */
enum {
  stateInit,
  stateScanActiveStart,
  stateScanActiveWaitConfirm,
  stateAssociate,
  stateAssociateWaitConfirm,
  stateListen,
  stateTerminate
};

/* Error codes */
enum {
  errorNoError,
  errorWrongConfirm,
  errorNotSuccessful,
  errorNoMessage,
  errorAllocFailed,
  errorInvalidParameter,
  errorNoScanResults
};

/* The current state of the applications state machine */
uint8_t state;

/* Information about the PAN we are part of */
panDescriptor_t coordInfo;

/* This is either the short address assigned by the PAN coordinator
   during association, or our own extended MAC address. */
uint8_t myAddress[8];
/* The devices address mode. If 2, then myAddress contains the short
   address assigned by the PAN coordinator. If 3, then myAddress is
   equal to the extended address. */
uint8_t myAddrMode;    

/* Wanted PAN ID will be loaded from aExtendedAddress */
uint8_t panId[2];

/* Data request packet for sending UART input to the coordinator */
nwkToMcpsMessage_t *pPacket;

/* The MSDU handle is a unique data packet identifier */
uint8_t msduHandle;

/* Number of pending data packets */
uint8_t numPendingPackets;

/* Signals that an MLME-Poll request is pending, and that we must wait for 
   the MLME-Poll confirm message before sending the next poll request. */
bool_t waitPollConfirm;

/* Time between MLME-Poll requests */
uint8_t pollInterval;

/* Application input queues */
anchor_t mMlmeNwkInputQueue;
anchor_t mMcpsNwkInputQueue;

/* Receive buffer */
appMessage_t *rxBuffer[RX_BUFFER_LEN];
uint8_t rxBufferHead = 0, rxBufferTail = 0;

/* Output state */
uint8_t outputState = 0;

/* Output compare value */
int32_t outputCmpVal = 0xFFFF;


/* FUNCTION DECLARATIONS *****************************************************/

/* Application Main Loop */
void main(void)
{ 
  /* Pointer for storing the messages from MLME, MCPS, and ASP. */
  void *pMsgIn;
  /* Stores the status code returned by some functions. */
  uint8_t rc;
  /* return value of Mlme_Main() - not used yet */
  uint8_t macStatus;
  
  /* Initialize variables */
  state = stateInit;

  /* Prepare input queues.*/
  MSG_InitQueue(&mMlmeNwkInputQueue);
  MSG_InitQueue(&mMcpsNwkInputQueue);

  /* Execute the application state machine */    
  while(state < stateTerminate)
  {
    /* Preset error to contain the success code */
    rc = errorNoError;
    
    /* Try to get a message from MLME */
    if (MSG_Pending(&mMlmeNwkInputQueue))
      pMsgIn = MSG_DeQueue(&mMlmeNwkInputQueue);
    else
      pMsgIn = NULL;
      
    switch(state)
    {
    case stateInit:
      /* Initialize the UART so that we can print out status messages */
#ifdef USE_UART
      Uart_Init();
#endif
      /* Initialize the 802.15.4 stack */
      Init_802_15_4();      
      /* Initialize the rest of the HW */
      HW_init();    
      /* Initialize addresses */
      Addr_init();
      
      LED_ON;
      
      /* Goto Energy Detection state. */
      state = stateScanActiveStart;
      /* Reset number of pending packets */
      numPendingPackets = 0;
      /* Allow sending a poll request */
      waitPollConfirm = FALSE;

      /* Print a welcome message to the UART */
#ifdef USE_UART
      Uart_Print("The ZigBeeLightReceiverEnd is initialized and ready.\n\n");
#endif
      break;
      
    case stateScanActiveStart:
      /* Start the Active scan, and goto wait for confirm state. */   
#ifdef USE_UART
      Uart_Print("Start scanning for a PAN coordinator\n"); 
#endif
      rc = App_StartScan(gScanModeActive_c);
      if (rc == errorNoError)
      {
        state = stateScanActiveWaitConfirm;
      }
      break;
      
    case stateScanActiveWaitConfirm:
      /* Stay in this state until the Scan confirm message
         arrives, and then goto the associate state. */
         
      /* ALWAYS free the beacon frame contained in the beacon notify indication.*/
      rc = App_WaitMsg(pMsgIn, gNwkBeaconNotifyInd_c);
      if (rc == errorNoError) {
        MSG_Free(((nwkMessage_t *)pMsgIn)->msgData.beaconNotifyInd.pBufferRoot);   
#ifdef USE_UART
        Uart_Print("Received an MLME-Beacon Notify Indication\n");  
#endif
      }
      
      /* Handle the Scan Confirm message. */
      rc = App_WaitMsg(pMsgIn, gNwkScanCnf_c);
      if (rc == errorNoError)
      {
        rc = App_HandleScanActiveConfirm(pMsgIn);
        if (rc == errorNoError)
        {                             
#ifdef USE_UART
          Uart_Print("Found a coordinator with the following properties:\n");
          Uart_Print("----------------------------------------------------");
          Uart_Print("\nAddress...........0x"); Uart_PrintHex(coordInfo.coordAddress, coordInfo.coordAddrMode == gAddrModeShort_c ? 2 : 8, 0);
          Uart_Print("\nPAN ID............0x"); Uart_PrintHex(coordInfo.coordPanId, 2, 0);
          Uart_Print("\nLogical Channel...0x"); Uart_PrintHex(&coordInfo.logicalChannel, 1, 0);
          Uart_Print("\nBeacon Spec.......0x"); Uart_PrintHex(coordInfo.superFrameSpec, 2, 0);
          Uart_Print("\nLink Quality......0x"); Uart_PrintHex(&coordInfo.linkQuality, 1, 0);
          Uart_Print("\n\n");         
#endif

          state = stateAssociate;
        }
        else
        {
          state = stateScanActiveStart;
#ifdef USE_UART
          Uart_Print("Scan did not find a suitable coordinator\n"); 
#endif
        }
      }
      break;

    case stateAssociate:
      /* Associate to the PAN coordinator */   
#ifdef USE_UART
      Uart_Print("Associating to PAN coordinator on channel 0x");
      Uart_PrintHex(&(coordInfo.logicalChannel), 1, gPrtHexNewLine_c); 
#endif
      rc = App_SendAssociateRequest();
      if (rc == errorNoError)
        state = stateAssociateWaitConfirm;
      break; 

    case stateAssociateWaitConfirm:
      /* Stay in this state until the Associate confirm message
         arrives, and then goto the Listen state. */
      rc = App_WaitMsg(pMsgIn, gNwkAssociateCnf_c);
      if (rc == errorNoError)
      {
        App_HandleAssociateConfirm(pMsgIn);
                       
#ifdef USE_UART
        Uart_Print("Successfully associated with the coordinator.\n");
        Uart_Print("We were assigned the short address 0x");
        Uart_PrintHex(myAddress, myAddrMode == gAddrModeShort_c ? 2 : 8, 0);
        Uart_Print("\n\nReady to send and receive data over the UART.\n\n");
#endif    

        LED_OFF;
        
        state = stateListen;
      }
      break; 
      
    case stateListen:
      /* Stay in this state forever. Handles poll confirm etc. */
      rc = App_HandleMlmeInput(pMsgIn);
      
      /* Handle incoming data */
      handleData();
      break;
    }
    
    if (pMsgIn)
      /* ALWAYS free messages from MLME */
      MSG_Free(pMsgIn);

    
    /* If we are associated then check MCPS queue and UART data buffer. */
    if (state == stateListen)
    {
      /* Get input from MCPS. */
      if (MSG_Pending(&mMcpsNwkInputQueue))
      {
        pMsgIn = MSG_DeQueue(&mMcpsNwkInputQueue);
        App_HandleMcpsInput(pMsgIn);
        /* ALWAYS free messages from MCPS */
        MSG_Free(pMsgIn);
      }

      /* Check if the UART buffer has data to be sent. */
      //App_TransmitData();
    }
  
    /* Call the MAC main function. */
    macStatus = Mlme_Main();
  }
}    


/* Hardware initialization */
void HW_init(void)
{
  // LED
  LED_DD |= LED_PIN; 
  LED_PE &= ~LED_PIN;
  LED_P  |= LED_PIN; 
  
  // KBI - zero cross detection
  ZC_DISABLE; // KBIE=0
  KBIPE = 0x80; // KBIPE7=1
  ZC_CLEAR; // KBACK=1
  
  // TPM - prescaler 128
  TPM1SC   = 0x0F;
  TPM1MODL = 0xFF;
  TPM1MODH = 0xFF;
  // software only output compare on channel 2
  TPM1C2SC = 0x10;
  TPM1C2VL = 0xFF;
  TPM1C2VH = 0xFF;
  
  // output pin
  OUTPUT_DD |= OUTPUT_PIN;
  OUTPUT_PE &= ~OUTPUT_PIN;
  OUTPUT_P  |= OUTPUT_PIN;
}


/****************************************************************************** 
* The Addr_init(void) function copies part of the aExtendedAddress to the
* PAN ID
*
******************************************************************************/
void Addr_init(void)
{    
  /* This is our own extended address (MAC address). It cannot be modified. */
  extern const uint8_t aExtendedAddress[8];
  
  panId[0] = aExtendedAddress[2];
  panId[1] = aExtendedAddress[3];
}


/****************************************************************************** 
* The handleData(void) function handles handles incoming data - decides,
* whether the message is destined for this device or not.
*
******************************************************************************/
void handleData(void)
{
  appMessage_t *pMsg;
  
  pMsg = getReceiveData(); // get incoming data from the buffer
  if (pMsg != NULL)
  {
    if (pMsg->channelId == ((myAddress[0] & 0xF0) >> 4))
    {
      // the message is for me, too
      handleMessage(pMsg);
    }
  }
}


/****************************************************************************** 
* The handleMessage(appMessage_t *pMsg) function takes data from the incoming
* message and enumerates new value of the outputCmpVal variable. Then fills
* the TPM1CH2 value register with that value.
*
******************************************************************************/
void handleMessage(appMessage_t *pMsg)
{
  if (pMsg == NULL) return;
  
  // 1250 (max val) = 16M (bus freq.) / 128 (prescaler) / 100 (zero-cross freq.)
  switch (pMsg->command)
  {
    case 0x01: // increase
      outputCmpVal -= pMsg->data * 12; // data - percentage
      ZC_ENABLE;
      break;
    case 0x02: // decrease
      outputCmpVal += pMsg->data * 12;
      ZC_ENABLE;
      break; 
    case 0x03: // switch on
      outputCmpVal = 0;
      OUTPUT_ON;
      ZC_DISABLE;
      TPM_DISABLE;
      break;
    case 0x04: // switch off
      outputCmpVal = 1250;
      OUTPUT_OFF;
      ZC_DISABLE;
      TPM_DISABLE;
      break;
  }
  
  if (outputCmpVal > 1250) outputCmpVal = 1250;
  if (outputCmpVal < 0) outputCmpVal = 0;
  
  TPM1C2VL = (uint8_t) ((outputCmpVal)      & 0xFF);
  TPM1C2VH = (uint8_t) ((outputCmpVal >> 8) & 0xFF);        
}


/****************************************************************************** 
* The KBI_ISR(void) interrupt routine handles zero crossing of the mains
* voltage - enables TPM1CH2 output compare interrupt.
*
******************************************************************************/
interrupt void KBI_ISR(void)
{
  // clear KBF
  ZC_CLEAR;
  // toggle the edge direction
  ZC_TOGGLE;
  
  outputState = 0;
  
  OUTPUT_OFF;
  
  // clear the counter
  TMP_CLEAR;
            
  // enable TMP1CH2 output compare interrupt
  TPM_ENABLE;
}

  
/******************************************************************************
* The TPM1CH2_ISR(void) interrupt routine handles the output compare interrupt
* of the TPM1CH2 - generates an impulse to switch on the power triac.
*                                                                          
******************************************************************************/
interrupt void TPM1CH2_ISR(void)
{  
  uint16_t cmpVal;
  
  // clear CH2F
  TMP_CLEARF;

  if (!outputState)
  {
    // generate an impulse
    cmpVal = (uint16_t) (outputCmpVal + 20);
    if (cmpVal > 1250) cmpVal = 1250;
    
    TPM1C2VL = (uint8_t) ((cmpVal)      & 0xFF);
    TPM1C2VH = (uint8_t) ((cmpVal >> 8) & 0xFF);
  
    outputState = 1;
    
    OUTPUT_ON;
  }
  else
  {
    // disable this interrupt
    TPM_DISABLE;
  
    outputState = 0;
    
    OUTPUT_OFF;
  }
}            


/******************************************************************************
* The App_StartScan(scanType) function will start the scan process of the
* specified type in the MAC. This is accomplished by allocating a MAC message,
* which is then assigned the desired scan parameters and sent to the MLME
* service access point.
* The function may return either of the following values:
*   errorNoError:          The Scan message was sent successfully.
*   errorInvalidParameter: The MLME service access point rejected the
*                          message due to an invalid parameter.
*   errorAllocFailed:      A message buffer could not be allocated.
*
******************************************************************************/
uint8_t App_StartScan(uint8_t scanType)
{
  mlmeMessage_t *pMsg;
  mlmeScanReq_t *pScanReq;
          
#ifdef USE_UART
  Uart_Print("Sending the MLME-Scan Request message to the MAC..."); 
#endif

  /* Allocate a message for the MLME (We should check for NULL). */
  pMsg = MSG_AllocType(mlmeMessage_t);
  if (pMsg != NULL)
  {
    /* This is a MLME-START.req command */
    pMsg->msgType = gMlmeScanReq_c;
    /* Create the Start request message data. */
    pScanReq = &pMsg->msgData.scanReq;
    /* gScanModeED_c, gScanModeActive_c, gScanModePassive_c, or gScanModeOrphan_c */
    pScanReq->scanType = scanType;
    /* ChannelsToScan & 0xFF - LSB, always 0x00 */
    pScanReq->scanChannels[0] = (uint8_t)((SCAN_CHANNELS)     & 0xFF);
    /* ChannelsToScan>>8 & 0xFF  */
    pScanReq->scanChannels[1] = (uint8_t)((SCAN_CHANNELS>>8)  & 0xFF);
    /* ChannelsToScan>>16 & 0xFF  */
    pScanReq->scanChannels[2] = (uint8_t)((SCAN_CHANNELS>>16) & 0xFF);
    /* ChannelsToScan>>24 & 0xFF - MSB */
    pScanReq->scanChannels[3] = (uint8_t)((SCAN_CHANNELS>>24) & 0xFF);
    /* Duration per channel 0-14 (dc). T[sec] = (16*960*((2^dc)+1))/1000000.
       A scan duration of 5 on 16 channels approximately takes 8 secs. */
    pScanReq->scanDuration = 5;
    
    /* Send the Scan request to the MLME. */
    if (MSG_Send(NWK_MLME, pMsg) == gSuccess_c)
    {       
#ifdef USE_UART
      Uart_Print("Done\n");  
#endif
      return errorNoError;
    }
    else
    {      
#ifdef USE_UART
      Uart_Print("Invalid parameter!\n"); 
#endif
      return errorInvalidParameter;
    }
  }
  else
  {
    /* Allocation of a message buffer failed. */  
#ifdef USE_UART
    Uart_Print("Message allocation failed!\n"); 
#endif
    return errorAllocFailed;
  }
}

/******************************************************************************
* The App_HandleScanActiveConfirm(nwkMessage_t *pMsg) function will handle the
* Active Scan confirm message received from the MLME when the Active scan has
* completed. The message contains a list of PAN descriptors. Based on link
* quality inforamtion in the pan descriptors the nearest coordinator is chosen.
* The corresponding pan descriptor is stored in the global variable coordInfo. 
*
* The function may return either of the following values:
*   errorNoError:       A suitable pan descriptor was found.
*   errorNoScanResults: No scan results were present in the confirm message.
*
******************************************************************************/
uint8_t App_HandleScanActiveConfirm(nwkMessage_t *pMsg)
{
  uint8_t panDescListSize   = pMsg->msgData.scanCnf.resultListSize;
  panDescriptor_t *pPanDesc = pMsg->msgData.scanCnf.resList.pPanDescriptorList;
  uint8_t rc = errorNoScanResults;
 
  /* Check if the scan resulted in any coordinator responses. */
  if (panDescListSize != 0)
  {
    /* Initialize link quality to very poor. */
    uint8_t i, bestLinkQuality = 0;
    
    /* Check all PAN descriptors. */
    for(i=0; i<panDescListSize; i++, pPanDesc++)
    {
      /* Only attempt to associate if the coordinator
         accepts associations and is non-beacon. */
      if ((pPanDesc->superFrameSpec[1] & gSuperFrameSpecMsbAssocPermit_c) && 
          ((pPanDesc->superFrameSpec[0] & gSuperFrameSpecLsbBO_c) == 0x0F))
      {
        /* Find the nearest coordinator using the link quality measure. */
        if (pPanDesc->coordPanId[0] == panId[0] && pPanDesc->coordPanId[1] == panId[1] &&
           pPanDesc->linkQuality > bestLinkQuality)
        {
          /* Save the information of the coordinator candidate. If we
             find a better candiate, the information will be replaced. */
          memcpy(&coordInfo, pPanDesc, sizeof(panDescriptor_t));
          bestLinkQuality = pPanDesc->linkQuality;
          rc = errorNoError;
        }
      }
    }
  }
  /* ALWAYS free the PAN descriptor list */
  MSG_Free(pMsg->msgData.scanCnf.resList.pPanDescriptorList);
  
  return rc;
}


/******************************************************************************
* The App_SendAssociateRequest(void) will create an Associate Request message
* and send it to the coordinator it wishes to associate to. The function uses
* information gained about the coordinator during the scan procedure.
*
* The function may return either of the following values:
*   errorNoError:          The Associate Request message was sent successfully.
*   errorInvalidParameter: The MLME service access point rejected the
*                          message due to an invalid parameter.
*   errorAllocFailed:      A message buffer could not be allocated.
*
******************************************************************************/
uint8_t App_SendAssociateRequest(void)
{
  mlmeMessage_t *pMsg;
  mlmeAssociateReq_t *pAssocReq;
         
#ifdef USE_UART
  Uart_Print("Sending the MLME-Associate Request message to the MAC...");
#endif
  
  /* Allocate a message for the MLME message. */
  pMsg = MSG_AllocType(mlmeMessage_t);
  if (pMsg != NULL)
  {
    /* This is a MLME-ASSOCIATE.req command. */
    pMsg->msgType = gMlmeAssociateReq_c;
    
    /* Create the Associate request message data. */
    pAssocReq = &pMsg->msgData.associateReq;
 
    /* Use the coordinator info we got from the Active Scan. */
    memcpy(pAssocReq->coordAddress, coordInfo.coordAddress, 8);
    memcpy(pAssocReq->coordPanId,   coordInfo.coordPanId, 2);
    pAssocReq->coordAddrMode      = coordInfo.coordAddrMode;
    pAssocReq->logicalChannel     = coordInfo.logicalChannel;
    pAssocReq->securityEnable     = FALSE;
    /* We want the coordinator to assign a short address to us. */
    pAssocReq->capabilityInfo     = gCapInfoAllocAddr_c;
      
    /* Send the Associate Request to the MLME. */
    if (MSG_Send(NWK_MLME, pMsg) == gSuccess_c)
    {                 
#ifdef USE_UART
      Uart_Print("Done\n");   
#endif
      return errorNoError;
    }
    else
    {
      /* One or more parameters in the message were invalid. */  
#ifdef USE_UART
      Uart_Print("Invalid parameter!\n");
#endif
      return errorInvalidParameter;
    }
  }
  else
  {
    /* Allocation of a message buffer failed. */ 
#ifdef USE_UART
    Uart_Print("Message allocation failed!\n");     
#endif
    return errorAllocFailed;
  }
}

/******************************************************************************
* The App_HandleAssociateConfirm(nwkMessage_t *pMsg) function will handle the
* Associate confirm message received from the MLME when the Association
* procedure has completed. The message contains the short address that the
* coordinator has assigned to us. This address is 0xfffe if we did not specify
* the gCapInfoAllocAddr_c flag in the capability info field of the Associate
* request. The address and address mode are saved in global variables. They
* will be used in the next demo application when sending data.
*
******************************************************************************/
void App_HandleAssociateConfirm(nwkMessage_t *pMsgIn)
{   
  mlmeMessage_t *pMsg;
  
  /* This is our own extended address (MAC address). It cannot be modified. */
  extern const uint8_t aExtendedAddress[8];
  
  /* If the coordinator assigns a short address of 0xfffe then,
     that means we must use our own extended address in all
     communications with the coordinator. Otherwise, we use
     the short address assigned to us. */
  if ((pMsgIn->msgData.associateCnf.assocShortAddress[0] >= 0xFE) && 
      (pMsgIn->msgData.associateCnf.assocShortAddress[1] == 0xFF))
  {
    myAddrMode = gAddrModeLong_c;
    memcpy(myAddress, (void *) aExtendedAddress, 8);
  }
  else
  {
    myAddrMode = gAddrModeShort_c;
    memcpy(myAddress, pMsgIn->msgData.associateCnf.assocShortAddress, 2);
  }
  
  /* Set RxOnWhenIdle = 1 */
  pMsg = MSG_AllocType(mlmeMessage_t);
  if(pMsg != NULL)
  {
    uint8_t value;   
    
    pMsg->msgType = gMlmeSetReq_c;
    pMsg->msgData.setReq.pibAttribute = gMacPibRxOnWhenIdle_c;
    value = 0x01;
    pMsg->msgData.setReq.pibAttributeValue = &value;
    NR MSG_Send(NWK_MLME, pMsg);
  }  
  else
  {
    /* Allocation of a message buffer failed. */    
#ifdef USE_UART
    Uart_Print("Message allocation failed!\n");     
#endif
  }
}


/******************************************************************************
* The App_HandleMlmeInput(nwkMessage_t *pMsg) function will handle various
* messages from the MLME, e.g. poll confirm.
*
* The function may return either of the following values:
*   errorNoError:   The message was processed.
*   errorNoMessage: The message pointer is NULL.
******************************************************************************/
uint8_t App_HandleMlmeInput(nwkMessage_t *pMsg)
{
  uint8_t ret = errorNoError;
  
  if (pMsg == NULL)
    return errorNoMessage;
  
  LED_ON;
  
  /* Handle the incoming message. The type determines the sort of processing.*/
  switch(pMsg->msgType) {
  case gNwkPollCnf_c:
    if (pMsg->msgData.pollCnf.status != gSuccess_c)
    {
      /* The Poll Confirm status was not successful. Usually this happens if
         no data was available at the coordinator. In this case we start
         polling at a lower rate to conserve power. */
      //pollInterval = POLL_INTERVAL_SLOW;
      
      /* If we get to this point, then no data was available, and we
         allow a new poll request. Otherwise, we wait for the data
         indication before allowing the next poll request. */
      //waitPollConfirm = FALSE;
    }
    
    break;
  }
  
  LED_OFF;
  
  return ret;
}

/******************************************************************************
* The App_HandleMcpsInput(mcpsToNwkMessage_t *pMsgIn) function will handle 
* messages from the MCPS, e.g. Data Confirm, and Data Indication.
*
******************************************************************************/
void App_HandleMcpsInput(mcpsToNwkMessage_t *pMsgIn)
{
  LED_ON;

  switch(pMsgIn->msgType)
  {
    /* The MCPS-Data confirm is sent by the MAC to the network 
       or application layer when data has been sent. */
  case gMcpsDataCnf_c:
    if (numPendingPackets)
      numPendingPackets--;
    break;

  case gMcpsDataInd_c:
    /* The MCPS-Data indication is sent by the MAC to the network 
       or application layer when data has been received. We simply 
       copy the received data to the ring buffer. */
    setReceiveData(pMsgIn->msgData.dataInd.msdu, pMsgIn->msgData.dataInd.msduLength);
    break;
  }
  
  LED_OFF;
}  


/* Helper funcs, inspired by Uart lib */ 
void setReceiveData(uint8_t *pData, uint8_t length)
{
  appMessage_t *pMsg;
  
  if (length < 3)
  {
    return; // message too short
  }
     
  pMsg = MSG_AllocType(appMessage_t);
  if (pMsg != NULL)
  {
    // copy the message to the buffer
    pMsg->channelId = *(pData++);
    pMsg->command = *(pData++);
    pMsg->data = *(pData++);
         
  	rxBuffer[rxBufferHead] = pMsg;
    rxBufferHead = (rxBufferHead + 1) & (RX_BUFFER_LEN - 1);
  }
  else
  {
    // allocation of the message buffer failed    
#ifdef USE_UART
    Uart_Print("Message allocation failed!\n");     
#endif
  }
}

     
appMessage_t *getReceiveData(void)
{
  appMessage_t *pMsg;
            
  if (rxBufferHead == rxBufferTail)
  {
    return NULL; // no messages in buffer, return NULL
  }
    
	pMsg = rxBuffer[rxBufferTail];	// read the message pointer
  rxBufferTail = (rxBufferTail + 1) & (RX_BUFFER_LEN - 1);

	return pMsg;
}


/******************************************************************************
* The App_WaitMsg(nwkMessage_t *pMsg, uint8_t msgType) function does not, as
* the name implies, wait for a message, thus blocking the execution of the
* state machine. Instead the function analyzes the supplied message to 
* determine whether or not the message is of the expected type.
* The function may return either of the following values:
*   errorNoError: The message was of the expected type.
*   errorNoMessage: The message pointer is NULL.
*   errorWrongConfirm: The message is not of the expected type.
*
******************************************************************************/
uint8_t App_WaitMsg(nwkMessage_t *pMsg, uint8_t msgType)
{
  /* Do we have a message? If not, the exit with error code */
  if (pMsg == NULL)
    return errorNoMessage;

  /* Is it the expected message type? If not then exit with error code */
  if (pMsg->msgType != msgType)
    return errorWrongConfirm;

  /* Found the expected message. Return with success code */
  return errorNoError;
}


/******************************************************************************
* The following functions are called by the MAC to put messages into the
* Application's queue. They need to be defined even if they are not used
* in order to avoid linker errors.
******************************************************************************/

uint8_t MLME_NWK_SapHandler(nwkMessage_t *pMsg)
{
  /* Put the incoming MLME message in the applications input queue. */
  MSG_Queue(&mMlmeNwkInputQueue, pMsg);
  return gSuccess_c;
}

uint8_t MCPS_NWK_SapHandler(mcpsToNwkMessage_t *pMsg)
{
  /* Put the incoming MCPS message in the applications input queue. */
  MSG_Queue(&mMcpsNwkInputQueue, pMsg);
  return gSuccess_c;
}

uint8_t ASP_APP_SapHandler(aspToAppMsg_t *pMsg)
{
  /* If the message is not handled anywhere it must be freed. */
  MSG_Free(pMsg);
  return gSuccess_c;
}