/**********************************************************************************************//**
  @file       radio_xbee.c
  @date       $Date$
  @revision   $Revision$

  @brief     Radio Support, for  XBee SX
**************************************************************************************************/

/**************************************************************************************************
*                                           INCLUDES
**************************************************************************************************/

#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include "ComTypeDefs.h"
//#include BOARD_HEADER
#include "cavalier_board_cfg_rev2.h"
#include "hal_uart_ti.h"
#include "sys_tick.h"
#include "device.h"
#include "atcmd.h"
#include "wpan.h"
#include "byteorder.h"
#include "xbee_sx_defs.h"
#include "radio_api.h"
#include "sys_utils.h"
#include "Monitor.h"
#include "hal_nv_ti.h"

extern uint8_t dest[8];
extern uint16_t transmit_status;
extern bool_t atcmd_pending;
extern uint16_t sleep_time, wake_time, ms_val, sq_val;
extern uint8_t sm_val;
extern uint8_t ota_packet[][];
extern uint8_t tail;
extern bool_t  reset_event;

/**************************************************************************************************
*                                           CONSTANTS
**************************************************************************************************/


/**************************************************************************************************
*                                           MACROS
**************************************************************************************************/

int xbee_disc_nodeid_cluster_handler( const wpan_envelope_t FAR *envelope,
	void FAR *context);

/// Include xbee_disc_nodeid_cluster_handler in the cluster table for the
/// the Digi Data endpoint.
#define XBEE_DISC_DIGI_DATA_CLUSTER_ENTRY		\
	{ DIGI_CLUST_NODEID_MESSAGE, xbee_disc_nodeid_cluster_handler, NULL,	\
		WPAN_CLUST_FLAG_INPUT | WPAN_CLUST_FLAG_NOT_ZCL}

int xbee_disc_atnd_response_handler( xbee_dev_t *xbee, const void FAR *raw,
		uint16_t length, void FAR *context);

/// Include xbee_disc_atnd_response_handler in xbee_frame_handlers[].
#define XBEE_FRAME_HANDLE_ATND_RESPONSE		\
	{ XBEE_FRAME_LOCAL_AT_RESPONSE, 0, xbee_disc_atnd_response_handler, NULL }

int xbee_disc_nodeid_frame_handler(xbee_dev_t *xbee, const void FAR *raw,
		uint16_t length, void FAR *context);

/// Include xbee_disc_nodeid_frame_handler in xbee_frame_handlers[] (only
/// necessary if ATAO is set to 0).
#define XBEE_FRAME_HANDLE_AO0_NODEID			\
	{ XBEE_FRAME_NODE_ID, 0, xbee_disc_nodeid_frame_handler, NULL }


int xbee_aggregate_update_handler(xbee_dev_t *xbee, const void FAR *raw,
		uint16_t length, void FAR *context);

#define XBEE_FRAME_HANDLE_AGGREGATE_ADRS_UPDATE			\
	{ XBEE_FRAME_AGGREGATE_ADDRESSING, 0, xbee_aggregate_update_handler, NULL }


/**************************************************************************************************
*                                           LOCAL VARIABLES
**************************************************************************************************/
// An instance required for the local XBee struct
xbee_dev_t my_xbee;

static const xbee_serial_t XBEE_SERPORT = { 0, 0 };		// This is just a dummy placeholder

int transparent_rx( const wpan_envelope_t FAR *envelope, void FAR *context);
// must be sorted by cluster ID
static const wpan_cluster_table_entry_t digi_data_clusters[] =
{
	// transparent serial goes here (cluster 0x0011)
	{ DIGI_CLUST_SERIAL, transparent_rx, NULL,
		WPAN_CLUST_FLAG_INOUT | WPAN_CLUST_FLAG_NOT_ZCL },

	//// handle join notifications (cluster 0x0095) when ATAO is not 0
	//XBEE_DISC_DIGI_DATA_CLUSTER_ENTRY,

	WPAN_CLUST_ENTRY_LIST_END
};

static const wpan_endpoint_table_entry_t sample_endpoints[] =
{
	// Endpoint/cluster for transparent serial and OTA command cluster
	{	WPAN_ENDPOINT_DIGI_DATA,		// endpoint
		WPAN_PROFILE_DIGI,				// profile ID
		NULL,							// endpoint handler
		NULL,							// ep_state
		0x0000,							// device ID
		0x00,							// version
		digi_data_clusters				// clusters
	},

	{ WPAN_ENDPOINT_END_OF_LIST }
};

