int main( void )
{
    halInit();
    moduleInit();    
    printf("\r\n****************************************************\r\n");    
    printf("Secure Communications Example - ROUTER - using AFZDO\r\n");
    HAL_ENABLE_INTERRUPTS();

#define MODULE_START_DELAY_IF_FAIL_MS 5000

    struct moduleConfiguration defaultConfiguration = DEFAULT_MODULE_CONFIGURATION_ROUTER;
    defaultConfiguration.securityMode = SECURITY_MODE_PRECONFIGURED_KEYS;
    defaultConfiguration.securityKey = key;
    start:
    while ((result = startModule(&defaultConfiguration, GENERIC_APPLICATION_CONFIGURATION)) != MODULE_SUCCESS)
    {
        printf("Module start unsuccessful. Error Code 0x%02X. Retrying...\r\n", result);
        delayMs(MODULE_START_DELAY_IF_FAIL_MS);
    }

    printf("On Network!\r\n");
    setLed(0);

    /* On network, display info about this network */
#ifdef DISPLAY_NETWORK_INFORMATION     
    displayNetworkConfigurationParameters();
    displayDeviceInformation();
#endif  

    /* Now the network is running - send a message to the coordinator every few seconds.*/
#define TEST_CLUSTER 0x77    

    while (1)
    {
        printf("Sending Message %u  ", counter++);
        result = afSendData(DEFAULT_ENDPOINT,DEFAULT_ENDPOINT,0, TEST_CLUSTER, testMessage, 5);
        if (result == MODULE_SUCCESS)
        {
            printf("Success\r\n");
        } else {
            printf("ERROR %02X ", result);
#ifdef RESTART_AFTER_ZM_FAILURE
            printf("\r\nRestarting\r\n");
            goto start;
#else        
            printf("stopping\r\n");
            while(1);
#endif
        }
        toggleLed(1);
        delayMs(2000);
    }
}
/** Converts the message payload into a ConfigRequestMessage and then gets the current configuration
and replies to the node with a ConfigResposeMessage containing the configuration information */
static void processConfigRequestMessage()
{    
    struct configRequestMessage req;
    deserializeConfigRequestMessage(zmBuf+AF_INCOMING_MESSAGE_PAYLOAD_START_FIELD, &req);  // Convert the bytes into a Message struct            
    displayConfigRequestMessage(&req);
    
    // Now create  the Config Response message....
    uint16_t shortAddressToReplyTo = AF_INCOMING_MESSAGE_SHORT_ADDRESS();
    
    struct configResponseMessage resp;
    resp.header = req.header;   // First start with the header from the config request msg
    resp.responseSequence = req.header.sequence; // This indicates how the response matches the request            
    resp.header.sequence++;     // Next, increment the sequence number
    resp.header.flags=0;        // Nothing further to do; we don't need a response
    
    resp.reserved1=0xBEAD;        // Helps troubleshooting to give this an obvious value
    resp.timestamp = timestamp; // In a server-side implementation this would be the unix timestamp
    resp.reserved2=0xEE;        // Another troubleshooting help
    resp.numParameters=0;       // Red, Blue, Green
    // Add the red LED value
    resp.kvps[resp.numParameters].oid = OID_RED_LED;
    resp.kvps[resp.numParameters].value = red;
    resp.numParameters++;
    // Add the blue LED value
    resp.kvps[resp.numParameters].oid = OID_BLUE_LED;
    resp.kvps[resp.numParameters].value = blue;
    resp.numParameters++;
    // Add the green LED value
    resp.kvps[resp.numParameters].oid = OID_GREEN_LED;
    resp.kvps[resp.numParameters].value = green;
    resp.numParameters++;
    
    // Now we can send the message
    displayConfigResponseMessage(&resp);
    uint8_t buffer[32];
    //uint8_t* buffer = (zmBuf + 100);         // To conserve RAM you could use the tail of zmBuf for serialization buffer
    serializeConfigResponseMessage(&resp, buffer);       // Convert our message struct to an array of bytes
    
    uint8_t numBytesSerialized = getSizeOfConfigResponseMessage(&resp);
    //printf("numBytesSerialized = %u; send to %04X\r\n", numBytesSerialized, shortAddressToReplyTo);
    
    moduleResult_t result = 0;
    result = afSendData(DEFAULT_ENDPOINT, DEFAULT_ENDPOINT, shortAddressToReplyTo, CONFIG_RESPONSE_MESSAGE_CLUSTER, buffer, getSizeOfConfigResponseMessage(&resp)); // Send the message
    if (result != MODULE_SUCCESS)
    {
        printf("afSendData error 0x%02X; ignoring...\r\n", result);
    } else {   
        printf("Success\r\n");
    }
}
static void stateMachine()
{
    while (1)
    {
        if (zigbeeNetworkStatus == NWK_ONLINE)
        {
            if(moduleHasMessageWaiting())      //wait until SRDY goes low indicating a message has been received.   
                displayMessages();
        }

        switch (state)
        {
        case STATE_IDLE:
        {
            /* process command line commands only if not doing anything else: */
            if (command != NO_CHARACTER_RECEIVED)
            {
                /* parse the command entered, and go to the required state */
                state = processCommand(command);

                command = NO_CHARACTER_RECEIVED;
            }
            /* note: other flags (for different messages or events) can be added here */
            break;
        }
        case STATE_INIT:
        {
            printf("Starting State Machine\r\n");
            state = STATE_GET_DEVICE_TYPE;
            break;
        }
        /* A button press during startup will cause the application to prompt for device type */
        case STATE_GET_DEVICE_TYPE: 
        {
            //printf("Current Configured DeviceType: %s\r\n", getDeviceTypeName());
            set_type:                
            /* if saving device type to flash memory:
                printf("Any other key to exit. Timeout in 5 seconds.\r\n");
                / long wait = 0;
                long timeout = TICKS_IN_ONE_MS * 5000l;
                while ((command == NO_CHARACTER_RECEIVED) && (wait != timeout))
                wait++;
             */
            while (command == NO_CHARACTER_RECEIVED)
            {
                printf("Setting Device Type: Press C for Coordinator, R for Router, or E for End Device.\r\n");
                delayMs(2000);
            }

            switch (command)
            {
            case 'C':
            case 'c':
                printf("Coordinator it is...\r\n");
                zigbeeDeviceType = COORDINATOR;
                break;
            case 'R':
            case 'r':
                printf("Router it is...\r\n");
                zigbeeDeviceType = ROUTER;
                break;

            case 'E':
            case 'e':
                printf("End Device it is...\r\n");
                zigbeeDeviceType = END_DEVICE;
                break;
            default:
                command = NO_CHARACTER_RECEIVED;
                goto set_type;

            }
            command = NO_CHARACTER_RECEIVED;
            state = STATE_MODULE_STARTUP;
            break;
        }
        case STATE_MODULE_STARTUP:
        {
#define MODULE_START_DELAY_IF_FAIL_MS 5000
            moduleResult_t result;
            /* Start with the default module configuration */
            struct moduleConfiguration defaultConfiguration = DEFAULT_MODULE_CONFIGURATION_COORDINATOR;
            /* Make any changes needed here (channel list, PAN ID, etc.)
                   We Configure the Zigbee Device Type (Router, Coordinator, End Device) based on what user selected */
            defaultConfiguration.deviceType = zigbeeDeviceType;
            while ((result = startModule(&defaultConfiguration, GENERIC_APPLICATION_CONFIGURATION)) != MODULE_SUCCESS)
            {
                printf("Module start unsuccessful. Error Code 0x%02X. Retrying...\r\n", result);
                delayMs(MODULE_START_DELAY_IF_FAIL_MS);
            }
            printf("Success\r\n");
            state = STATE_DISPLAY_NETWORK_INFORMATION;
            zigbeeNetworkStatus = NWK_ONLINE;
            break;
        }
        case STATE_DISPLAY_NETWORK_INFORMATION:                 
        {
            printf("Module Information:\r\n");
            /* On network, display info about this network */
            displayNetworkConfigurationParameters();
            displayDeviceInformation();
            displayCommandLineInterfaceHelp();
            state = STATE_IDLE;   //startup is done!
            break;
        }
        case STATE_VALID_SHORT_ADDRESS_ENTERED:  //command line processor has a valid shortAddressEntered
        {
            printf("Valid Short Address Entered\r\n");
            state = pendingState;
            break;
        }
        case STATE_VALID_LONG_ADDRESS_ENTERED:
        {
            /* flip byte order */
            int8_t temp[8];
            int i;
            for (i=0; i<8; i++)
                temp[7-i] = longAddressEntered[i];

            memcpy(longAddressEntered, temp, 8);  //Store LSB first since that is how it will be sent:
            state = pendingState;
            break;
        }
        case STATE_SEND_MESSAGE_VIA_SHORT_ADDRESS:
        {
            printf("Send via short address to %04X\r\n", shortAddressEntered);
            moduleResult_t result = afSendData(DEFAULT_ENDPOINT,DEFAULT_ENDPOINT,shortAddressEntered, TEST_CLUSTER, testMessage, 5);
            if (result == MODULE_SUCCESS)
            {
                printf("Success\r\n");
            } else {
                printf("Could not send to that device (Error Code 0x%02X)\r\n", result);

#ifdef RESTART_AFTER_ZM_FAILURE
                printf("\r\nRestarting\r\n");
                state = STATE_MODULE_STARTUP;
                continue;
#endif
            }
            state = STATE_IDLE;
            break;
        }
        case STATE_SEND_MESSAGE_VIA_LONG_ADDRESS:
        {
            printf("Send via long address to (LSB first)");
            printHexBytes(longAddressEntered, 8);
            moduleResult_t result = afSendDataExtended(DEFAULT_ENDPOINT, DEFAULT_ENDPOINT, longAddressEntered,
                    DESTINATION_ADDRESS_MODE_LONG, TEST_CLUSTER, testMessage, 5);
            if (result == MODULE_SUCCESS)
            {
                printf("Success\r\n");
            } else {
                printf("Could not send to that device (Error Code 0x%02X)\r\n", result);
#ifdef RESTART_AFTER_ZM_FAILURE
                printf("\r\nRestarting\r\n");
                state = STATE_MODULE_STARTUP;
                continue;
#endif
            }
            state = STATE_IDLE;
            break;
        }
        case STATE_FIND_VIA_SHORT_ADDRESS:
        {
            printf("Looking for that device...\r\n");
            moduleResult_t result = zdoRequestIeeeAddress(shortAddressEntered, SINGLE_DEVICE_RESPONSE, 0);
            if (result == MODULE_SUCCESS)
            {
#ifndef ZDO_NWK_ADDR_RSP_HANDLED_BY_APPLICATION
                displayZdoAddressResponse(zmBuf + SRSP_PAYLOAD_START);
#endif
            } else {
                printf("Could not locate that device (Error Code 0x%02X)\r\n", result);
            }
            state = STATE_IDLE;
            break;
        }
        case STATE_FIND_VIA_LONG_ADDRESS:
        {
            printf("Looking for that device...\r\n");
            moduleResult_t result = zdoNetworkAddressRequest(longAddressEntered, SINGLE_DEVICE_RESPONSE, 0);
            if (result == MODULE_SUCCESS)
            {
#ifndef ZDO_NWK_ADDR_RSP_HANDLED_BY_APPLICATION
                displayZdoAddressResponse(zmBuf + SRSP_PAYLOAD_START);
#endif
            } else {
                printf("Could not locate that device (Error Code 0x%02X)\r\n", result);
            }
            state = STATE_IDLE;
            break;
        }
        default:     //should never happen
        {
            printf("UNKNOWN STATE (%u)\r\n", state);
            state = STATE_IDLE;
        }
        break;
        }
    }
}
void stateMachine()
{
    while (1)
    {
        if (zigbeeNetworkStatus == NWK_ONLINE)
        {  
            if(moduleHasMessageWaiting())      //wait until SRDY goes low indicating a message has been received.
            {
                getMessage();                      
                displayMessage();
            }   
        }
        
        switch (state)
        {
        case STATE_IDLE:
            {
                if (stateFlags & STATE_FLAG_SEND_INFO_MESSAGE)  //if there is a pending info message to be sent
                {
                    state = STATE_SEND_INFO_MESSAGE;            //then send the message and clear the flag
                    stateFlags &= ~STATE_FLAG_SEND_INFO_MESSAGE;
                }
                /* Other flags (for different messages or events) can be added here */
                break;
            }
            
        case STATE_MODULE_STARTUP:
            {
#define MODULE_START_DELAY_IF_FAIL_MS   5000    // Must be greater than MODULE_START_FAIL_LED_ONTIME
                moduleResult_t result;
                struct moduleConfiguration defaultConfiguration = DEFAULT_MODULE_CONFIGURATION_END_DEVICE;
                
                /* Uncomment below to restrict the device to a specific PANID
                defaultConfiguration.panId = 0x1234;
                */
                
                struct applicationConfiguration endDeviceConf;
                endDeviceConf.endPoint = 1;
                endDeviceConf.latencyRequested = LATENCY_NORMAL;
                endDeviceConf.profileId = 0xcc00; // the clock profile is 0xCC00
                endDeviceConf.deviceId = 0x8866;
                endDeviceConf.deviceVersion = 0x01;
                endDeviceConf.numberOfBindingInputClusters = 4; // number of binding input cluster
                endDeviceConf.bindingInputClusters[0] = 0x0000; // basic cluster
                endDeviceConf.bindingInputClusters[1] = 0x0003; // identify cluster
                endDeviceConf.bindingInputClusters[2] = 0xfc01; // synchronise clock cluster
                endDeviceConf.bindingInputClusters[3] = 0xfc02; // send string message cluster
                endDeviceConf.numberOfBindingOutputClusters = 4; // number of binding output cluster
                endDeviceConf.bindingOutputClusters[0] = 0x0000;
                endDeviceConf.bindingOutputClusters[1] = 0x0003;
                endDeviceConf.bindingOutputClusters[2] = 0xfc01;
                endDeviceConf.bindingOutputClusters[3] = 0xfc02;
                
                struct applicationConfiguration ac;
                ac.endPoint = 1;
                ac.deviceVersion = 0x10;
                ac.profileId = 0xfafa;
                ac.latencyRequested = LATENCY_NORMAL;
                
                ac.numberOfBindingInputClusters = 2;
                ac.bindingInputClusters[0] =        0x0000;    
                ac.bindingInputClusters[1] =        0x0900;    
                ac.numberOfBindingOutputClusters = 2;
                ac.bindingOutputClusters[0] =        0x0000;
                ac.bindingOutputClusters[1] =        0x0900;
                
                /* Below is an example of how to restrict the device to only one channel:
                defaultConfiguration.channelMask = CHANNEL_MASK_17;
                printf("DEMO - USING CUSTOM CHANNEL 17\r\n");
                */
                
                //while ((result = startModule(&defaultConfiguration, GENERIC_APPLICATION_CONFIGURATION)) != MODULE_SUCCESS)
                while ((result = startModule(&defaultConfiguration, &ac)) != MODULE_SUCCESS)
                {
                    SET_NETWORK_FAILURE_LED_ON();          // Turn on the LED to show failure
                    printf("FAILED. Error Code 0x%02X. Retrying...\r\n", result);
                    delayMs(MODULE_START_DELAY_IF_FAIL_MS/2);                    
                    SET_NETWORK_FAILURE_LED_OFF();
                    delayMs(MODULE_START_DELAY_IF_FAIL_MS/2);
                }
                
                INIT_BOOSTER_PACK_LEDS();                
                SET_NETWORK_LED_ON();
                SET_NETWORK_FAILURE_LED_OFF();
                printf("Success\r\n"); 
                /* Module is now initialized so store MAC Address */
                zbGetDeviceInfo(DIP_MAC_ADDRESS);
                memcpy(hdr.mac, zmBuf+SRSP_DIP_VALUE_FIELD, 8);
#define MESSAGE_PERIOD_SECONDS  4
                int16_t timerResult = initTimer(MESSAGE_PERIOD_SECONDS);
                if (timerResult != 0)
                {
                    printf("timerResult Error %d, STOPPING\r\n", timerResult);
                    while (1);   
                }
                
                state = STATE_DISPLAY_NETWORK_INFORMATION;
                break;
            }
        case STATE_DISPLAY_NETWORK_INFORMATION:
            {
                printf("~ni~");
                /* On network, display info about this network */              
                displayNetworkConfigurationParameters();                
                displayDeviceInformation();
                /* Set module GPIOs as output and turn them off */
                if ((sysGpio(GPIO_SET_DIRECTION, ALL_GPIO_PINS) != MODULE_SUCCESS) || (sysGpio(GPIO_CLEAR, ALL_GPIO_PINS) != MODULE_SUCCESS))
                {
                    printf("ERROR\r\n");
                }
                state = STATE_SEND_INFO_MESSAGE;
                break;   
            }
        case STATE_SEND_INFO_MESSAGE:
            {
                printf("~im~");
                
                struct infoMessage im;
                /* See infoMessage.h for description of these info message fields.*/
                im.header = hdr;
                im.deviceType = DEVICETYPE_TESLA_CONTROLS_END_DEVICE_DEMO;
                im.header.sequence = sequenceNumber++;
                im.numParameters = getSensorValues(im.kvps);    // Does two things: Loads infoMessage with sensor value KVPs and gets the number of them
                                
                // now, add status message interval
                im.kvps[im.numParameters].oid = OID_STATUS_MESSAGE_INTERVAL;
                im.kvps[im.numParameters].value = MESSAGE_PERIOD_SECONDS;
                im.numParameters++;
                
                // add zigbee module information:
                if (sysVersion() != MODULE_SUCCESS)
                {
                    printf("ERROR retriving module information\r\n");
                } else {                
                displaySysVersion();
                
                // Product ID
                im.kvps[im.numParameters].oid = OID_MODULE_PRODUCT_ID;
                im.kvps[im.numParameters].value = zmBuf[SYS_VERSION_RESULT_PRODUCTID_FIELD];
                im.numParameters++;
                
                // FW - Major
                im.kvps[im.numParameters].oid = OID_MODULE_FIRMWARE_MAJOR;
                im.kvps[im.numParameters].value = zmBuf[SYS_VERSION_RESULT_FW_MAJOR_FIELD];
                im.numParameters++;    
                
                // FW - Minor
                im.kvps[im.numParameters].oid = OID_MODULE_FIRMWARE_MINOR;
                im.kvps[im.numParameters].value = zmBuf[SYS_VERSION_RESULT_FW_MINOR_FIELD];
                im.numParameters++;  

                // FW - Build
                im.kvps[im.numParameters].oid = OID_MODULE_FIRMWARE_BUILD;
                im.kvps[im.numParameters].value = zmBuf[SYS_VERSION_RESULT_FW_BUILD_FIELD];
                im.numParameters++;
                }
                
                printInfoMessage(&im);
#define RESTART_DELAY_IF_MESSAGE_FAIL_MS 5000
                uint8_t messageBuffer[MAX_INFO_MESSAGE_SIZE];
                serializeInfoMessage(&im, messageBuffer);
                setLed(SEND_MESSAGE_LED); //indicate that we are sending a message
                moduleResult_t result = afSendData(DEFAULT_ENDPOINT, DEFAULT_ENDPOINT, 0, INFO_MESSAGE_CLUSTER, messageBuffer, getSizeOfInfoMessage(&im)); // and send it
                clearLed(SEND_MESSAGE_LED);                
                if (result != MODULE_SUCCESS)
                {
                    zigbeeNetworkStatus = NWK_OFFLINE;
                    printf("afSendData error %02X; restarting...\r\n", result);
                    delayMs(RESTART_DELAY_IF_MESSAGE_FAIL_MS);  //allow enough time for coordinator to fully restart, if that caused our problem
                    state = STATE_MODULE_STARTUP;
                } else {   
                    printf("Success\r\n");
                    state = STATE_IDLE;
                }
                break;   
            }
        default:     //should never happen
            {
                printf("UNKNOWN STATE\r\n");
                state = STATE_MODULE_STARTUP;
            }
            break;
        }
    } 
}
static void stateMachine()
{
    while (1)
    {
        if (zigbeeNetworkStatus == NWK_ONLINE)
        {
            if(moduleHasMessageWaiting())      //wait until SRDY goes low indicating a message has been received.
            {
                getMessage();                 
                displayMessage();          
            }   
        }
        
        switch (state)
        {
        case STATE_IDLE:
            {
                /* process command line commands only if not doing anything else: */
                if (command != NO_CHARACTER_RECEIVED)
                {
                    /* parse the command entered, and go to the required state */
                    state = processCommand(command);                    
                    command = NO_CHARACTER_RECEIVED;
                }
                /* note: other flags (for different messages or events) can be added here */
                break;
            }
        case STATE_INIT:
            {
                printf("Starting State Machine\r\n");
                state = STATE_GET_DEVICE_TYPE;
                break;
            }
            /* A button press during startup will cause the application to prompt for device type */
        case STATE_GET_DEVICE_TYPE: 
            {
            set_type:                
                while (command == NO_CHARACTER_RECEIVED)
                {
                    printf("Setting Device Type: Press C for Coordinator, R for Router, or E for End Device.\r\n");
#define DEVICE_TYPE_DELAY_MS    2000
#define DEVICE_TYPE_DELAY_PER_CYCLE_MS  100
                    uint16_t delayCycles = DEVICE_TYPE_DELAY_MS / DEVICE_TYPE_DELAY_PER_CYCLE_MS;
                    while ((command == NO_CHARACTER_RECEIVED) && (delayCycles--) > 0)
                        delayMs(DEVICE_TYPE_DELAY_PER_CYCLE_MS);
                }
                
                switch (command)
                {
                case 'C':
                case 'c':
                    printf("Coordinator it is...\r\n");
                    zigbeeDeviceType = COORDINATOR;
                    break;
                case 'R':
                case 'r':
                    printf("Router it is...\r\n");
                    zigbeeDeviceType = ROUTER;
                    break;
                    
                case 'E':
                case 'e':
                    printf("End Device it is...\r\n");
                    zigbeeDeviceType = END_DEVICE;
                    break;
                default:
                    command = NO_CHARACTER_RECEIVED;
                    goto set_type;
                    
                }
                command = NO_CHARACTER_RECEIVED;
                state = STATE_MODULE_STARTUP;
                break;
            }
        case STATE_MODULE_STARTUP:
            {
#define MODULE_START_DELAY_IF_FAIL_MS 5000
                moduleResult_t result;
                /* Start with the default module configuration */
                struct moduleConfiguration defaultConfiguration = DEFAULT_MODULE_CONFIGURATION_COORDINATOR;
                /* Make any changes needed here (channel list, PAN ID, etc.)
                We Configure the Zigbee Device Type (Router, Coordinator, End Device) based on what user selected */
                defaultConfiguration.deviceType = zigbeeDeviceType;
                
                /* Change this below to be your operating region - MODULE_REGION_NORTH_AMERICA or MODULE_REGION_EUROPE */
#define OPERATING_REGION    (MODULE_REGION_NORTH_AMERICA) // or MODULE_REGION_EUROPE
                
                while ((result = expressStartModule(&defaultConfiguration, GENERIC_APPLICATION_CONFIGURATION, OPERATING_REGION)) != MODULE_SUCCESS)
                {
                    SET_NETWORK_FAILURE_LED_ON();          // Turn on the LED to show failure
                    handleModuleError(result);
                    printf("Retrying...\r\n");
                    delayMs(MODULE_START_DELAY_IF_FAIL_MS/2);                    
                    SET_NETWORK_FAILURE_LED_OFF();
                    delayMs(MODULE_START_DELAY_IF_FAIL_MS/2);
                }                
                printf("Success\r\n");
                state = STATE_DISPLAY_NETWORK_INFORMATION;
                zigbeeNetworkStatus = NWK_ONLINE;
                break;
            }
        case STATE_DISPLAY_NETWORK_INFORMATION:                 
            {
                printf("Module Information:\r\n");
                /* On network, display info about this network */
                displayNetworkConfigurationParameters();
                displayDeviceInformation();
                displayCommandLineInterfaceHelp();
                state = STATE_IDLE;   //startup is done!
                break;
            }
        case STATE_VALID_SHORT_ADDRESS_ENTERED:  //command line processor has a valid shortAddressEntered
            {
                state = pendingState;
                break;
            }
        case STATE_VALID_LONG_ADDRESS_ENTERED:
            {
                /* Flip byte order because we need to send it LSB first */
                int8_t temp[8];
                int i;
                for (i=0; i<8; i++)
                    temp[7-i] = longAddressEntered[i];
                
                memcpy(longAddressEntered, temp, 8);  //Store LSB first since that is how it will be sent:
                state = pendingState;
                break;
            }
        case STATE_SEND_MESSAGE_VIA_SHORT_ADDRESS:
            {
                printf("Send to 0x%04X\r\n", shortAddressEntered);
                moduleResult_t result = afSendData(DEFAULT_ENDPOINT,DEFAULT_ENDPOINT,shortAddressEntered, TEST_CLUSTER, testMessage, 5);
                if (result == MODULE_SUCCESS)
                {
                    printf("Success\r\n");
                } else {
                    handleModuleError(result);                    
#ifdef RESTART_AFTER_ZM_FAILURE
                    printf("\r\nRestarting\r\n");
                    state = STATE_MODULE_STARTUP;
                    continue;
#endif
                }
                state = STATE_IDLE;
                break;
            }
        case STATE_SEND_MESSAGE_VIA_LONG_ADDRESS:
            {
                printf("Send to (LSB first)");
                printHexBytes(longAddressEntered, 8);
                moduleResult_t result = afSendDataExtended(DEFAULT_ENDPOINT, DEFAULT_ENDPOINT, longAddressEntered,
                                                           DESTINATION_ADDRESS_MODE_LONG, TEST_CLUSTER, testMessage, 5);
                if (result == MODULE_SUCCESS)
                {
                    printf("Success\r\n");
                } else {
                    handleModuleError(result);
#ifdef RESTART_AFTER_ZM_FAILURE
                    printf("\r\nRestarting\r\n");
                    state = STATE_MODULE_STARTUP;
                    continue;
#endif
                }
                state = STATE_IDLE;
                break;
            }
            
#ifdef LEAVE_REQUEST            
        case STATE_MANAGEMENT_LEAVE_REQUEST:
            {
                printf("Sending Leave Request for MAC (LSB first)");
                printHexBytes(longAddressEntered, 8);
                moduleResult_t result = zdoManagementLeaveRequest(longAddressEntered, 0);
                
                if (result == MODULE_SUCCESS)
                {
                    printf("Success\r\n");
                } else {
                    handleModuleError(result);
                }
                state = STATE_IDLE;
                break;
            }
#endif
            
        case STATE_FIND_VIA_SHORT_ADDRESS:
            {
                printf("Looking for that device...\r\n");
                moduleResult_t result = zdoRequestIeeeAddress(shortAddressEntered, INCLUDE_ASSOCIATED_DEVICES, 0);
                if (result == MODULE_SUCCESS)
                {
#ifndef ZDO_NWK_ADDR_RSP_HANDLED_BY_APPLICATION
                    displayZdoAddressResponse(zmBuf + SRSP_PAYLOAD_START);
#endif
                } else {
                    handleModuleError(result);
                }
                state = STATE_IDLE;
                break;
            }
            
        case STATE_FIND_VIA_LONG_ADDRESS:
            {
                printf("Looking for that device...\r\n");
                moduleResult_t result = zdoNetworkAddressRequest(longAddressEntered, INCLUDE_ASSOCIATED_DEVICES, 0);
                if (result == MODULE_SUCCESS)
                {
#ifndef ZDO_NWK_ADDR_RSP_HANDLED_BY_APPLICATION
                    displayZdoAddressResponse(zmBuf + SRSP_PAYLOAD_START);
#endif
                } else {
                    handleModuleError(result);
                }
                state = STATE_IDLE;
                break;
            }
            
#ifdef INCLUDE_PERMIT_JOIN        
        case STATE_SET_PERMIT_JOIN_ON:
            {
                printf("Turning joining ON...\r\n");
                moduleResult_t result = zdoManagementPermitJoinRequest(shortAddressEntered, PERMIT_JOIN_ON_INDEFINITELY, 0);
                if (result == MODULE_SUCCESS)
                {
                    printf("Success\r\n");
                } else {
                    handleModuleError(result);
                }
                state = STATE_IDLE;
                break; 
            }
            
        case STATE_SET_PERMIT_JOIN_OFF:
            {
                printf("Turning joining OFF...\r\n");
                moduleResult_t result = zdoManagementPermitJoinRequest(shortAddressEntered, PERMIT_JOIN_OFF, 0);
                if (result == MODULE_SUCCESS)
                {
                    printf("Success\r\n");
                } else {
                    handleModuleError(result);
                }
                state = STATE_IDLE;
                break; 
            }   
#endif
            
#ifdef DISCOVER_NETWORKS             
        case STATE_NETWORK_DISCOVERY_REQUEST:
            {
                printf("Scanning...\r\n");
                moduleResult_t result = zdoNetworkDiscoveryRequest(ANY_CHANNEL_MASK, BEACON_ORDER_480_MSEC);
                if (result == MODULE_SUCCESS)
                {
                    printf("Success\r\n");
                } else {
                    handleModuleError(result);
                }
                state = STATE_IDLE;
                break; 
            }
#endif
            
#ifdef INCLUDE_NETWORK_TOPOLOGY
        case STATE_GET_NETWORK_TOPOLOGY:
            {
                printf("Displaying NWK Topology...........\r\n");                
                initSet(&searchedSet);
                // Start the recursive search for all children of the short address
                //removeAllFromSet();
                int8_t numChildren = displayChildren(shortAddressEntered);
                state = STATE_IDLE;
                break;
            }
#endif
            
        default:     //should never happen
            {
                printf("UNKNOWN STATE (%u)\r\n", state);
                state = STATE_IDLE;
            }
            break;
        }
    }
}
void stateMachine()
{
    while (1)
    {
        switch (state)
        {
        case STATE_IDLE:
            {
                if (stateFlags & STATE_FLAG_SEND_INFO_MESSAGE)  //if there is a pending info message to be sent
                {
                    state = STATE_SEND_INFO_MESSAGE;            //then send the message and clear the flag
                    stateFlags &= ~STATE_FLAG_SEND_INFO_MESSAGE;
                }
                //note: other flags (for different messages or events) can be added here
                break;
            }
            
        case STATE_ZNP_STARTUP:
            {
#define ZNP_START_DELAY_IF_FAIL_MS 5000
                /* Start the network; if fails then wait a second and try again. */
                signed int startResult = startZnp(END_DEVICE);
                while (startResult != ZNP_SUCCESS)
                {
                    printf("FAILED. Error Code %i, ZNP Result %i. Retrying...\r\n", startResult, znpResult);
                    delayMs(ZNP_START_DELAY_IF_FAIL_MS);
                    startResult = startZnp(END_DEVICE);
                }
                printf("Success\r\n"); 
                
                //ZNP Initialized so store MAC Address
                memcpy(hdr.mac, getMacAddress(), 8); 
#ifdef SEND_MESSAGE_ON_TIMER
                signed int timerResult = initTimer(4, WAKEUP_AFTER_TIMER);
                if (timerResult != 0)
                {
                    printf("timerResult Error %i, STOPPING\r\n", timerResult);
                    while (1);   
                }
#endif
                state = STATE_DISPLAY_NETWORK_INFORMATION;
                break;
            }
        case STATE_DISPLAY_NETWORK_INFORMATION:
            {
                printf("~ni~");
                /* On network, display info about this network */              
                getNetworkConfigurationParameters();                
                getDeviceInformation();    
                state = STATE_SEND_INFO_MESSAGE;
                break;   
            }
        case STATE_SEND_INFO_MESSAGE:
            {
                printf("~im~");
                setLed(1);
                im.header->sequence = sequenceNumber++;                
                im.cause = infoMessageCause;
                unsigned char* panid = getDeviceInformationProperty(DIP_PANID);
                im.parameters[0] = CONVERT_TO_INT(*panid, *(panid+1));      //PAN ID
                im.parameters[1] = getVcc3();                               //VCC
                im.parameters[2] = getLightSensor();                        //Light Sensor
                printInfoMessage(&im);
#define ZNP_RESTART_DELAY_IF_MESSAGE_FAIL_MS 5000
                unsigned char msgBuf[100];
                serializeInfoMessage(&im, msgBuf);
                afSendData(DEFAULT_ENDPOINT,DEFAULT_ENDPOINT,0, INFO_MESSAGE_CLUSTER, msgBuf, getSizeOfInfoMessage(&im));
                if (znpResult != ZNP_SUCCESS)
                {
                    printf("afSendData error %i; restarting...\r\n", znpResult);
                    delayMs(ZNP_RESTART_DELAY_IF_MESSAGE_FAIL_MS);  //allow enough time for coordinator to fully restart, if that caused our problem
                    state = STATE_ZNP_STARTUP;
                } else {      
                    state = STATE_IDLE;
                    clearLeds();        
                    HAL_SLEEP();                    
                }
                break;   
            }
        default:     //should never happen
            {
                printf("UNKNOWN STATE\r\n");
                state = STATE_ZNP_STARTUP;
            }
            break;
        }
    } 
}
int main( void )
{
    halInit();
    printf("\r\n****************************************************\r\n");    
    printf("Secure Communications Example - ROUTER - using AFZDO\r\n");
    HAL_ENABLE_INTERRUPTS();
    
    //Simple idiot check to ensure that we didn't accidentally define both security options.
#if defined(USE_SECURITY_MODE_PRECONFIGURED_KEYS) && defined(USE_SECURITY_MODE_COORD_DIST_KEYS)
    printf("ERROR - only select one security option!\r\n");
    while (1);
#endif 
    
    /* Initialize the ZNP */
    printf("Initializing the ZNP\r\n");
    znpInit(); 
    handleReturnValue();
    
    /* Set Startup Options (will restore the ZNP to default values on reset) */
    printf("Setting StartupOptions\r\n");    
    setStartupOptions(STARTOPT_CLEAR_CONFIG + STARTOPT_CLEAR_STATE);
    handleReturnValue();
    
    /* Reset the ZNP */
    printf("Reset the ZNP\r\n");    
    znpReset();
    handleReturnValue();
    
    /* Set Zigbee Device Type to be ROUTER */
    printf("Setting Zigbee Device Type\r\n"); 
    setZigbeeDeviceType(ROUTER);
    handleReturnValue();

    /* Configure security mode, if it is being used */
#ifdef USE_SECURITY_MODE_PRECONFIGURED_KEYS
    printf("PRECONFIGURED KEYS\r\n");
    /* Turn security ON with pre-configured keys */
    setSecurityMode(SECURITY_MODE_PRECONFIGURED_KEYS);
    handleReturnValue();    
    
    /* All devices on the network must be loaded with the same key */ 
    setSecurityKey(key);
    handleReturnValue();    
#endif
    
#ifdef USE_SECURITY_MODE_COORD_DIST_KEYS
    printf("COORDINATOR DISTRIBUTING KEYS\r\n");
    
    /* Turn security ON with the coordinator distributing keys. 
    Since this is the router we don't need to load a key; it will be sent to us by the coordinator! */
    setSecurityMode(SECURITY_MODE_COORD_DIST_KEYS);
    handleReturnValue();
#endif
    
#if !defined(USE_SECURITY_MODE_PRECONFIGURED_KEYS) && !defined(USE_SECURITY_MODE_COORD_DIST_KEYS)
    printf("WARNING - NO SECURITY OPTION SELECTED; SECURITY OFF\r\n");
#endif 
    
    /* Configure the ZNP for our application */
    printf("Registering Application\r\n");   
    afRegisterGenericApplication();
    handleReturnValue();
    
    /* Now, start the application. We will receive a START_REQUEST_SRSP, and then if it is successful, a START_CONFIRM. */
    printf("Starting the Application\r\n");     
    zdoStartApplication();
    handleReturnValue();
    
    /** Wait until we get on the network. 
    We will receive a ZDO_STATE_CHANGE_IND message whenever the state changes. */
    waitForDeviceState(DEV_ROUTER);

    printf("On Network!\r\n");
    setLed(0);
    
    /* On network, display info about this network */
#ifdef DISPLAY_NETWORK_INFORMATION     
    getNetworkConfigurationParameters();                
    getDeviceInformation();
#endif  
    
    /* Now the network is running - send a message to the coordinator every few seconds.*/
    unsigned char counter = 0;
    unsigned char testMessage[] = {0xE0,0xE1,0xE2,0xE3,0xE4,0xE5,0xE6,0xE7,0xE8,0xE9};
#define TEST_CLUSTER 0x77    
    
    while (1)
    {
        printf("Sending Message %u\r\n", counter++);
        afSendData(DEFAULT_ENDPOINT,DEFAULT_ENDPOINT,0, TEST_CLUSTER, testMessage, 10);
        handleReturnValue();
        toggleLed(1);         
        delayMs(5000);          
    }   
}