#include "api.h"
#include "fsm.h"
#include "app.h"
#include "HART\hart_lib.h"
#include "HART\hart_serial.h"
#include "DustNetworks\dn_whmt.h"

#include <drivers/pwr/adi_pwr.h> // for systick handling

//=========================== define =========================================

// network info
#define JOIN_DUTY_CYCLE_5          0x0D // 5%
#define JOIN_DUTY_CYCLE_50         0xF0 // 50%
#define JOIN_DUTY_CYCLE_100        0xFF // 100%

// this is only used ind 'api_setHartCompliantMode'
#define HART_COMPLIANT_MODE 1

//#define NETWORK_ID                0x1236  // AwiaNet
//uint8_t JoinKey[]    = {0x00, 0x00, 0x12, 0x36, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

//#define NETWORK_ID                0x8BCD    // WiHTEST
//uint8_t JoinKey[]    = {0x00, 0x00, 0x8B, 0xCD, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

#define NETWORK_ID                1229  // Dust networks
const uint8_t JoinKey[]    = {0x44, 0x55, 0x53, 0x54, 0x4E, 0x45, 0x54, 0x57, 0x4F, 0x52, 0x4B, 0x53, 0x52, 0x4F, 0x43, 0x4B};

// MAC adddress is normaly loaded from the mote's flash
// this MAC is used only with 'api_setMacAddress;
// format (3x header | 2x expanded device type | 3x UID)
const uint8_t macAddress[] = {0x00, 0x1B, 0x1E, 0xE2, 0x8E, 0x00, 0x00, 0xFF}; // 3xheader | 2*Exp.DeviceType | 3*ID

#define GATEWAY_ADDR         0xF981
#define NETWORK_MANAGER_ADDR 0xF980

//==== services
#define SERVICES_NUM            (HART_BURST_NUM + HART_EVENT_NUM)
#define SERVICE_ID_MAINTENANCE  0x80

#define SERVICE_STATE_INACTIVE  0x00
#define SERVICE_STATE_ACTIVE    0x01
#define SERVICE_STATE_REQUESTED 0x02

#define SERVICE_EVENT_CREATED_UPDATED 0
#define SERVICE_EVENT_DELETED         1
#define SERVICE_EVENT_REJECTED        2
#define SERVICE_EVENT_DELAYED         3

//==== application domains
#define APP_DOMAIN_PUBLISH        0x00
#define APP_DOMAIN_EVENT          0x01
#define APP_DOMAIN_MAINTENANCE    0x02
#define APP_DOMAIN_BLOCKTRANSFER  0x03

//===== Send
#define PACKET_PRIORITY_LOW		  0x00
#define PACKET_PRIORITY_MEDIUM	0x01
#define PACKET_PRIORITY_HIGH		0x02 // publishing data



//=========================== typedef =========================================

typedef void (*api_reply_callback)(void);

typedef struct
{
    uint8_t  domain;
    uint32_t dataPeriod;
    uint16_t destAddr;
    uint8_t  state;
} api_service_t;

typedef struct
{
    // C Library API
    api_reply_callback  replyCb;

    // receiving replies
    uint8_t             replyBuf[MAX_FRAME_LENGTH];

    // receiving notifications
    uint8_t             notifBuf[MAX_FRAME_LENGTH];

    // Sending responses to hart commands
    uint8_t             seqNum;
    uint16_t            srcAddr;
    uint8_t             tranType; // Reliable or unreliable
    uint8_t             payloadBuf[MAX_FRAME_LENGTH - DN_SEND_REQ_LEN]; // max length for dn_whmt_send command
    uint8_t             payloadLen; // there might be more then 1 cmd in the response, so the length off buffer must be global

    // Publishing data (bursts)
    hart_lib_wirelessPacket_t   hartBurstBuf;
    uint8_t                     serviceIdx;

    // Sending (forwarding from modem) hartPayload to mote
    // there are two buffers (payloadBuf and hartPayloadBuf) because, request from maintenace port
    // can come asyncronously and could possibly corrupt data in payloadBuf
    uint8_t             hartPayloadBuf[MAX_FRAME_LENGTH - DN_SEND_REQ_LEN]; // max length for dn_whmt_send command

    uint8_t             joinDutyCycle;

} api_vars_t;

//=========================== variables =======================================

api_vars_t            api_vars;
api_service_t         moteServices[SERVICES_NUM]; // mote originated services

//=========================== prototypes ======================================

// api
void api_getMacAddress_reply(void);
void api_getMoteStatus_reply(void);
void api_setJoinDutyCycle_reply(void);
void api_setNetworkID_reply(void);
void api_setJoinKey_reply(void);
void api_setHartDeviceInfo_reply(void);
void api_setHartDeviceStatus_reply(void);
void api_setHartCompliantMode_reply(void);
void api_setMacAddress_reply(void);
void api_setBatteryLife_reply(void);
void api_join_reply(void);
void api_getService_reply(void);
void api_setService_reply(void);
void api_send_reply(void);
void api_hartPayload_reply(void);
void api_disconnect_reply(void);
void api_reset_reply(void);

// maintenace port
void api_hartPayload_request(uint16_t cmdId, uint8_t* payload, uint8_t len);

// notifications
void api_notif_cb(uint8_t cmdId, uint8_t subCmdId);

// replies
void api_reply_cb(uint8_t cmdId);
void api_setReplyCallback(api_reply_callback cb);

//============================== api ==========================================