// Set dynamically
//static xbee_command_list_context_t _xbee_atcmd_query_sleep_head;
extern xbee_command_list_context_t _xbee_atcmd_query_regs_head;
extern uint32_t radio_woke_up_time;
extern uint32_t os;
extern uint32_t ow;
extern uint8_t  db_val;
extern uint8_t  sm_val;
extern uint16_t transmit_status;

int receive_handler( xbee_dev_t *xbee, const void FAR *raw,
	uint16_t length, void FAR *context);

int _handle_transmit_status( xbee_dev_t *xbee,
	const void FAR *payload, uint16_t length, void FAR *context);

const xbee_dispatch_table_entry_t xbee_frame_handlers[] =
{
	XBEE_FRAME_HANDLE_LOCAL_AT,

	//XBEE_FRAME_HANDLE_ATND_RESPONSE,		// for processing ATND responses

	// this entry is for when ATAO is not 0
	XBEE_FRAME_HANDLE_RX_EXPLICIT,		// rx messages via endpoint table
	// next two entries are used when ATAO is 0
	//XBEE_FRAME_HANDLE_AO0_NODEID,			// for processing NODEID messages
	{ XBEE_FRAME_RECEIVE, 0, receive_handler, NULL },		// rx messages direct
    { XBEE_FRAME_TRANSMIT_STATUS, 0, _handle_transmit_status, NULL },
	XBEE_FRAME_HANDLE_AGGREGATE_ADRS_UPDATE,

	XBEE_FRAME_TABLE_END
};

uint16_t radio_mode;  // NORMAL, PASSTHROUGH
bool_t isRadioSleepEnabled;
uint32_t tx_interrupt_cnt = 0;
extern bool_t atag_received;

/**************************************************************************************************
*                                           LOCAL FUNCTION PROTOTYPES
**************************************************************************************************/

void _xbee_assert_reset_pin( xbee_dev_t *xbee, bool_t asserted );
void clear_spi_if (void);
Result_t set_dhdl_broadcast(void);

/**************************************************************************************************
*                                           GLOBAL API FUNCTIONS
**************************************************************************************************/

