/******************************************************************************
* ZigBeeLightReceiverCoord - Coordinator device code                          *
*                                                                             *
* based on the Freescale MyApp_Ex06a 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 4

/* 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 bufers length */
#define TX_BUFFER_LEN 16
#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);
uint8_t getTransmitData(uint8_t *pBuffer);
void setTransmitData(appMessage_t *msg);  
void setReceiveData(uint8_t *pData, uint8_t length);
appMessage_t *getReceiveData(void);

uint8_t App_StartScan(uint8_t scanType);
void    App_HandleScanEdConfirm(nwkMessage_t *pMsg);
uint8_t App_StartCoordinator(void);
uint8_t App_HandleMlmeInput(nwkMessage_t *pMsg);
uint8_t App_SendAssociateResponse(nwkMessage_t *pMsgIn);
void    App_HandleMcpsInput(mcpsToNwkMessage_t *pMsgIn);
void    App_TransmitData(void);
uint8_t App_WaitMsg(nwkMessage_t *pMsg, uint8_t msgType);

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

/* The various states of the application state machine. */
enum {
  stateInit,
  stateScanEdStart,
  stateScanEdWaitConfirm,
  stateStartCoordinator,
  stateStartCoordinatorWaitConfirm,
  stateListen,
  stateTerminate
};

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

/* Device types */
enum 
{
  devRecEnd   = 1,
  devTrans    = 2,
  devRecCoord = 3
};
         
/* Maps of associated receivers (by lightning channel number) */
uint16_t associatedRecs[16];

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

/* The status parameter of the latest confirm message from the MLME */
uint8_t confirmStatus;

/* The current logical channel (frequency band) */
uint8_t logicalChannel;

/* The coordinators short address will be loaded from aExtendedAddress */
uint8_t shortAddress[2];

/* 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;

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

/* Transmit buffer */
appMessage_t *txBuffer[TX_BUFFER_LEN];
uint8_t txBufferHead = 0, txBufferTail = 0;

/* 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 */
  void *pMsgIn;
  /* Stores the error/success code returned by some functions. */
  uint8_t ret;
  /* 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 return code to contain the success code */
    ret = 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 = stateScanEdStart;
 
      /* Print a welcome message to the UART */
#ifdef USE_UART
      Uart_Print("The ZigBeeLightReceiverCoord is initialized and ready.\n\n");
#endif
      break;
      
    case stateScanEdStart:
      /* Start the Energy Detection scan, and goto wait for confirm state. */  
#ifdef USE_UART
      Uart_Print("Initiating the Energy Detection Scan\n");    
#endif
      ret = App_StartScan(gScanModeED_c);
      if (ret == errorNoError)
      {
        state = stateScanEdWaitConfirm;
      }
      break;
      
    case stateScanEdWaitConfirm:
      /* Stay in this state until the MLME Scan confirm message arrives,
         and has been processed. Then goto Start Coordinator state. */
      ret = App_WaitMsg(pMsgIn, gNwkScanCnf_c);
      if (ret == errorNoError)
      {
        /* Process the ED scan confirm. The logical
           channel is selected by this function. */
        App_HandleScanEdConfirm(pMsgIn);
        state = stateStartCoordinator;
      }
      break;

    case stateStartCoordinator:
      /* Start up as a PAN Coordinator on the selected channel. */  
#ifdef USE_UART
      Uart_Print("\nStarting as PAN coordinator on channel 0x");
      Uart_PrintHex(&logicalChannel, 1, FALSE);
      Uart_Print("\n");