void api_init(void)
{
    // reset local variables
    memset(&api_vars, 0, sizeof(api_vars));

    api_vars.joinDutyCycle = JOIN_DUTY_CYCLE_100;
	
	// enable battery and accelerometer // TODO this shoul be called after loading MAC
        app_enableMeasurement(HART34_VARCODE_PRIMARY, true);
        //app_enableMeasurement(HART34_VARCODE_BAT_LIFE, true);

    // initialize the whmt module
    dn_whmt_init(
        api_notif_cb,                // notifCb
        api_vars.notifBuf,           // notifBuf
        sizeof(api_vars.notifBuf),   // notifBufLen
        api_reply_cb                 // replyCb
    );

    // this is used for API serial timeout
    NVIC_SetPriority(SysTick_IRQn, 10);    // Low priority
}

//===== reply_cb
/**
 This function is called whenever a reply is received through the SmartMesh.
 C Library. It calls the reply function that was armed at the start of the
 current event.
 */
void api_reply_cb(uint8_t cmdId)
{
    // stop systick
    SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk;

    // issue reply callback
    api_vars.replyCb();
}

//===== setReplyCallback
/**
 Set the callback function that the C Library will execute when the next reply
 is received and the reply buffer is ready to be parsed.
 */
void api_setReplyCallback(api_reply_callback cb)
{
    uint32_t clock;

    // save parameter
    api_vars.replyCb = cb;

    adi_pwr_GetClockFrequency(ADI_CLOCK_HCLK, &clock);
    SysTick_Config((clock / 1000) * SERIAL_RESPONSE_TIMEOUT_MS);
}

//===== response_timeout
/**
 This event is scheduled after each mote API command is sent, effectively
 placing a timeout for the mote to reply.
 */
//void api_response_timeout(void)
void SysTick_Handler(void)
{
    // issue cancel command
    dn_whmt_cancelTx();

    // stop SysTick
    SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk;

    // print error
    ERR_MSG("Serial timeout occured\n");

    // reset all serial connections
    fsm_enterState(2000, FSM_STATE_START);
}