void clear_spi_if (void)
{
    uint8_t buffer[] = { 0x7e, 0x7e, 0x7e, 0x7e };  // lubricant
    // Send dummy bytes through SPI to fix startup problem
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_fr_set (void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x04, 0x08, 0x01, 0x46, 0x52, 0x5e };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_wr_set (void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x04, 0x08, 0x01, 0x57, 0x52, 0x4d };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_sm_zero_set (void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x05, 0x08, 0x01, 0x53, 0x4D, 0x00, 0x56 };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_sm_eight_set (void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x05, 0x08, 0x01, 0x53, 0x4D, 0x08, 0x4e };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_dh_set (void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x08, 0x08, 0x01, 0x44, 0x48, 0x00, 0x00, 0x00, 0x00, 0x6a };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_dl_set (void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x08, 0x08, 0x01, 0x44, 0x4c, 0x00, 0x00, 0xff, 0xff, 0x68 };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_so_set (void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x05, 0x08, 0x01, 0x53, 0x4f, 0x0a, 0x4a };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_clear_ms (void)
{
    uint8_t buffer[] = {  0x7e, 0x00, 0x08, 0x08, 0x01, 0x4d, 0x53, 0x00, 0x00, 0x56 };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_clear_sq (void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x06, 0x08, 0x01, 0x53, 0x51, 0x00, 0x00, 0x52 };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_reset(void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x04, 0x08, 0x01, 0x46, 0x52, 0x5e };
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_db_read(void)
{
    uint8_t buffer[] = { 0x7E, 0x00, 0x04, 0x08, 0x01, 0x44, 0x42, 0x70};
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_os_read(void)
{
    uint8_t buffer[] = { 0x7E, 0x00, 0x04, 0x08, 0x01, 0x4F, 0x53, 0x54};
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_ow_read(void)
{
    uint8_t buffer[] = { 0x7e, 0x00, 0x04, 0x08, 0x01, 0x4F, 0x57, 0x50};
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_ms_read(void)
{
    uint8_t buffer[] = { 0x7E, 0x00, 0x04, 0x08, 0x01, 0x4D, 0x53, 0x56};
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_sq_read(void)
{
    uint8_t buffer[] = { 0x7E, 0x00, 0x04, 0x08, 0x01, 0x53, 0x51, 0x52};
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_sm_read(void)
{
    uint8_t buffer[] = {0x7E, 0x00, 0x04, 0x08, 0x01, 0x53, 0x4D, 0x56};
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_dh_read(void)
{
    uint8_t buffer[] = {0x7E, 0x00, 0x04, 0x08, 0x01, 0x44, 0x48, 0x6A};
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

void atcmd_dl_read(void)
{
    uint8_t buffer[] = {0x7E, 0x00, 0x04, 0x08, 0x01, 0x44, 0x4C, 0x66};
    xbee_ser_write( &my_xbee.serport, buffer, sizeof(buffer) );
}

/**********************************************************************************************//**
 * @brief   Initialize XBee Radio Services
 *
 * @param   None
 *
 * @return  None
 ***************************************************************************************************/
Result_t radio_init( void )
{
	int status;

	// Reset XBEE!
	ASSERT_XBEE_RESET();
	milli_wait( 10 );
	DEASSERT_XBEE_RESET();
	milli_wait(100);

	isRadioSleepEnabled = FALSE;

	// initialize the serial and device layer for this XBee device
	if( xbee_dev_init( &my_xbee, &XBEE_SERPORT, NULL, &_xbee_assert_reset_pin ) != 0 ) {
		debug_printf( "xbee_dev_init: Failed to initialize device.\n");
		return RESULT_FAIL;
	}

#if 1
	// Initialize the WPAN layer of the XBee device driver.  This layer enables
	// endpoints and clusters.
	xbee_wpan_init( &my_xbee, sample_endpoints );
#endif

	clear_spi_if();  // send dummy bytes through SPI to handle power-up cycle

	// Initialize the AT Command layer for this XBee device and have the
	// driver query it for basic information (hardware version, firmware version,
	// serial number, IEEE address, etc.)
	if( xbee_cmd_init_device( &my_xbee ) != 0 ) {
		debug_printf( "Ack1! Unable to have the AT Commands at init time complete\n" );
	}

	do {
		xbee_dev_tick( &my_xbee );
		status = xbee_cmd_query_status( &my_xbee );
	} while (status == -EBUSY);

	if(status) {
		debug_printf( "Ack2! Error %d waiting for query to complete.\n", status);
	}
    return RESULT_SUCCESS;
}

/**********************************************************************************************//**
 * @brief   Main Radio layer tick (as fast as possible)
 *
 * @param   None
 *
 * @return  None
 ***************************************************************************************************/
void radio_tick( void )
{
    wpan_tick( &my_xbee.wpan_dev );
}

/**********************************************************************************************//**
 * @brief	Returns Radio Awake / Sleep based on pin
 *
 * @param   None
 *
 * @return  TRUE if radio is awake
 ***************************************************************************************************/
bool_t radio_isAwake( void )
{
	return( READ_XBEE_SLEEP_STAT_PIN() ? TRUE : FALSE );
}

/**********************************************************************************************//**
 * @brief	Set Radio Sleep Mode (SM)
 *
 * @param   isEnabled:  TRUE - enable, FALSE - disable
 *
 * @return
 ***************************************************************************************************/
void radio_enableSleepMode( bool_t isEnabled )
{
	if( isEnabled )
	{
		// Enable pin IRQ
		MAKE_XBEE_SLEEP_STAT_INT_RISING();
		SET_XBEE_SLEEP_STAT_INT();
		timerTickEvent.radio_woke_up = 0;
		isRadioSleepEnabled = TRUE;
	} else {
		// Disable pin IRQ
		CLR_XBEE_SLEEP_STAT_INT();
		isRadioSleepEnabled = FALSE;
	}
}

Result_t mesh_send_packet( const addr64 *ieee_addr_be, void *data, uint16_t length )
{
    uint8_t packet[270];
    uint8_t *pkt_pt;

    length += 0x0e;  // add in length of header
    pkt_pt = &packet[0];
    *pkt_pt++ = 0x7e;
    *pkt_pt++ = 0x7e;
    *pkt_pt++ = 0x7e;
    *pkt_pt++ = 0x7e;
    *pkt_pt++ = 0x7e;
    *pkt_pt++ = (uint8_t) (length >> 8);
    *pkt_pt++ = (uint8_t) (length & 0xff);
    *pkt_pt++ = (uint8_t) (0x10);  // transmit frame
    *pkt_pt++ = (uint8_t) (0x55);  // a non-zero ID for transmit
    *pkt_pt++ = (uint8_t) dest[0];
    *pkt_pt++ = (uint8_t) dest[1];
    *pkt_pt++ = (uint8_t) dest[2];
    *pkt_pt++ = (uint8_t) dest[3];
    *pkt_pt++ = (uint8_t) dest[4];
    *pkt_pt++ = (uint8_t) dest[5];
    *pkt_pt++ = (uint8_t) dest[6];
    *pkt_pt++ = (uint8_t) dest[7];
    *pkt_pt++ = (uint8_t) 0xff; // reserved, must be 0xff
    *pkt_pt++ = (uint8_t) 0xfe; // reserved, must be 0xfe
    *pkt_pt++ = (uint8_t) 0x00; // broadcast hops is maximum
    *pkt_pt++ = (uint8_t) 0x00; // transmit options
    memcpy(pkt_pt, data, (length+3));  // add also the 0x7e, the length in the copy
    packet[7+length] = _xbee_checksum( &packet[7], (length), 0xff);  // ignore SOF and length in CRC

#if 0
    uint16_t i;
    for (i = 0; i <= (7+length); i++) {  // include SOF, Checksum, and Length in the debug print
        debug_printf("%02x ", packet[i]);
        if ((i%16) == 15)
            debug_printf("\n");
    }
    debug_printf("\n");
#endif

    xbee_ser_write( &my_xbee.serport, packet, (length+8));

    return RESULT_SUCCESS;
}

/**********************************************************************************************//**
 * @brief	Handler for Aggregate Addressing Update frame (0x8E)
 *
 * @param
 *
 * @return  int - errno
 ***************************************************************************************************/
int xbee_aggregate_update_handler( xbee_dev_t *xbee, const void FAR *raw,
								   uint16_t length, void FAR *context )
{
	const XBeeAPI_AggregateAddrsUpdateFrm_t FAR *frame = raw;
	char addr[ADDR64_STRING_LENGTH];

	// this XBee frame handler (standard API) doesn't use the context parameter
	XBEE_UNUSED_PARAMETER( context );

	if (frame == NULL) {
		return -EINVAL;
	}

	if( length != sizeof( XBeeAPI_AggregateAddrsUpdateFrm_t ) ) {
		// invalid frame length
		return -EBADMSG;
	}

	// Update our new Aggregator Address
	debug_printf("\t\tNew Aggregator Address: %s\n", addr64_format( addr, (const addr64 *)&frame->new_addrs ) );
	memcpy((void *)&dest[0], (const void *)&frame->new_addrs, 8 );
	atag_received = TRUE;
	return 0;
}


// function receiving data on transparent serial cluster when ATAO != 0
int transparent_rx( const wpan_envelope_t FAR *envelope, void FAR *context)
{
#if 0 	// *** TBD
	transparent_dump( &envelope->ieee_address, envelope->payload,
		envelope->length);
#endif
	return 0;
}

// function receiving data on transparent serial cluster when ATAO=0
int receive_handler( xbee_dev_t *xbee, const void FAR *raw,
	uint16_t length, void FAR *context)
{
#if 0	// *** TBD
	const xbee_frame_receive_t FAR *rx_frame = raw;

	if (length >= offsetof( xbee_frame_receive_t, payload))
	{
		transparent_dump( &rx_frame->ieee_address, rx_frame->payload,
				length - offsetof( xbee_frame_receive_t, payload));
	}
#endif
	return 0;
}

// Process transmit status message
int _handle_transmit_status( xbee_dev_t *xbee,
	const void FAR *payload, uint16_t length, void FAR *context)
{

	   const xbee_frame_transmit_status_t FAR *frame = payload;
#ifdef BUILDTYPE_Debug
		// it may be necessary to push information up to user code so they know when
		// a packet has been received or if it didn't make it out
//	   debug_printf( "Transmit Status: id 0x%02x to 0x%04x retries=%d del=0x%02x disc=0x%02x\n",
//	      frame->frame_id,
//	      be16toh( frame->network_address_be), frame->retries, frame->delivery,
//	      frame->discovery);
#endif
	   transmit_status = frame->delivery << 8 | frame->discovery;
		return 0;
}

/**********************************************************************************************//**
 * @brief	Assert/Deassert nReset pin
 *
 * @param   None
 *
 * @return  None
 ***************************************************************************************************/
void _xbee_assert_reset_pin( xbee_dev_t *xbee, bool_t asserted )
{
	(void)xbee; // don't need this

	if( asserted ) {
		ASSERT_XBEE_RESET();
	} else {
		DEASSERT_XBEE_RESET();
	}
}

Result_t set_dhdl_broadcast(void)
{
    uint8_t timeout;

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_dh_set();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(200);
        }
    }
    if (timeout == 0)
        return RESULT_FAIL;

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_dl_set();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(200);
        }
    }
    if (timeout == 0)
        return RESULT_FAIL;

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_wr_set();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(200);
        }
    }
    if (timeout == 0)
        return RESULT_FAIL;
    return RESULT_SUCCESS;
}

Result_t mesh_soft_reset(void)
{
    uint8_t timeout;

    timeout = 20;
    atcmd_pending = TRUE;
    atcmd_fr_set();           // a soft reset will force a mesh timing sync for sleep timing
    while ((--timeout) && (atcmd_pending)) {
        radio_tick();
        milli_wait(100);
    }
    return (timeout == 0) ? RESULT_FAIL : RESULT_SUCCESS;
}

void mesh_init(void)
{
    uint8_t timeout;

    // hokey sequencing, after spending time with Digi Transport
    // If we are using SPI, and API1 mode, we can't deterministically come into
    // command mode on the mesh module.  The module will wake up from a reset,
    // send a sleep poll request frame to the base station, and when it gets a
    // response goes into sleep mode 8.  Can't send API1 frames to the module
    // until the mesh wakes up, up to fifteen minutes with the system out of
    // sleep.  So, we will reset the module, immediately send a ATCMD SM=0, and
    // see the response.  If successful, we are done!  If not, retry the reset,
    // ATCMD SM=0, etc.  Ultimately, we will beat the response from the base
    // station and catch the module before it goes to sleep.
    // The reset pulse can be as short as 1 ms to effect a reset, and we should
    // see a modem status frame indicating the successful reset.

    reset_event = FALSE;
    while (reset_event == FALSE) {
        radio_init();
        radio_enableSleepMode( TRUE );
//        debug_printf("Reset status: %s\n", (reset_event ? "TRUE" : "FALSE"));
    }
    reset_event = FALSE;

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_sm_zero_set();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_sm_read();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_dh_set();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_dh_read();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_dl_set();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_dl_read();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_os_read();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_ow_read();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_so_set();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    atcmd_pending = TRUE;
    while (atcmd_pending) {
        timeout = 20;
        atcmd_sm_eight_set();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
        if (timeout == 0) {
            break;
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_sm_read();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    timeout = 0;
    while (timeout == 0) {
        timeout = 20;
        atcmd_pending = TRUE;
        atcmd_wr_set();
        while ((--timeout) && (atcmd_pending)) {
            radio_tick();
            milli_wait(100);
        }
    }

    while (!mesh_soft_reset())
        ;
    milli_wait(150);

    // force a route discovery by sending a broadcast frame
    dest[0] = dest[1] = dest[2] = dest[3] = dest[4] = dest[5] = 0x00;
    dest[6] = dest[7] = 0xff;
}

/***************************************************************************************************
 *                                    INTERRUPT SERVICE ROUTINE
 **************************************************************************************************
 * @brief   Port1 IRQ Handler
 *
 * @param
 *
 * @return  its an ISR for gods sake!
 ***************************************************************************************************/
#pragma vector=PORT1_VECTOR
__interrupt void Port1_irq(void)
{
	switch(__even_in_range( P1IV, 0x10)) {
		case 0:
			break;                           // Vector  0:  No interrupt

		case P1IV_SLEEP_STAT:                // SLEEP pin from Digi (triggers on rising edge)
			timerTickEvent.radio_woke_up = 1;
			radio_woke_up_time = tick_GetUptime();
			tx_interrupt_cnt++;
			LPM3_EXIT;
			break;

		default:
			break;
	}
}