#endif
      ret = App_StartCoordinator();
      if (ret == errorNoError)
      {
        /* If the Start request was sent successfully to
           the MLME, then goto Wait for confirm state. */
        state = stateStartCoordinatorWaitConfirm;
      }
      break; 

    case stateStartCoordinatorWaitConfirm:
      /* Stay in this state until the Start confirm message
         arrives, and then goto the Listen state. */
      ret = App_WaitMsg(pMsgIn, gNwkStartCnf_c);
      if (ret == errorNoError)
      {                                      
#ifdef USE_UART
        Uart_Print("Started the coordinator with PAN ID 0x");
        Uart_PrintHex((uint8_t *)panId, 2, 0);
        Uart_Print(", and short address 0x");
        Uart_PrintHex((uint8_t *)shortAddress, 2, 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 associate, disassociate etc. */
      ret = App_HandleMlmeInput(pMsgIn);
      
      /* Handle incoming data */
      handleData();
      break;
    }
    
    if (pMsgIn)
    {
      /* Messages from the MLME must always be freed. */
      MSG_Free(pMsgIn);
    }

    /* If we are associated then check MCPS queue and UART data buffer. */
    if (state == stateListen)
    {
      /* Check for input from MCPS (data related)*/
      if (MSG_Pending(&mMcpsNwkInputQueue))
      {
        /* Get the message from MCPS */
        pMsgIn = MSG_DeQueue(&mMcpsNwkInputQueue);
        /* Process the message */
        App_HandleMcpsInput(pMsgIn);
        /* Messages from the MCPS must always be freed. */
        MSG_Free(pMsgIn);
      }

      /* Check if the UART buffer has data to be sent to the device. */
      App_TransmitData();
    }
    
    /* Call the MAC main function continuously. */
    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 parts of the aExtendedAddress to the
* PAN ID and short address
*
******************************************************************************/
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];
                                 
  shortAddress[0] = aExtendedAddress[0];
  shortAddress[1] = aExtendedAddress[1];
}