//=== getMacAddr
void api_getMacAddress(void)
{
    //print status
    DEBUG_MSG("Get MAC addr\n");

    // arm callback
    api_setReplyCallback(api_getMacAddress_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_getNVParameter_macAddress(
                (dn_whmt_getNVParameter_macAddress_rpt*)(api_vars.replyBuf)
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_getMacAddress);
}

void api_getMacAddress_reply(void)
{
    // parse reply
    uint8_t pollingAddr;
    dn_whmt_getNVParameter_macAddress_rpt* reply = (dn_whmt_getNVParameter_macAddress_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success

        DEBUG_MSG("Mac: %02X-%02X-%02X-%02X-%02X-%02X-%02X-%02X ",
                  reply->macAddr[0],reply->macAddr[1],reply->macAddr[2],reply->macAddr[3],reply->macAddr[4],reply->macAddr[5],reply->macAddr[6],reply->macAddr[7]);

        // Check if MAC address is HART compliant. This should indicate that all NV properties are properly set. !!!
        if( (reply->macAddr[0] != 0x00) | (reply->macAddr[1] != 0x1B) | (reply->macAddr[2] != 0x1E))
        {
            ERR_MSG("\nInvalid MAC addr!!!\nMote's Non-volatile memory must be initialized before running this code.\nExiting...\n\n");
            break;
        }
				else
				    DEBUG_MSG("OK!\n");

        

        // save MAC and get polling addr
        pollingAddr = hart_lib_init(reply->macAddr);

        // initialize the serial connection
        hart_serial_init(
            reply->macAddr,
            pollingAddr,
            api_hartPayload_request
        );

        // choose next step
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_getMoteStatus);

        break;

    case MOTE_RC_READ_FAIL:
    case MOTE_RC_LOW_VOLTAGE:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//=== getMoteStatus
void api_getMoteStatus(void)
{
    //print status
    DEBUG_MSG("Mote Status\n");

    // arm callback
    api_setReplyCallback(api_getMoteStatus_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_getParameter_moteStatus(
                (dn_whmt_getParameter_moteStatus_rpt*)(api_vars.replyBuf)
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_getMoteStatus);

}

void api_getMoteStatus_reply(void)
{
    // parse reply
    dn_whmt_getParameter_moteStatus_rpt* reply = (dn_whmt_getParameter_moteStatus_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // TODO update config change counter
        switch (reply->state)
        {

        case MOTE_STATE_IDLE:

            // Init the mote and start join
            fsm_enterState(MIN_TX_INTERPACKET_DELAY_MS, FSM_STATE_JOINING);
            break;

        // no action on these states
        // they will be handled in next event notif.
        case MOTE_STATE_INIT:
        case MOTE_STATE_SEARCHING:
        case MOTE_STATE_NEGOTIATING:
				case MOTE_STATE_CONNECTED:
            DEBUG_MSG("State %d\n", reply->state);
            break;
       
        case MOTE_STATE_OPERATIONAL:

            // load table of enabled services from mote
            fsm_enterState(MIN_TX_INTERPACKET_DELAY_MS, FSM_STATE_CONNECTED);
            break;

        case MOTE_STATE_SUSPENDED    :

            // turn of all bursts and idle
            fsm_enterState(MIN_TX_INTERPACKET_DELAY_MS, FSM_STATE_SUSPENDED);
            break;

        // unexpected mote states
        case MOTE_STATE_DISCONNECTED :
        case MOTE_STATE_RADIO_TEST   :
        case MOTE_STATE_LISTEN       :
            ERR_MSG("Unexpected state: %d", reply->state);
            break;

        default:
            MOTE_RC_ERR();
            fsm_enterState(0, FSM_STATE_START);
            break;
        }
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}
//===== set join duty cycle
void api_setJoinDutyCycle(void)
{

    // print status
    DEBUG_MSG("Set Join Duty Cycle: %d\n", api_vars.joinDutyCycle);

    // arm callback
    api_setReplyCallback(api_setJoinDutyCycle_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_setParameter_joinDutyCycle(
                api_vars.joinDutyCycle,
                (dn_whmt_setParameter_joinDutyCycle_rpt*)(api_vars.replyBuf)
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setJoinDutyCycle);
}
void api_setJoinDutyCycle_reply(void)
{
    // parse reply
    dn_whmt_setParameter_joinDutyCycle_rpt* reply = (dn_whmt_setParameter_joinDutyCycle_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // next step
        fsm_scheduleEvent(CMD_PERIOD_MS, api_join);
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

// ==== set network ID
void api_setNetworkID(void)
{
    // print status
    DEBUG_MSG("Set Network ID: %d\n", NETWORK_ID);

    // arm callback
    api_setReplyCallback(api_setNetworkID_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_setNVParameter_networkId(
                DN_MEMORY_NV_ONLY,
                NETWORK_ID,
                (dn_whmt_setNVParameter_networkId_rpt*)(api_vars.replyBuf)
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setNetworkID);
}

void api_setNetworkID_reply(void)
{
    // parse reply
    dn_whmt_setNVParameter_networkId_rpt* reply = (dn_whmt_setNVParameter_networkId_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // choose next step
        fsm_scheduleEvent(CMD_PERIOD_MS, api_setJoinDutyCycle);
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_WRITE_FAIL:
    case MOTE_RC_LOW_VOLTAGE:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

// ==== set join key
void api_setJoinKey(void)
{
    // print status
    DEBUG_MSG("Set Join Key\n");

    // arm callback
    api_setReplyCallback(api_setJoinKey_reply);

    // copy join key from constant string
    memcpy(api_vars.payloadBuf, JoinKey, sizeof(JoinKey));

    // issue function
    if(DN_ERR_NONE != dn_whmt_setNVParameter_joinKey(
                DN_MEMORY_NV_ONLY,
                api_vars.payloadBuf,
                (dn_whmt_setNVParameter_joinKey_rpt*)(api_vars.replyBuf)
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setJoinKey);
}

void api_setJoinKey_reply(void)
{
    // parse reply
    dn_whmt_setNVParameter_joinKey_rpt* reply = (dn_whmt_setNVParameter_joinKey_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // choose next step
        // try to connect only to SMART MESH
        // in HART testing wait for 'connect' command
        fsm_enterState(0, FSM_STATE_JOINING);
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_WRITE_FAIL:
    case MOTE_RC_LOW_VOLTAGE:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//===== set parameter HART device info
void api_setHartDeviceInfo(void)
{
    // print status
    DEBUG_MSG("Set HART device info\n");

    // arm callback
    api_setReplyCallback(api_setHartDeviceInfo_reply);

    // build replies
    hart_lib_cmd(0,  NULL, 0, &api_vars.payloadBuf[0],  sizeof(api_vars.payloadBuf), HART_MASTER_NETWORK_MANAGER); // cmd0
    hart_lib_cmd(20, NULL, 0, &api_vars.payloadBuf[23], sizeof(api_vars.payloadBuf), HART_MASTER_NETWORK_MANAGER); // cmd20

    // issue function
    if(DN_ERR_NONE != dn_whmt_setParameter_hartDeviceInfo(
                &api_vars.payloadBuf[1],  // first byte is RC
                &api_vars.payloadBuf[24], // first byte is RC
                (dn_whmt_setParameter_hartDeviceInfo_rpt*) (api_vars.replyBuf)
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setHartDeviceInfo);
}

void api_setHartDeviceInfo_reply(void)
{
    // parse reply
    dn_whmt_setParameter_hartDeviceInfo_rpt* reply = (dn_whmt_setParameter_hartDeviceInfo_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // choose next step
        fsm_scheduleEvent(CMD_PERIOD_MS, api_setBatteryLife);
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//===== set parameter HART device status
void api_setHartDeviceStatus(void)
{
    // print status
    DEBUG_MSG("Set HART device status\n");

    // arm callback
    api_setReplyCallback(api_setHartDeviceStatus_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_setParameter_hartDeviceStatus(
                hart_lib_getStatus(HART_MASTER_NETWORK_MANAGER),                  // status
                hart_lib_getExtendedStatus(),                                     // extended satus
                (dn_whmt_setParameter_hartDeviceStatus_rpt*) (api_vars.replyBuf)  // reply buffer
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setHartDeviceStatus);
}

void api_setHartDeviceStatus_reply(void)
{
    // parse reply
    dn_whmt_setParameter_hartDeviceStatus_rpt* reply = (dn_whmt_setParameter_hartDeviceStatus_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // choose next step
        //fsm_scheduleEvent(CMD_PERIOD_MS, api_setJoinDutyCycle);

        // next step
        // TODO DEBUG ONLY
        // this should jump to join, but setting join duty cycle decreases join time
        if (fsm_getState() == FSM_STATE_JOINING)
            fsm_scheduleEvent(CMD_PERIOD_MS, api_setJoinDutyCycle);
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//===== set NV parameter HART compliant mode according to HART_COMPLIANT_MODE macro
void api_setHartCompliantMode(void)
{
    // print status
    DEBUG_MSG("Set NV HART compliant mode: %d\n", HART_COMPLIANT_MODE);

    // arm callback
    api_setReplyCallback(api_setHartCompliantMode_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_setNVParameter_compliantMode(
                DN_MEMORY_NV_ONLY,
                HART_COMPLIANT_MODE,
                (dn_whmt_setNVParameter_compliantMode_rpt*) (api_vars.replyBuf)
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setHartCompliantMode);
}

void api_setHartCompliantMode_reply(void)
{
    // parse reply
    dn_whmt_setNVParameter_compliantMode_rpt* reply = (dn_whmt_setNVParameter_compliantMode_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // choose next step
        fsm_scheduleEvent(CMD_PERIOD_MS, api_setMacAddress);
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_WRITE_FAIL:
    case MOTE_RC_LOW_VOLTAGE:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//===== set NV parameter MAC address
void api_setMacAddress(void)
{
    uint8_t pollingAddr;
    // print status
    DEBUG_MSG("Set NV MAC address\n");

    // arm callback
    api_setReplyCallback(api_setMacAddress_reply);

    // build MAC address
    memcpy(api_vars.payloadBuf, macAddress, sizeof(macAddress));

    // init hart_lib.c with this MAC
    pollingAddr = hart_lib_init(api_vars.payloadBuf);

    // initialize the serial connection
    hart_serial_init(
        api_vars.payloadBuf, // mac address
        pollingAddr,
        api_hartPayload_request
    );

    // issue function
    if(DN_ERR_NONE != dn_whmt_setNVParameter_macAddress(
                DN_MEMORY_NV_ONLY,
                api_vars.payloadBuf,
                (dn_whmt_setNVParameter_macAddress_rpt*) (api_vars.replyBuf)
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setMacAddress);
}

void api_setMacAddress_reply(void)
{
    // parse reply
    dn_whmt_setNVParameter_macAddress_rpt* reply = (dn_whmt_setNVParameter_macAddress_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // choose next step
        fsm_scheduleEvent(CMD_PERIOD_MS, api_setNetworkID);
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_WRITE_FAIL:
    case MOTE_RC_LOW_VOLTAGE:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//==== set parameter battery life

#define PWR_STATE_NOMINAL         0x00
#define PWR_STATE_LOW             0x01
#define PWR_STATE_CRITICALLY_LOW  0x02
#define PWR_STATE_RECHARGING_LOW  0x03
#define PWR_STATE_RECHARGING_HIGH 0x04

void api_setBatteryLife(void)
{
    uint16_t          bat_life;
    uint8_t           pwr_state = PWR_STATE_NOMINAL;
    app_deviceVar_t  *batLife;

    // print status
    DEBUG_MSG("Set battery life\n");

    // get battery state
    batLife = app_getDeviceVar(HART34_VARCODE_BAT_LIFE );

    // check if the value si valid
    if (batLife->enabled)
    {
        bat_life = (uint16_t)batLife->value;
        if (bat_life < 85 ) // cca. 66%
            pwr_state = PWR_STATE_LOW;
        if (bat_life < 30)  // cca. 12%
            pwr_state = PWR_STATE_CRITICALLY_LOW;

        // print status
        DEBUG_MSG("Set Battery Life: %d/255, Alarm: %d\n", bat_life, pwr_state);

        // arm callback
        api_setReplyCallback(api_setBatteryLife_reply);

        // issue function
        if(DN_ERR_NONE != dn_whmt_setParameter_batteryLife(
                    bat_life,  // battery life in days
                    pwr_state, // battery status
                    (dn_whmt_setParameter_batteryLife_rpt*) (api_vars.replyBuf)
                ))
            fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setBatteryLife);
    }
    else
    {
        // failed to obtain valid power data
        ERR_MSG("Battery error\n");
        fsm_scheduleEvent(CMD_PERIOD_MS, api_setHartDeviceStatus);
    }
}

void api_setBatteryLife_reply(void)
{
    // parse reply
    dn_whmt_setParameter_batteryLife_rpt* reply = (dn_whmt_setParameter_batteryLife_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // choose next step
        fsm_scheduleEvent(CMD_PERIOD_MS, api_setHartDeviceStatus);
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//===== join
void api_join(void)
{
    // print status
    DEBUG_MSG("Join\n");

    // arm callback
    api_setReplyCallback(api_join_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_join(
                (dn_whmt_join_rpt*)(api_vars.replyBuf)     // reply
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_join);
}

void api_join_reply(void)
{
    // parse reply
    dn_whmt_join_rpt* reply = (dn_whmt_join_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        break;

    case MOTE_RC_INVALID_STATE:
        ERR_MSG("Invalid state for join cmd\n");
        break;

    case MOTE_RC_INCOMPLETE_JOIN_INFO:
        ERR_MSG("Incomplete join info\n");
        break;

    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

// ==== get service
void api_getService(void)
{
    // print status
    DEBUG_MSG("Get Service\n");

    // arm callback
    api_setReplyCallback(api_getService_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_getParameter_service(
                api_vars.serviceIdx,                                    // service ID
                (dn_whmt_getParameter_service_rpt*)(api_vars.replyBuf)  // reply
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_getService);
}

void api_getService_reply(void)
{
    api_service_t* thisService              = &moteServices[api_vars.serviceIdx];
    uint8_t i;

    // parse reply
    dn_whmt_getParameter_service_rpt* reply = (dn_whmt_getParameter_service_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // print service info
        DEBUG_MSG("serviceId: %u, State: %u, Flags: %u, Domain: %u, Addr: 0x%X, time: %u\n",
                  reply->serviceId,
                  reply->serviceState,
                  reply->serviceFlags,
                  reply->appDomain,
                  reply->destAddr,
                  reply->time
                 );

        // save service information into local service table
        thisService             = &moteServices[reply->serviceId];
        thisService->state      = 0x7F & reply->serviceState;   // first bit is pending flag
        thisService->domain     = reply->appDomain;
        thisService->dataPeriod = reply->time;
        thisService->destAddr   = reply->destAddr;

        // read next service
        api_vars.serviceIdx++;
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_getService);

        break;

    case MOTE_RC_NOT_FOUND:
        // all active services are successfuly loaded to local copy (moteServices)
        // mote is connected and is ready to publish data
        // all other services are inactive
        for (i = api_vars.serviceIdx; i < SERVICES_NUM; i++)
            moteServices[i].state = SERVICE_STATE_INACTIVE;

        // write how many services were loaded
        // this should be zero if mote was reset
        DEBUG_MSG("%d mote originated services loaded\n", api_vars.serviceIdx);

        api_vars.serviceIdx = 0; // no next services

        // we are ready to request services for publishing
        hart_lib_burstEnable(
            HART_BURST_ALL,
            api_sendHartBurst,
            true
        );

        fsm_enterState(0, FSM_STATE_PUBLISHING);

        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

// ==== set service
void api_setService(void)
{
    api_service_t *thisService = &moteServices[api_vars.serviceIdx];

    // print status
    DEBUG_MSG("Set Service\n");

    // arm callback
    api_setReplyCallback(api_setService_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_setParameter_service(
                api_vars.serviceIdx,          //service ID
                0x01,                         //mote is source of data generated
                thisService->domain,          //app domain
                thisService->destAddr,        //destination address
                thisService->dataPeriod,      //data period in ms
                (dn_whmt_setParameter_service_rpt*)(api_vars.replyBuf)     // reply
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setService);
}

void api_setService_reply(void)
{
    // parse reply
    dn_whmt_setParameter_service_rpt* reply = (dn_whmt_setParameter_service_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // flag service state change
        moteServices[api_vars.serviceIdx].state = SERVICE_STATE_REQUESTED;
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_UNKNOWN_PARAM:
    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

// ==== send HART responses to manager and gateway
void api_sendHartResponse(void)
{
    uint8_t seqNum;

    // print status
    //DEBUG_MSG("Send HART response\n");

    // seqNum is used only with reliable packets
    if (api_vars.tranType == DN_TRANTYPE_ACKNOWLEDGED)
        seqNum = api_vars.seqNum;
    if (api_vars.tranType == DN_TRANTYPE_BESTEFFORT)
        seqNum = 0xFF;

    // arm callback
    api_setReplyCallback(api_send_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_send(
                api_vars.tranType,                        // tranType
                DN_TRANDIR_RESPONSE,                      // tranDir
                api_vars.srcAddr,                         // destination address: Manager
                SERVICE_ID_MAINTENANCE,                   // serviceId
                APP_DOMAIN_MAINTENANCE,                   // appDomain
                PACKET_PRIORITY_HIGH,                     // priority
                0xFFFF,																	  // reserved
                seqNum,						                        // sequence number
                api_vars.payloadLen,                      // payloadLen
                api_vars.payloadBuf,                      // payload
                (dn_whmt_send_rpt*)(api_vars.replyBuf)    // reply
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_sendHartResponse);
}

//==== send WirelessHART bursts
void api_sendHartBurst(void)
{
    uint8_t                    burstNum    = HART_BURST0;  // currently we support only one burst
    hart_lib_wirelessPacket_t *thisBuf     = &api_vars.hartBurstBuf;
    int16_t                    thisBufLen;
    api_service_t             *thisService = &moteServices[burstNum];

    // check if we can publish data in this FSM state
    if (fsm_getState() != FSM_STATE_PUBLISHING)
    {
        ERR_MSG("FSM not ready to publish bursts\n");
        return;
    }

    // Check if we support this service
    if (burstNum >= SERVICES_NUM)
    {
        MOTE_INVALID_SERVICE_ERR();
        return;
    }

    switch (thisService->state)
    {
    // if this service is inactive, request it
    case SERVICE_STATE_INACTIVE:

        api_vars.serviceIdx     = burstNum;
        thisService->domain     = APP_DOMAIN_PUBLISH;
        hart_lib_burstGetPeriod(burstNum, &thisService->dataPeriod);
        thisService->destAddr   = GATEWAY_ADDR;
        thisService->state      = SERVICE_STATE_REQUESTED;
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_setService);
        break;

    // if this service is requested, wait for its approval/denial (do not send now)
    // if service is active, continue
    case SERVICE_STATE_REQUESTED:

        DEBUG_MSG("Waiting for service\n");
        break;

    case SERVICE_STATE_ACTIVE:

        // print status
        DEBUG_MSG("Send HART burst %d\n", burstNum );

        // get the burst
        thisBufLen = hart_lib_burst(burstNum, thisBuf);

        // arm callback
        api_setReplyCallback(api_send_reply);

        // issue function
        if(DN_ERR_NONE != dn_whmt_send(
                    DN_TRANTYPE_BESTEFFORT,                   // tranType
                    DN_TRANDIR_RESPONSE,                      // tranDir
                    thisService->destAddr,                    // destination address
                    burstNum,                                 // serviceId
                    APP_DOMAIN_PUBLISH,                       // appDomain
                    PACKET_PRIORITY_MEDIUM,                   // priority: medium
                    0xFFFF,																	  // reserved
                    0xFF,						                          // sequence number - not used in best effort tranType
                    thisBufLen,                               // payloadLen
                    (uint8_t*)(thisBuf),             // payload
                    (dn_whmt_send_rpt*)(api_vars.replyBuf)    // reply
                ))
            fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_sendHartBurst);
        break;
    default:
        break;
    }
}

//==== this reply is common for sendHartBurst and sendHartResponse
void api_send_reply(void)
{
    // parse reply
    dn_whmt_send_rpt* reply = (dn_whmt_send_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        break;

    case MOTE_RC_INVALID_LEN:
    case MOTE_RC_UNKNOWN_CMD:
    case MOTE_RC_INVALID_VALUE:
    case MOTE_RC_NOT_FOUND:
    case MOTE_RC_NO_RESOURCES:
    case MOTE_RC_INVALID_STATE:
        MOTE_SEND_ERR();
        ERR_MSG("Send FAILED!!! RC=%d\n", reply->RC);
        break;

    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//==== this function is called when hartPayload comes from maintenance port
void api_hartPayload_request(uint16_t cmdId, uint8_t* payload, uint8_t len)
{
    hart_lib_wirelessCmd_t *thisCmd = (hart_lib_wirelessCmd_t*)api_vars.hartPayloadBuf;

    // check length of the message and current state of the FSM
    if( (sizeof(api_vars.hartPayloadBuf) < (len + 3)) || (!fsm_ready()))
    {
        ERR_MSG("HART payload err\n");
        hart_serial_errorReply(31,0); // cmd 31 for expanded cmds
        HART_PAYLOAD_ERR();           // increment error counter
    }
    else
    {
        // Format: 16bitCMD | dataLen | data
        dn_write_uint16_t(thisCmd->cmdId, cmdId); // command number
        thisCmd->dataLen = len;                   // length of data field
        memcpy(thisCmd->data, payload, len);      // data

        // issue the api command
        api_hartPayload();
    }
}

//==== forward commands from maintenance port to mote
void api_hartPayload(void)
{
    // print status
    DEBUG_MSG("HART payload\n");

    // arm callback
    api_setReplyCallback(api_hartPayload_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_hartPayload(
                api_vars.hartPayloadBuf[2] + 3,                // extract payload_len from HART formated payload
                api_vars.hartPayloadBuf,                       // payload
                (dn_whmt_hartPayload_rpt*)(api_vars.replyBuf)  // reply buf
            ))
    {
        // this means, that we are not able to respon within hart modem TIMEOUT
        // TODO: start delayed response sequence
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_hartPayload);
    }
}

void api_hartPayload_reply(void)
{
    // parse reply
    dn_whmt_hartPayload_rpt* reply = (dn_whmt_hartPayload_rpt*)api_vars.replyBuf;
    uint16_t                 receivedCmdId;
    hart_lib_wirelessCmd_t   toModemBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // send reply to maintenance port
        // copy to local buffer
        memcpy(&toModemBuf, reply->payload, reply->payloadLen);

        // Format: 16bitCMD | dataLen | data
        // get received command id
        dn_read_uint16_t(&receivedCmdId, toModemBuf.cmdId);

        // pass it to maintenance port
        hart_serial_txHartFrame(
            receivedCmdId,      // cmd
            toModemBuf.data,    // RC + data
            toModemBuf.dataLen  // length of data
        );
        break;

    case MOTE_RC_INVALID_VALUE:
    case MOTE_RC_INVALID_LEN:

    default:         // unexpected response code
        // TODO must send error response to HART modem!!!
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//==== disconnect
void api_disconnect(void)
{
    // print status
    DEBUG_MSG("Disconnect\n");

    // arm callback
    api_setReplyCallback(api_disconnect_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_disconnect(
                (dn_whmt_disconnect_rpt*) api_vars.replyBuf
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_disconnect);
}

void api_disconnect_reply(void)
{
    // parse reply
    dn_whmt_disconnect_rpt* reply = (dn_whmt_disconnect_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // next step
        // if disconnect was called before reset, then reset
        // TODO or reset after disconnect notf??????
        // if(fsm_getState() == FSM_STATE_RESETTING)

        break;

    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//==== reset
void api_reset(void)
{
    // print status
    DEBUG_MSG("Reset\n");

    // arm callback
    api_setReplyCallback(api_reset_reply);

    // issue function
    if(DN_ERR_NONE != dn_whmt_reset(
                (dn_whmt_reset_rpt*) api_vars.replyBuf
            ))
        fsm_scheduleEvent(MIN_TX_INTERPACKET_DELAY_MS, api_reset);
}

void api_reset_reply(void)
{
    // parse reply
    dn_whmt_reset_rpt* reply = (dn_whmt_reset_rpt*)api_vars.replyBuf;

    // evaluate the response code
    switch (reply->RC)
    {
    case MOTE_RC_OK: // success
        // TODO what next? reset the MCU? reset mote or MCU first?
        //fsm_enterState(0, FSM_STATE_RESETTING);
        break;

    default:         // unexpected response code
        MOTE_RC_ERR();
        fsm_enterState(0, FSM_STATE_START);
        break;
    }
}

//======================== notification handling ==============================

//===== notif_cb
/**
 This function is called whenever a notification is received through the
 SmartMesh C Library. The notification variables are than available through
 the notification buffer that can be cast to the correct type based on the
 given command ID (notification type).
 */

//==== mote events
#define MOTE_EVENT_BOOT                0x01
#define MOTE_EVENT_ALARM_CHANGED       0x02
#define MOTE_EVENT_TIME_CHANGED        0x04
#define MOTE_EVENT_JOIN_FAIL           0x08
#define MOTE_EVENT_DISCONNECTED        0x10
#define MOTE_EVENT_OPERATIONAL         0x20
#define MOTE_EVENT_CONFIG_CHANGED      0x40
#define MOTE_EVENT_JOIN_STARTED        0X100

void api_notif_cb(uint8_t cmdId, uint8_t subCmdId)  // subCmdId is not used in WirelessHART mote
{
    //==== parsing notification pointers
    dn_whmt_events_nt*             events_notif;
    //dn_whmt_timeIndication_nt*     timeIndication_notif;
    dn_whmt_dataReceived_nt*       dataReceived_notif;
    dn_whmt_serviceIndication_nt*  serviceIndication_notif;
    //dn_whmt_advReceived_nt*        advReceived_notif;
    dn_whmt_suspendStarted_nt*     suspendStarted_notif;

    //==== parsing data pointers
    // used in data received notification
    hart_lib_wirelessPacket_t*     pOut;           // pointer to response buffer
    hart_lib_wirelessCmd_t*        thisCmdOutput;  // pointer to currently processed cmd
    hart_lib_wirelessCmd_t*        thisCmdInput;   // pointer to this cmd output buffer
    uint8_t                        rdIdx;          // read index of datareceived payload
    uint16_t                       hartCmdId;      // HART cmd ID
    int16_t                        hartResponseLen;
    hart_lib_master_t              master;         // master which issued this request

    // used in service notification
    api_service_t*                 thisService;

    switch (cmdId)
    {
    case CMDID_TIMEINDICATION:
        // parse notification
        // timeIndication_notif = (dn_whmt_timeIndication_nt*)api_vars.notifBuf;
        DEBUG_MSG("Notif: TIME INDICATION\n");
        break;

    case CMDID_SERVICEINDICATION:
        // parse notification
        serviceIndication_notif = (dn_whmt_serviceIndication_nt*)api_vars.notifBuf;
        if (serviceIndication_notif->serviceId < SERVICES_NUM)
            thisService         = &moteServices[serviceIndication_notif->serviceId];
        else
        {
            MOTE_INVALID_SERVICE_ERR();
            break;
        }

        DEBUG_MSG("Notif: SERVICE INDICATION\neventcode=%d mgrRc=%d id=%d state=%d flags=%d domain=%d addr=0x%X time=%d\n",
                  serviceIndication_notif->eventCode,
                  serviceIndication_notif->netMgrCode,
                  serviceIndication_notif->serviceId,
                  serviceIndication_notif->serviceState,
                  serviceIndication_notif->serviceFlags,
                  serviceIndication_notif->appDomain,
                  serviceIndication_notif->destAddr,
                  serviceIndication_notif->time
                 );

        switch (serviceIndication_notif->eventCode)
        {
        case SERVICE_EVENT_CREATED_UPDATED:
            thisService->state      = SERVICE_STATE_ACTIVE;
            thisService->domain     = serviceIndication_notif->appDomain;
            thisService->dataPeriod = serviceIndication_notif->time;
            thisService->destAddr   = serviceIndication_notif->destAddr;

            break;

        case SERVICE_EVENT_DELETED:
            thisService->state      = SERVICE_STATE_INACTIVE;
            break;

        case SERVICE_EVENT_REJECTED:
            // TODO: request it again again
            thisService->state      = SERVICE_STATE_INACTIVE;
            break;

        case SERVICE_EVENT_DELAYED:
            thisService->state      = SERVICE_STATE_REQUESTED;
            break;
        default:
            break;
        }


        break;

    case CMDID_EVENTS:
        // parse notification
        events_notif = (dn_whmt_events_nt*)api_vars.notifBuf;
        /*
                switch (events_notif->state)
                {
                case MOTE_STATE_IDLE:
                    DEBUG_MSG("State: IDLE\n");
                    break;

                case MOTE_STATE_SEARCHING:
                    DEBUG_MSG("State: SEARCHING\n");
                    break;

                case MOTE_STATE_NEGOTIATING:
                    DEBUG_MSG("State: NEGOTIATING\n");
                    break;

                case MOTE_STATE_CONNECTED:
                    DEBUG_MSG("State: CONNECTED\n");
                    break;

                case MOTE_STATE_OPERATIONAL:
                    DEBUG_MSG("State: OPERATIONAL\n");
                    break;

                default: // unexpected mote state
                    MOTE_RC_ERR();
                    fsm_enterState(0, FSM_STATE_START);
                    break;
                }
        */
        switch (events_notif->events)
        {
        case MOTE_EVENT_BOOT:
            DEBUG_MSG("Event: BOOT\n");

            switch (fsm_getState())
            {
            case FSM_STATE_START:
                // initialization is in process - DO NOTHING
                //fsm_enterState(0, FSM_STATE_NOT_INITIALIZED);
                break;

            case FSM_STATE_DISCONNECTED:
                fsm_enterState(0, FSM_STATE_DISCONNECTED);
                break;

            case FSM_STATE_CONNECTED:
            case FSM_STATE_PUBLISHING:
            case FSM_STATE_SUSPENDED:
            case FSM_STATE_JOINING:
            //       case FSM_STATE_REQ_SERVICE:
            case FSM_STATE_PRE_JOIN:
                fsm_enterState(0, FSM_STATE_JOINING);
                break;

            // we issued the reset command, reset MCU as well
            case FSM_STATE_RESETTING:
                fsm_enterState(0, FSM_STATE_START);
                break;

            case FSM_STATE_SEND_FAILED:
                break;

            default:
                //increment error counter
                FSM_INVALID_STATE_ERR();
                ERR_MSG("FSM ERR\n");
                fsm_enterState(0, FSM_STATE_RESETTING);
                break;
            }

        case MOTE_EVENT_ALARM_CHANGED:
            DEBUG_MSG("Event: ALARM CHANGED Alarms: 0x%X\n",
                      events_notif->moteAlarms // errors: NV flash, low voltage, factory-programed settings, not ready to send
                     );
            break;

        case MOTE_EVENT_TIME_CHANGED:
            DEBUG_MSG("Event: TIME CHANGED\n");
            break;

        case MOTE_EVENT_OPERATIONAL:
            DEBUG_MSG("Event: OPERATIONAL\n");

            fsm_enterState(0, FSM_STATE_CONNECTED);
            break;

        case MOTE_EVENT_CONFIG_CHANGED:
            DEBUG_MSG("Event: CONFIG CHANGED\n");

            // increment the Configuration Change Counter
            hart_lib_incCfgChangeCnt();
            break;

        case MOTE_EVENT_JOIN_FAIL:
            ERR_MSG("Event: JOIN FAIL\n");

            // retry TODO

            // return to prejoin state
            fsm_enterState(0, FSM_STATE_PRE_JOIN);
            break;

        case MOTE_EVENT_DISCONNECTED:
            DEBUG_MSG("Event: DISCONNECTED\n");

            // mote will not try to join again
            // fsm_enterState(0, FSM_STATE_DISCONNECTED);
            break;

        case MOTE_EVENT_JOIN_STARTED:
            DEBUG_MSG("Event: JOIN STARTED\n");
            break;

        default:
            MOTE_INVALID_EVENT_ERR();
            break;
        }
        break;

    case CMDID_DATARECEIVED:

        // parse notification
        dataReceived_notif = (dn_whmt_dataReceived_nt*) api_vars.notifBuf;

        //DEBUG_MSG("Notif: DATA RECEIVED length: %d\n",
        //          dataReceived_notif->pktLength
        //         );

        if(
            dataReceived_notif->tranDir  == DN_TRANDIR_REQUEST //&&
            //(dataReceived_notif->tranType == DN_TRANTYPE_ACKNOWLEDGED) // Broadcast messages are not acknowledged
        )
        {
            // Manager issued request

            // store paramteres - this parameters must be used when replying
            api_vars.seqNum   = dataReceived_notif->seqNum;
            api_vars.srcAddr  = dataReceived_notif->srcAddr;
            api_vars.tranType = dataReceived_notif->tranType;

            // we need this information for proper status byte ;)
            if(dataReceived_notif->srcAddr == GATEWAY_ADDR)
                master = HART_MASTER_GATEWAY;
            else
                master = HART_MASTER_NETWORK_MANAGER;

            // Build response: Status|Extended Status|Cmd1|Length1(includes response code)|RC1|Data1|Cmd2|Length2|RC2|Data2...
            pOut  = (hart_lib_wirelessPacket_t*) api_vars.payloadBuf;

            // build header
            pOut->status        = hart_lib_getStatus(master);
            pOut->extStatus     = hart_lib_getExtendedStatus();
            api_vars.payloadLen = 2; // 2x status written
            rdIdx               = 2; // start at 3rd byte; ignore received status and extended status from master

            // repeat this for every received HART command
            do
            {
                // init the cmd pointers
                thisCmdInput  = (hart_lib_wirelessCmd_t*) &dataReceived_notif->data[rdIdx];
                thisCmdOutput = (hart_lib_wirelessCmd_t*) &api_vars.payloadBuf[api_vars.payloadLen];

                // decode HART cmd id
                dn_read_uint16_t(&hartCmdId, thisCmdInput->cmdId);
                DEBUG_MSG("WHART cmd %d received, len: %d\n",
                          hartCmdId,
                          thisCmdInput->dataLen);

                // build HART reply
                hartResponseLen = hart_lib_cmd(
                                      hartCmdId,                        // command number
                                      thisCmdInput->data,               // input hart data
                                      thisCmdInput->dataLen,            // input hart data length
                                      thisCmdOutput->data,              // RC|data
                                      sizeof(api_vars.payloadBuf) - api_vars.payloadLen, // free space in buffer
                                      master                            // network manager or gateway
                                  );

                // check the hart_lib output
                // we must respond if the packet was reliable!!!
                if(hartResponseLen > HART_LIB_DO_NOT_ANSWER)
                {
                    dn_write_uint16_t(thisCmdOutput->cmdId, hartCmdId);
                    thisCmdOutput->dataLen = hartResponseLen;

                    // increment idexes
                    api_vars.payloadLen += (thisCmdOutput->dataLen + 3); // payload + header (2xCMD, 1xLen)
                    rdIdx               += (thisCmdInput->dataLen  + 3);
                }
                else
                {
                    // send empty packet SmartMesh WirelessHART User's Guide p.123
                    ERR_MSG("NOT answering\n");
                    api_vars.payloadLen = 0;
                    break; // TODO we should process valid commads!!!
                }
            }
            while(rdIdx < dataReceived_notif->pktLength);

            // issue send fucntion
            api_sendHartResponse();
        }
        else
        {
            // manager Responsed to request from mote
            // this shoudl never happen
            ERR_MSG("response message\n");
        }
        break;

    case CMDID_ADVRECEIVED:
        // parse notification
        //advReceived_notif = (dn_whmt_advReceived_nt*) api_vars.notifBuf;
        //DEBUG_MSG("Notif: ADVERTISEMENT INDICATION NetId: 0x%X",
        //          advReceived_notif->netId
        //         );
        DEBUG_MSG("Notif: ADVERTISEMENT RECEIVED\n");
        break;

    case CMDID_SUSPENDSTARTED:
        // parse notification
        suspendStarted_notif = (dn_whmt_suspendStarted_nt*) api_vars.notifBuf;

        DEBUG_MSG("Notif: SUSPEND STARTED duration: %d\n",
                  suspendStarted_notif->duration
                 );

        // enter suspended state (turns off bursts and all radio communication)
        fsm_enterState(0, FSM_STATE_SUSPENDED);
        break;

    default:
        MOTE_INVALID_NOTIF_ERR();
        break;
    }
}