/****************************************************************************** 
* The handleData(void) function handles handles incoming data - decides,
* whether the message is destined for this device or not. It also hands the
* message over to the transmit buffer to be transmitted to the end devices.
*
******************************************************************************/
void handleData(void)
{
  appMessage_t *pMsg;
  
  pMsg = getReceiveData(); // get incoming data from the buffer
  if (pMsg != NULL)
  {
    if (pMsg->channelId == ((shortAddress[0] & 0xF0) >> 4))
    {
      // the message is for me, too
      handleMessage(pMsg);
    }
    
    // hand over to the tx buffer
    setTransmitData(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_HandleScanEdConfirm(nwkMessage_t *pMsg) function will handle the
* ED scan confirm message received from the MLME when the ED scan has completed.
* The message contains the ED scan result list. This function will search the
* list in order to select the logical channel with the least energy. The
* selected channel is stored in the global variable called 'logicalChannel'.
*
******************************************************************************/
void App_HandleScanEdConfirm(nwkMessage_t *pMsg)
{  
  uint8_t n, minEnergy;
  uint8_t *pEdList;
            
#ifdef USE_UART
  Uart_Print("Recevied the MLME-Scan Confirm message from the MAC\n");       
#endif
    
  /* Get a pointer to the energy detect results */
  pEdList = pMsg->msgData.scanCnf.resList.pEnergyDetectList;
  
  /* Set the minimum energy to a large value */
  minEnergy = 0xFF;

  /* Select default channel */
  logicalChannel = 11;
 
  /* Search for the channel with least energy */
  for(n=0; n<16; n++)
  {
    if (pEdList[n] < minEnergy)
    {
      minEnergy = pEdList[n];
      /* Channel numbering is 11 to 26 both inclusive */
      logicalChannel = n + 11; 
    }
  }
                        
#ifdef USE_UART
  /* Print out the result of the ED scan */
  Uart_Print("ED scan returned the following results:\n  [");
  Uart_PrintHex(pEdList, 16, gPrtHexBigEndian_c | gPrtHexCommas_c);
  Uart_Print("]\n\n");
  
  /* Print out the selected logical channel */
  Uart_Print("Based on the ED scan the logical channel 0x");
  Uart_PrintHex(&logicalChannel, 1, 0);
  Uart_Print(" was selected\n");   
#endif
  
  /* The list of detected energies must be freed. */
  MSG_Free(pEdList);
}

/******************************************************************************
* 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 MAC PIB attributes "macShortAddress", and 
* "macAssociatePermit" are modified.
*
* 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_StartCoordinator(void)
{
  /* Message for the MLME will be allocated and attached to this pointer */
  mlmeMessage_t *pMsg;
            
#ifdef USE_UART
  Uart_Print("Sending the MLME-Start 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)
  {
    /* Pointer which is used for easy access inside the allocated message */
    mlmeStartReq_t *pStartReq;
    /* Return value from MSG_send - used for avoiding compiler warnings */
    uint8_t ret;
    /* Boolean value that will be written to the MAC PIB */
    uint8_t boolFlag;
    
    /* Set-up MAC PIB attributes. Please note that Set, Get,
       and Reset messages are not freed by the MLME. */
    
    /* We must always set the short address to something
       else than 0xFFFF before starting a PAN. */
    pMsg->msgType = gMlmeSetReq_c;
    pMsg->msgData.setReq.pibAttribute = gMacPibShortAddress_c;
    pMsg->msgData.setReq.pibAttributeValue = (uint8_t *) shortAddress;
    ret = MSG_Send(NWK_MLME, pMsg);
    
    /* We must set the Association Permit flag to TRUE 
       in order to allow devices to associate to us. */
    pMsg->msgType = gMlmeSetReq_c;
    pMsg->msgData.setReq.pibAttribute = gMacPibAssociationPermit_c;
    boolFlag = TRUE;
    pMsg->msgData.setReq.pibAttributeValue = &boolFlag;
    ret = MSG_Send(NWK_MLME, pMsg);
    
    /* This is a MLME-START.req command */
    pMsg->msgType = gMlmeStartReq_c;
    
    /* Create the Start request message data. */
    pStartReq = &pMsg->msgData.startReq;
    /* PAN ID - LSB, MSB. */
    memcpy(pStartReq->panId, (void *)panId, 2);
    /* Logical Channel - the default of 11 will be overridden */
    pStartReq->logicalChannel = logicalChannel;
    /* Beacon Order - 0xF = turn off beacons */
    pStartReq->beaconOrder = 0x0F;  
    /* Superframe Order - 0xF = turn off beacons */
    pStartReq->superFrameOrder = 0x0F;
    /* Be a PAN coordinator */
    pStartReq->panCoordinator = TRUE;
    /* Dont use battery life extension */
    pStartReq->batteryLifeExt = FALSE;
    /* This is not a Realignment command */
    pStartReq->coordRealignment = FALSE;
    /* Dont use security */
    pStartReq->securityEnable = FALSE;
      
    /* Send the Start 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 Start Request 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_SendAssociateResponse(nwkMessage_t *pMsgIn) will create the response
* message to an Associate Indication (device sends an Associate Request to its
* MAC. The request is transmitted to the coordinator where it is converted into
* an Associate Indication). This function will extract the devices long address,
* and various other flags from the incoming indication message for building the
* response message.
*
* The function may return either of the following values:
*   errorNoError:          The Associate Response 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_SendAssociateResponse(nwkMessage_t *pMsgIn)
{
  mlmeMessage_t *pMsg;
  mlmeAssociateRes_t *pAssocRes;
  uint8_t devChannelId, devId;
  uint8_t devType; 
  uint8_t requestStatus = gSuccess_c;
                          
#ifdef USE_UART
  Uart_Print("Sending the MLME-Associate Response message to the MAC...");      
#endif
 
  /* Allocate a message for the MLME */
  pMsg = MSG_AllocType(mlmeMessage_t);
  if (pMsg != NULL)
  {
    // gather information about the device
    devType = pMsgIn->msgData.associateInd.deviceAddress[1] & 0x03;
    
    if (devType == devTrans)
    {
      devId = pMsgIn->msgData.associateInd.deviceAddress[0];
      devChannelId = 0;
    }
    else if (devType == devRecEnd)
    {
      // device ID - lower 4 bits
      devId = pMsgIn->msgData.associateInd.deviceAddress[0] & 0x0F;
      // channel ID - higher 4 bits
      devChannelId = ((pMsgIn->msgData.associateInd.deviceAddress[0] & 0xF0) >> 4);
    }
    
    /* This is a MLME-ASSOCIATE.res command */
    pMsg->msgType = gMlmeAssociateRes_c;
    
    /* Create the Associate response message data. */
    pAssocRes = &pMsg->msgData.associateRes;
    
    /* Assign a short address to the device. If a device does not want to use 
       short addresses at all in the PAN, a short address of 0xFFFE must
       be assigned to it. But we don't want such devices => reject them. */
    if (pMsgIn->msgData.associateInd.capabilityInfo & gCapInfoAllocAddr_c)
    {
      /* Assign a unique short address less than 0xfffe if the device requests so. */
      pAssocRes->assocShortAddress[0] = pMsgIn->msgData.associateInd.deviceAddress[0];
      pAssocRes->assocShortAddress[1] = pMsgIn->msgData.associateInd.deviceAddress[1];
      
      /* Association granted. May also be gPanAtCapacity_c or gPanAccessDenied_c. */
      requestStatus = gSuccess_c;
    }
    else
    {
      /* A short address of 0xfffe means that the device is granted access to
         the PAN (Associate successful) but that long addressing is used.*/
      pAssocRes->assocShortAddress[0] = 0xFE;
      pAssocRes->assocShortAddress[1] = 0xFF;
      
      /* Association denied. May also be gPanAtCapacity_c or gSuccess_c. */
      requestStatus = gPanAccessDenied_c;
    }
    
    pAssocRes->status = requestStatus;
    
    /* Get the 64 bit address of the device requesting association. */
    memcpy(pAssocRes->deviceAddress, pMsgIn->msgData.associateInd.deviceAddress, 8);
    /* Do not use security */
    pAssocRes->securityEnable = FALSE;
    
    /* Send the Associate Response to the MLME. */
    if (MSG_Send(NWK_MLME, pMsg) == gSuccess_c)
    {                      
#ifdef USE_UART    
      Uart_Print("Done\n");    
#endif
      
      if (requestStatus == gSuccess_c)
      {
        if (devType == devRecEnd)
        {
          // save association info
          associatedRecs[devChannelId] |= (1 << devId);
                    
#ifdef USE_UART
          Uart_Print("Receiver associated");  
#endif
        }
        else 
        {
#ifdef USE_UART
        Uart_Print("Transmitter associated");
#endif              
        }
      }
      else
      {          
#ifdef USE_UART
        Uart_Print("Device rejected");
#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_HandleMlmeInput(nwkMessage_t *pMsg) function will handle various
* messages from the MLME, e.g. (Dis)Associate Indication.
*
* 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 gNwkAssociateInd_c:     
#ifdef USE_UART
    Uart_Print("Received an MLME-Associate Indication from the MAC\n");       
#endif
    /* A device sent us an Associate Request. We must send back a response.  */
    ret = App_SendAssociateResponse(pMsg); 
    break;
    
  case gNwkCommStatusInd_c:
    /* Sent by the MLME after the Association Response has been transmitted. */  
#ifdef USE_UART
    Uart_Print("Received an MLME-Comm-Status Indication from the MAC\n");    
#endif
    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 App_TransmitData() function will perform (single/multi buffered)
* data transmissions of data received from the transmitter end-device.
* The constant MAX_PENDING_DATA_PACKETS determines the maximum
* number of packets pending for transmission in the MAC. A global variable
* is incremented each time a data packet is sent to the MCPS, and decremented
* when the corresponding MCPS-Data Confirm message is received. If the counter
* reaches the defined maximum no more data buffers are allocated until the
* counter is decreased below the maximum number of pending packets.
*
******************************************************************************/
void App_TransmitData(void)
{   
  uint8_t i = 0, devChannelId;
  uint16_t devList;
  uint8_t devShortAddress[2] = {0x00, 0x01};
  static uint8_t dataBuffer[DEFAULT_DATA_LENGTH];
  
  uint8_t msduLength = getTransmitData(dataBuffer);
  if (msduLength)
  {
    /* Data was available in the transmit buffer. Now create an
       MCPS-Data Request message containing the UART data for each
       device on the channel. */
                               
    devChannelId = *(dataBuffer); // first byte of the message
    devList = associatedRecs[devChannelId];
       
    for (i = 0; i < 16 && devList > 0; i++)
    {
      if ((devList >> i) & 0x01)
      {
        // the device with ID i is associed, send the message to it
        
        if ((numPendingPackets < MAX_PENDING_DATA_PACKETS) && (pPacket == NULL))
        {        
          /* If the maximum number of pending data buffes is below maximum limit 
             and we do not have a data buffer already then allocate one. */
          pPacket = MSG_Alloc(sizeof(nwkToMcpsMessage_t) - 1 + DEFAULT_DATA_LENGTH);

          if (pPacket != NULL)
          {
            devShortAddress[0] = (uint8_t) ((devChannelId & 0x0F) << 4) | i;
            
            pPacket->msgType = gMcpsDataReq_c;   
  					/* Copy data to be sent to packet */
  					memcpy(pPacket->msgData.dataReq.msdu, (void *) dataBuffer, msduLength);
            /* Create the header */
            memcpy(pPacket->msgData.dataReq.dstAddr, devShortAddress, 2);
            memcpy(pPacket->msgData.dataReq.srcAddr, (void *) shortAddress, 2);
            memcpy(pPacket->msgData.dataReq.dstPanId, (void *) panId, 2);
            memcpy(pPacket->msgData.dataReq.srcPanId, (void *) panId, 2);
            pPacket->msgData.dataReq.dstAddrMode = gAddrModeShort_c;
            pPacket->msgData.dataReq.srcAddrMode = gAddrModeShort_c;
            pPacket->msgData.dataReq.msduLength = msduLength;
            /* Request MAC level acknowledgement, and 
               direct transmission of the data packet */
            pPacket->msgData.dataReq.txOptions = gTxOptsAck_c;
            /* Give the data packet a handle. The handle is
               returned in the MCPS-Data Confirm message. */
            pPacket->msgData.dataReq.msduHandle = msduHandle++;
            
            /* Send the Data Request to the MCPS */
            NR MSG_Send(NWK_MCPS, pPacket);
            /* Prepare for another data buffer */
            pPacket = NULL;
            numPendingPackets++;
          }
        }
      }
    }
  }
} 


/* Helper funcs, inspired by Uart lib */
uint8_t getTransmitData(uint8_t *pBuffer)
{
  appMessage_t *pMsg;
  
  if (txBufferHead == txBufferTail)
  {
    return 0; // no messages in buffer, exit
  }
    
	pMsg = txBuffer[txBufferTail];	// read the data, increment the pointer
  txBufferTail = (txBufferTail + 1) & (TX_BUFFER_LEN - 1);

  // copy the message to the buffer
  *(pBuffer++) = pMsg->channelId;
  *(pBuffer++) = pMsg->command;
  *(pBuffer++) = pMsg->data;
  
  MSG_Free(pMsg);
  pMsg = NULL;

  return 3; // True indicates new data ready
}

     
void setTransmitData(appMessage_t *pMsg)
{
	txBuffer[txBufferHead] = pMsg;
  txBufferHead = (txBufferHead + 1) & (TX_BUFFER_LEN - 1);
}


/******************************************************************************
* 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;
}