/*****************************************************************************/
/**
 * Description: Implementations of 814 linklayer scan net.
 *
 * Date: 2021-11-10 19:04
 * LastEditors: Fan Hao
 * LastEditTime: 2022-07-16 17:04
 * Revisions: Rev 0.1
 * History:
 * [2022-07-16]: New branch for DiJian.
 * [2021-11-10]: Initial version.
 *
 * Copyright(c) 2021 Beijing Institute of Technology.
 * Lab of Communication and Networking
 *
 ******************************************************************************/

/***************************** Include Files *********************************/
#include <string.h>
#include "linklayer814.h"
#include "linklayer814_types.h"
#include "node_list.h"
#include "msg_def.h"
#include "../error.h"
#include "../common.h"
#include "../bit_array/bit_array.h"
#include "../bit_array/bit_macros.h"

/************************** Constant Definitions *****************************/
#define INIT_BEAM_NO 0
#define BEAM_PER_ANT 37
#define INIT_ANT_NO 0
#define ANT_NUM     6

#define BM_LSTEN_TIMEOUT 40

#define SQUENCE_SCAN_SLOTS      0

#define INTRA_INIT_SCAN_SLOTS    2
#define INTRA_BM_ADVT_SLOTS      4
#define INTRA_WAY2ACQ_SLOTS      1

#define INTER_INIT_SCAN_SLOTS    4
#define INTER_BM_ADVT_SLOTS      20
#define INTER_WAY2ACQ_SLOTS      2

//#define INIT_SCAN_SLOT_MAX 37
//#define BM_ADVT_SLOT_MAX   2
#define WAY2_ACQ_SLOT_MAX  120

/***************** Macros (Inline Functions) Definitions *********************/

/**************************** Type Definitions *******************************/
typedef enum {
	SCANPKT_NONE = 0,
	SCANPKT_INTER_BM = 1,
	SCANPKT_INTRA_BM = 2,
	SCANPKT_INTER_2WACQ = 3,
	SCANPKT_INTRA_2WACQ = 4
} SCANPKT_TYPE;
/************************** Variable Definitions *****************************/

/************************** Function Prototypes ******************************/
static void LockStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state);
static void BmrecvStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state);
static void W2ackStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state);

static void StageUpdate(L2_SCAN_FSM* scanFsmPtr, SCAN_STAGE newStage);
//static int32_t Check_W2AcqAck(NODE_ENTRY* nodeEntry);
static int32_t Scan_InitScanHandler(L2_INST* instPtr);
static int32_t Scan_BmAdvtHandler(L2_INST* instPtr);
static int32_t Scan_2WAcqHandler(L2_INST* instPtr);
static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType);
static int32_t ScanPkt_Packing(L2_INST* instPtr, SCANPKT_TYPE pktType);

/**
 * @brief   Update Init-Scan lock state.
 */
static inline void LockStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
	ASSERT(scanFsmPtr != NULL);
	DBG_INFO("Update BeamLockState from [%d] to [%d].\n",
			scanFsmPtr->isLocked, state);
	scanFsmPtr->isLocked = state;
	return;
}

/**
 * @brief   Update Base-info Advertisement state.
 */
static void BmrecvStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
	ASSERT(scanFsmPtr != NULL);
	DBG_INFO("Update BmRecvState from [%d] to [%d].\n",
			scanFsmPtr->isBmRcved, state);
	scanFsmPtr->isBmRcved = state;
	return;
}

/**
 * @brief   Update 2-Way Acquisition ack state.
 */
static void W2ackStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
	ASSERT(scanFsmPtr != NULL);
	DBG_INFO("Update W2acqAckState from [%d] to [%d].\n",
			scanFsmPtr->isAcked, state);
	scanFsmPtr->isAcked = state;
	return;
}

/*****************************************************************************/

/**
 * @brief   Update current Scan-Net stage
 *
 * @param   scanFsmPtr  A pointer to scan net fsm.
 *          newStage    New state.
 *
 * @return  NONE.
 *
 ******************************************************************************/
static void StageUpdate(L2_SCAN_FSM* scanFsmPtr, SCAN_STAGE newStage) {
	DBG_INFO("ScanNet current stage: %u -> ScanNet new stage: %u \n",
			scanFsmPtr->scanStage, newStage);
	scanFsmPtr->scanStage = newStage;
}
/*****************************************************************************/
/**
 * @brief   Packing Scan message to a bit array.
 *
 * @param   bitarr	pointer to bit array buffer.
 *          msgType	message type.
 *
 * @return	EPARAM
 *          SUCCESS
 *
 ******************************************************************************/
static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType) {
	ASSERT(bitarr != NULL);
	int32_t Status = SUCCESS;
	if (bitarr->num_of_bits < NS_MSG_LEN || msgType < L2_MSG_BM
			|| msgType > L2_MSG_SCAN_CONT_5) {
		return (EPARAM);
	}
	uint64_t dataChunk = 0;
	memset(bitarr->words, 0xAA, (size_t)(bitarr->num_of_bits / 8));
	switch (msgType) {
	case L2_MSG_BM:
		if (bitarr->num_of_bits < BM_MSG_LEN) {
			return EPARAM;
		}
		/* message type all zero */
		dataChunk = 0;
		FrameInsertChunk(bitarr, BM_MSG_TYPE_OFFSET, BM_MSG_TYPE_LEN, dataChunk);

		/* source node id */
		dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
		FrameInsertChunk(bitarr, BM_MSG_SRC_ADDR_OFFSET, BM_MSG_SRC_ADDR_LEN, dataChunk);

		/* net management node id */
		dataChunk = (L2_GetHeadId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
		FrameInsertChunk(bitarr, BM_MSG_NETMG_ADDR_OFFSET, BM_MSG_NETMG_ADDR_LEN, dataChunk);

		/* time unit number */
		dataChunk = L2_GetNextFrameNum(instPtr);
		FrameInsertChunk(bitarr, BM_MSG_TUNIT_NUM_OFFSET, BM_MSG_TUNIT_NUM_LEN, dataChunk);

		/* frame number */
		dataChunk = L2_GetNextSlotNum(instPtr) / L2_SLOTS_PER_SUBFRAME;
		FrameInsertChunk(bitarr, BM_MSG_TFRAME_NUM_OFFSET, BM_MSG_TFRAME_NUM_LEN, dataChunk);

		/* mini slot number, 20 mini slots per frame. */
		dataChunk = L2_GetNextSlotNum(instPtr) % L2_SLOTS_PER_SUBFRAME;
		FrameInsertChunk(bitarr, BM_MSG_TSLOT_NUM_OFFSET, BM_MSG_TSLOT_NUM_LEN, dataChunk);

		/* FIXME : Only for test */
		/* longitude. */
		dataChunk = 203;
		FrameInsertChunk(bitarr, BM_MSG_LONGITUDE_OFFSET, BM_MSG_LONGITUDE_LEN, dataChunk);

		/* latitude. */
		dataChunk = 589;
		FrameInsertChunk(bitarr, BM_MSG_LATITUDE_OFFSET, BM_MSG_LATITUDE_LEN, dataChunk);

		/* Height. */
		dataChunk = 589;
		FrameInsertChunk(bitarr, BM_MSG_HEIGHT_OFFSET, BM_MSG_HEIGHT_LEN, dataChunk);

		/* East. */
		dataChunk = 19;
		FrameInsertChunk(bitarr, BM_MSG_EAST_SPEED_OFFSET, BM_MSG_EAST_SPEED_LEN, dataChunk);

		/* North. */
		dataChunk = 30;
		FrameInsertChunk(bitarr, BM_MSG_NORTH_SPEED_OFFSET, BM_MSG_NORTH_SPEED_LEN, dataChunk);

		/* Upper. */
		dataChunk = 69;
		FrameInsertChunk(bitarr, BM_MSG_UPPER_SPEED_OFFSET, BM_MSG_UPPER_SPEED_LEN, dataChunk);

		/* Append CRC */
		L2_CrcAppend((uint8_t *) bitarr->words, BM_MSG_CRC_OFFSET / 8, BM_MSG_CRC_OFFSET / 8, L2_CRC8_31);

		break;
	case L2_MSG_SCAN_INIT:
		if (bitarr->num_of_bits < NS_MSG_LEN) {
			return EPARAM;
		}
		/* Sequence tag */
		dataChunk = 0;
		FrameInsertChunk(bitarr, NS_MSG_INI_SER_TAG_OFFSET, NS_MSG_INI_SER_TAG_LEN, dataChunk);

		/* word tag */
		dataChunk = 3;
		FrameInsertChunk(bitarr, NS_MSG_INI_WTAG_OFFSET, NS_MSG_INI_WTAG_LEN, dataChunk);

		/* message tag */
		dataChunk = 28;
		FrameInsertChunk(bitarr, NS_MSG_INI_MTAG_OFFSET, NS_MSG_INI_MTAG_LEN, dataChunk);

		/* message sub tag */
		dataChunk = 0;
		FrameInsertChunk(bitarr, NS_MSG_INI_STAG_OFFSET, NS_MSG_INI_STAG_LEN, dataChunk);

		/* message length tag */
		dataChunk = 5;
		FrameInsertChunk(bitarr, NS_MSG_INI_LEN_OFFSET, NS_MSG_INI_LEN_LEN, dataChunk);

		/* message type */
		dataChunk = (L2_GetNodeId(instPtr) == HEAD_NODE) ? 0 : 1;
		FrameInsertChunk(bitarr, NS_MSG_INI_TYPE_OFFSET, NS_MSG_INI_TYPE_LEN, dataChunk);

		/* source node id */
		dataChunk = (L2_GetNodeId(instPtr) & 0x1F)
					| (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
		FrameInsertChunk(bitarr, NS_MSG_INI_SRC_ADDR_OFFSET, NS_MSG_INI_SRC_ADDR_LEN, dataChunk);

		/* destination node id */
		dataChunk = (L2_GetNodeId(instPtr) == HEAD_NODE) ?
						(BROADCAST_ID | (BROADCAST_ID << 5)) 
						: (L2_GetHeadId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
		FrameInsertChunk(bitarr, NS_MSG_INI_LEN_OFFSET, NS_MSG_INI_LEN_LEN, dataChunk);
		if (L2_GetNodeType(instPtr) == HEAD_NODE) {
			/* current net stage */
			switch (instPtr->mainState) {
			case INIT_STATE:
				dataChunk = NET_INI_STAGE;
				break;
			case SEEK_STATE:
				dataChunk = NET_INTER_SEEK_STAGE;
				break;
			case NORMAL_STATE:
				dataChunk = NET_NORMAL_STAGE;
				break;
			default:
				dataChunk = NET_ERROR_STAGE;
				break;
			}
			FrameInsertChunk(bitarr, NS_MSG_INI_NET_STAGE_OFFSET, NS_MSG_INI_NET_STAGE_LEN, dataChunk);

			 // Begin Scan Time Word
			dataChunk = instPtr->netStartTime;
			FrameInsertChunk(bitarr, NS_MSG_INI_SCAN_TIME_OFFSET, NS_MSG_INI_SCAN_TIME_LEN, dataChunk);
		}
		break;
	case L2_MSG_SCAN_CONT_1:
		if (bitarr->num_of_bits < NS_MSG_LEN) {
			return EPARAM;
		}
		/* word tag */
		dataChunk = 2;
		FrameInsertChunk(bitarr, NS_MSG_CONT_1_WTAG_OFFSET, NS_MSG_CONT_1_WTAG_LEN, dataChunk);

		/* Continue word identification. */
		dataChunk = 1;
		FrameInsertChunk(bitarr, NS_MSG_CONT_1_CONTID_OFFSET, NS_MSG_CONT_1_CONTID_LEN, dataChunk);

		/* Time tag */
		dataChunk = 0x00FF00FF00FF;
		FrameInsertChunk(bitarr, NS_MSG_CONT_1_TIME_TAG_OFFSET, NS_MSG_CONT_1_TIME_TAG_LEN, dataChunk);

		/* East speed */
		dataChunk = 0xF0F0;
		FrameInsertChunk(bitarr, NS_MSG_CONT_1_EAST_SPEED_OFFSET, NS_MSG_CONT_1_EAST_SPEED_LEN, dataChunk);

		break;
	case L2_MSG_SCAN_CONT_2:
		if (bitarr->num_of_bits < NS_MSG_LEN) {
			return EPARAM;
		}
		/* word tag */
		dataChunk = 2;
		FrameInsertChunk(bitarr, NS_MSG_CONT_2_WTAG_OFFSET, NS_MSG_CONT_2_WTAG_LEN, dataChunk);

		/* Continue word identification. */
		dataChunk = 2;
		FrameInsertChunk(bitarr, NS_MSG_CONT_2_CONTID_OFFSET, NS_MSG_CONT_2_CONTID_LEN, dataChunk);

		/* North speed */
		dataChunk = 0x0F0F;
		FrameInsertChunk(bitarr, NS_MSG_CONT_2_NORTH_SPEED_OFFSET, NS_MSG_CONT_2_NORTH_SPEED_LEN, dataChunk);

		/* Upper speed */
		dataChunk = 0x0FF0;
		FrameInsertChunk(bitarr, NS_MSG_CONT_2_UPPER_SPEED_OFFSET, NS_MSG_CONT_2_UPPER_SPEED_LEN, dataChunk);

		/* Height */
		dataChunk = 0x00FF00;
		FrameInsertChunk(bitarr, NS_MSG_CONT_2_HEIGHT_OFFSET, NS_MSG_CONT_2_HEIGHT_LEN, dataChunk);

		break;
	case L2_MSG_SCAN_CONT_3:
		if (bitarr->num_of_bits < NS_MSG_LEN) {
			return EPARAM;
		}
		/* word tag */
		dataChunk = 2;
		FrameInsertChunk(bitarr, NS_MSG_CONT_3_WTAG_OFFSET, NS_MSG_CONT_3_WTAG_LEN, dataChunk);

		/* Continue word identification. */
		dataChunk = 3;
		FrameInsertChunk(bitarr, NS_MSG_CONT_3_CONTID_OFFSET, NS_MSG_CONT_3_CONTID_LEN, dataChunk);

		/* Longitude */
		dataChunk = 0x00FF00FF;
		FrameInsertChunk(bitarr, NS_MSG_CONT_3_LONGITUDE_OFFSET, NS_MSG_CONT_3_LONGITUDE_LEN, dataChunk);

		/* Latitude */
		dataChunk = 0xFF00FF00;
		FrameInsertChunk(bitarr, NS_MSG_CONT_3_LATITUDE_OFFSET, NS_MSG_CONT_3_LATITUDE_LEN, dataChunk);

		break;
	case L2_MSG_SCAN_CONT_4:
		if (bitarr->num_of_bits < NS_MSG_LEN) {
			return EPARAM;
		}
		/* word tag */
		dataChunk = 2;
		FrameInsertChunk(bitarr, NS_MSG_CONT_4_WTAG_OFFSET, NS_MSG_CONT_4_WTAG_LEN, dataChunk);

		/* Continue word identification. */
		dataChunk = 4;
		FrameInsertChunk(bitarr, NS_MSG_CONT_4_CONTID_OFFSET, NS_MSG_CONT_4_CONTID_LEN, dataChunk);

		/* Message key */
		dataChunk = 0xF0F0F0F0F0F0;
		FrameInsertChunk(bitarr, NS_MSG_CONT_4_KEY_OFFSET, NS_MSG_CONT_4_KEY_LEN, dataChunk);
		break;
	case L2_MSG_SCAN_CONT_5:
		if (bitarr->num_of_bits < NS_MSG_LEN) {
			return EPARAM;
		}
		/* word tag */
		dataChunk = 2;
		FrameInsertChunk(bitarr, NS_MSG_CONT_5_WTAG_OFFSET, NS_MSG_CONT_5_WTAG_LEN, dataChunk);

		/* Continue word identification. */
		dataChunk = 5;
		FrameInsertChunk(bitarr, NS_MSG_CONT_5_CONTID_OFFSET, NS_MSG_CONT_5_CONTID_LEN, dataChunk);

		/* Message key */
		dataChunk = 0x0F0F0F0F0F;
		FrameInsertChunk(bitarr, NS_MSG_CONT_5_KEY_OFFSET, NS_MSG_CONT_5_KEY_LEN, dataChunk);
		break;
	default:
		Status = EPARAM;
		break;
	}
	return Status;
}

static int32_t ScanPkt_Packing(L2_INST* instPtr, SCANPKT_TYPE pktType){
	uint32_t filedSize = 0;
	uint8_t* buff = instPtr->sendPktData;
	BIT_ARRAY* scanPkt = bit_array_create(L2_MSG_MAX);
	if (scanPkt == NULL) {
		DBG_ERROR("Failed to allocate memory for scanMsg.\n");
		return NUM_SLOTS_NONE;
	}
	switch (pktType) {
	case SCANPKT_INTRA_BM:
		if (ScanMsg_Packing(instPtr, scanPkt, L2_MSG_BM) != SUCCESS) {
			DBG_ERROR("Failed to packing L2_MSG_BM frame.\n");
			goto failed;
		}
		memcpy(buff, scanPkt->words, (size_t)(BM_MSG_LEN / 8));
		break;
	case SCANPKT_INTRA_2WACQ:
		for (int i = 0; i < ARRAY_SIZE(intraW2AcqFrameMsgs); i++) {
			if (ScanMsg_Packing(instPtr, scanPkt, intraW2AcqFrameMsgs[i]) == SUCCESS) {
				memcpy((uint8_t*)buff + (size_t)(i * (NS_MSG_LEN / 8)),
						scanPkt->words, (size_t)(NS_MSG_LEN / 8));
			}
			else {
				DBG_ERROR("Failed to packing intraW2AcqFrameMsgs[%d].\n", i);
				goto failed;
			}
			filedSize += NS_MSG_LEN;
			if (filedSize / 8 >= INTRA_W2ACQ_FRAME_LENGTH_MAX) {
				DBG_ERROR("Message is too long to be properly packed.\n");
				goto failed;
			}
		}
		L2_CrcAppend(buff, W2ACQ_FRAME_CRC_DLEN, W2ACQ_FRAME_CRC_POS, L2_CRC16_8005);
		break;
	default:
		DBG_ERROR("Scan Packet type [%d] is not surpported yet.\n", pktType);
		goto failed;
	}
	bit_array_free(scanPkt);
	return SUCCESS;
failed:
	bit_array_free(scanPkt);
	return FAILURE;
}

/*****************************************************************************/
/**
 * @brief   Member node scan net InitScan stage handler.
 *
 * @param   instPtr  A pointer to l2 instance
 *
 * @return used slot (5ms slot).
 *
 ******************************************************************************/
static int32_t Scan_InitScanHandler(L2_INST* instPtr) {
	ASSERT(instPtr != NULL);
	L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
	DBG_INFO("** Netsetup - InitScan Procedure (Attempts: %u)**\n", scanFsmPtr->scanCnt);

	if(scanFsmPtr->scanMode == INTRA_CLUSTER_MODE){
		if (scanFsmPtr->isLocked == true) {
			StageUpdate(scanFsmPtr, BM_ADVT_STAGE);
			/* Antenna setting (reserved.) */
			L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
		} /* Otherwise remain INIT_SCAN_STAGE */
		else {
			/* Antenna setting (reserved.) */
			L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
			scanFsmPtr->scanCnt++;
		}
	}
	else if(scanFsmPtr->scanMode == INTER_CLUSTER_MODE){
		if (scanFsmPtr->isLocked == true) {
			StageUpdate(scanFsmPtr, BM_ADVT_STAGE);
			/* Antenna setting (reserved.) */
			L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_BM_FRAME);
		} /* Otherwise remain INIT_SCAN_STAGE */
		else {
			/* Antenna setting (reserved.) */
			L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_SCAN_FRAME);
			scanFsmPtr->scanCnt++;
		}
	}
	else{
		DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
		return NUM_SLOTS_NONE;
	}
	return (NUM_SLOTS_SKIP);
}

/*****************************************************************************/

/**
 * @brief   Member node scan net BmAdvt stage handler.
 *
 * @param   scanFsmPtr  A pointer to scan net fsm.
 *          mainState    A pointer to main node state.
 *
 * @return used slot (5ms slot).
 *
 ******************************************************************************/
static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
	L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
	DBG_INFO("** Netsetup - BmAdvt Procedure (Timeout: %u) **\n", scanFsmPtr->bmTimeout);

	// if received a valid bm frame.
	if (scanFsmPtr->isBmRcved == true) {
		scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
		DBG_INFO("Received Bm frame, skipping 10ms for turn around...\n");
		// TODO: check functionality.
		StageUpdate(scanFsmPtr, WAY2_ACQ_STAGE);
		return NUM_SLOTS_10MS;
	}
	// no valid bm frame was received.
	else {
		/* if time out reached, go back to init scan stage. */
		if (scanFsmPtr->bmTimeout > BM_LSTEN_TIMEOUT) {
			if(scanFsmPtr->scanMode == INTRA_CLUSTER_MODE){
				/* Antenna setting (reserved.) */
				L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
			}
			else if(scanFsmPtr->scanMode == INTER_CLUSTER_MODE){
				/* Antenna setting (reserved.) */
				L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_SCAN_FRAME);
			}
			else{
				DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
				return NUM_SLOTS_NONE;
			}
			DBG_WARNING("BM timeout, restart net scan from INIT_SCAN_STAGE.\n");
			scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
			StageUpdate(scanFsmPtr, INIT_SCAN_STAGE);
		}
		else {
			if(scanFsmPtr->scanMode == INTRA_CLUSTER_MODE){
				/* Antenna setting (reserved.) */
				L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
			}
			else if(scanFsmPtr->scanMode == INTER_CLUSTER_MODE){
				/* Antenna setting (reserved.) */
				L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_BM_FRAME);
			}
			else{
				DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
				return NUM_SLOTS_NONE;
			}
			scanFsmPtr->bmTimeout++;
		}
		return NUM_SLOTS_SKIP;
	}
}

/*****************************************************************************/
/**
 * @brief   Scan net 2Way-Acq stage handler.
 *
 * @param   instPtr	a pointer to linklayer layer instance
 *
 * @return used slot (5ms slot).
 *
 ******************************************************************************/
static int32_t Scan_2WAcqHandler (L2_INST* instPtr){
	L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
	DBG_INFO("** Netsetup - 2WAcq Procedure (Timeout: %u) **\n", scanFsmPtr->w2acqTimeout);
	L2_FRAME_TYPE frameType = L2_NONE_FRAME;

	if(scanFsmPtr->scanMode == INTRA_CLUSTER_MODE){
		frameType = L2_INTRA_W2_ACQ_FRAME;
	}
	else if(scanFsmPtr->scanMode == INTER_CLUSTER_MODE){
		frameType = L2_INTER_W2_ACQ_FRAME;
	}
	else{
		DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
		return FAILURE;
	}

	// 2 way acq failed, back to init stage.
	if (scanFsmPtr->w2acqTimeout >= WAY2_ACQ_SLOT_MAX) {
		DBG_WARNING("2WAcq timeout, restart net scan from INIT_SCAN_STAGE.\n");
		scanFsmPtr->w2acqTimeout = 0; // reset counter for next scan.
		StageUpdate(scanFsmPtr, INIT_SCAN_STAGE);
	}
	else {
		scanFsmPtr->w2acqTimeout++; // increase counter.
	}
	L2_SetupNextSlot(instPtr, L2_TOR_RX, frameType);
	return SUCCESS;
}

/*****************************************************************************/

/**
 * @brief   Scan net packet handler.
 *
 * @param   instPtr	a pointer to linklayer layer instance
 *
 * @return	SUCCESS
 * 			error code
 *
 ******************************************************************************/
int32_t ScanNet_PktHandler(L2_INST* instPtr) {
	ASSERT(instPtr != NULL);
	uint32_t Status;
	L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
	uint64_t dataChunk;
	BIT_ARRAY* scanMsg = bit_array_create(L2_MSG_MAX);
	if (scanMsg == NULL) {
		free(scanMsg);
		return EALLOC;
	}
	switch (L2_GetLastFrameType(instPtr)) {
	case L2_INTER_SCAN_FRAME:
	case L2_INTRA_SCAN_FRAME:
		// refer to "linklayer814.c/L2_FrameStateUpdate"
		// INTRA_BM_FRAME could be received if current frame type is INTRA_BM_FRAME
		// in net setup stage(for member node).
		// When SCAN frame was considered as received, handle the frame as BM frame.
		break;
	case L2_INTER_BM_FRAME:
	case L2_INTRA_BM_FRAME: {
		for (int i = 0; i < ARRAY_SIZE(intraBmFrameMsgs); i++) {
			memcpy(scanMsg->words,
					instPtr->recvPktData + (size_t)(i * (BM_MSG_LEN / 8)),
					(size_t)(BM_MSG_LEN / 8));
			switch (intraBmFrameMsgs[i]) {
			case L2_MSG_BM:
				FrameGetChunk(scanMsg, BM_MSG_SRC_ADDR_OFFSET, BM_MSG_SRC_ADDR_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.NodeId = dataChunk & 0x000000000000001F;
				scanFsmPtr->BMrcvLog.ClusterId = (dataChunk>>5) & 0x000000000000001F;

				FrameGetChunk(scanMsg, BM_MSG_TUNIT_NUM_OFFSET, BM_MSG_TUNIT_NUM_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.frameNum = (uint32_t) dataChunk;

				FrameGetChunk(scanMsg, BM_MSG_TFRAME_NUM_OFFSET, BM_MSG_TFRAME_NUM_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.slotNum = (uint32_t)(dataChunk * L2_SLOTS_PER_SUBFRAME);
				
				FrameGetChunk(scanMsg, BM_MSG_TSLOT_NUM_OFFSET, BM_MSG_TSLOT_NUM_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.slotNum = scanFsmPtr->BMrcvLog.slotNum + (uint32_t) dataChunk;

				FrameGetChunk(scanMsg, BM_MSG_LONGITUDE_OFFSET, BM_MSG_LONGITUDE_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.Longitude = (uint32_t)(dataChunk);
				
				FrameGetChunk(scanMsg, BM_MSG_LATITUDE_OFFSET, BM_MSG_LATITUDE_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.Latitude = (uint32_t)(dataChunk);
				
				FrameGetChunk(scanMsg, BM_MSG_HEIGHT_OFFSET, BM_MSG_HEIGHT_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.Height = (uint32_t)(dataChunk);
				
				FrameGetChunk(scanMsg, BM_MSG_EAST_SPEED_OFFSET, BM_MSG_EAST_SPEED_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.EastSpeed = (uint32_t)(dataChunk);
				
				FrameGetChunk(scanMsg, BM_MSG_NORTH_SPEED_OFFSET, BM_MSG_NORTH_SPEED_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.NorthSpeed = (uint32_t)(dataChunk);
				
				FrameGetChunk(scanMsg, BM_MSG_UPPER_SPEED_OFFSET, BM_MSG_UPPER_SPEED_LEN, &dataChunk);
				scanFsmPtr->BMrcvLog.UpperSpeed = (uint32_t)(dataChunk);
				break;
			default:
				DBG_ERROR("UNSURPPORTED message type [%d]. \n", intraBmFrameMsgs[i]);
				Status = EIGNORE;
				goto failed;
			}
		}
		if(L2_GetNodeType(instPtr) != HEAD_NODE){
			if (IsNodeInList(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId) == false) {
				AppendNode(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId, (uint8_t) MEMBER_NODE);
			}
			// TODO: Update Node Info

			if ( L2_isCxn2Net(instPtr) == false ) {
				// TODO: Check
				L2_TimeAlign(instPtr, scanFsmPtr->BMrcvLog.frameNum, scanFsmPtr->BMrcvLog.slotNum);
			}
			// trigger flag
			BmrecvStateUpdate(scanFsmPtr, true);
		}
		else{
			DBG_ERROR("Node type [%d] is not supposed to receive BM frame", L2_GetNodeType(instPtr));
			Status = EUNEXP;
			goto failed;
		}
		break;
	}
	case L2_INTER_W2_ACQ_FRAME: 
	case L2_INTRA_W2_ACQ_FRAME: {
		for (int i = 0; i < ARRAY_SIZE(intraW2AcqFrameMsgs); i++) {
			memcpy(scanMsg->words,
					instPtr->recvPktData + (size_t)(i * (NS_MSG_LEN / 8)),
					(size_t)(NS_MSG_LEN / 8));
			switch (intraW2AcqFrameMsgs[i]) {
			case L2_MSG_SCAN_INIT:
				FrameGetChunk(scanMsg, NS_MSG_INI_SRC_ADDR_OFFSET, NS_MSG_INI_SRC_ADDR_LEN, &dataChunk);
				scanFsmPtr->W2ACQrcvLog.NodeId = dataChunk & 0x001F;
				scanFsmPtr->W2ACQrcvLog.ClusterId = (dataChunk>>5) & 0x0001F;

				FrameGetChunk(scanMsg, NS_MSG_INI_NET_STAGE_OFFSET, NS_MSG_INI_NET_STAGE_LEN, &dataChunk);
				switch (dataChunk) {
				case NET_INI_STAGE:
					scanFsmPtr->W2ACQrcvLog.NetStage = INIT_STATE;
					break;
				case NET_NORMAL_STAGE:
					scanFsmPtr->W2ACQrcvLog.NetStage = NORMAL_STATE;
					break;
				case NET_INTRA_SEEK_STAGE:
					scanFsmPtr->W2ACQrcvLog.NetStage = INTRA_SEEK_STATE;
					break;
				case NET_INTER_SEEK_STAGE:
					scanFsmPtr->W2ACQrcvLog.NetStage = INTER_SEEK_STATE;
					break;
				case NET_SEEK_STAGE:
					scanFsmPtr->W2ACQrcvLog.NetStage = SEEK_STATE;
					break;
				default:
					scanFsmPtr->W2ACQrcvLog.NetStage = NONE_STATE;
					DBG_WARNING("UNEXPECTED head node state.");
					break;
				}

				FrameGetChunk(scanMsg, NS_MSG_INI_SCAN_TIME_OFFSET, NS_MSG_INI_SCAN_TIME_LEN, &dataChunk);
				scanFsmPtr->W2ACQrcvLog.NetScanTime = (uint32_t)(dataChunk);
				break;
			case L2_MSG_SCAN_CONT_1:
				FrameGetChunk(scanMsg, NS_MSG_CONT_1_EAST_SPEED_OFFSET, NS_MSG_CONT_1_EAST_SPEED_LEN, &dataChunk);
				scanFsmPtr->W2ACQrcvLog.EastSpeed = (uint16_t)(dataChunk);
				break;
			case L2_MSG_SCAN_CONT_2:
				FrameGetChunk(scanMsg, NS_MSG_CONT_2_NORTH_SPEED_OFFSET, NS_MSG_CONT_2_NORTH_SPEED_LEN, &dataChunk);
				scanFsmPtr->W2ACQrcvLog.NorthSpeed = (uint16_t)(dataChunk);

				FrameGetChunk(scanMsg, NS_MSG_CONT_2_UPPER_SPEED_OFFSET, NS_MSG_CONT_2_UPPER_SPEED_LEN, &dataChunk);
				scanFsmPtr->W2ACQrcvLog.UpperSpeed = (uint16_t)(dataChunk);
				
				FrameGetChunk(scanMsg, NS_MSG_CONT_2_HEIGHT_OFFSET, NS_MSG_CONT_2_HEIGHT_LEN, &dataChunk);
				scanFsmPtr->W2ACQrcvLog.Height = (uint32_t)(dataChunk);
				break;
			case L2_MSG_SCAN_CONT_3:
				FrameGetChunk(scanMsg, NS_MSG_CONT_3_LONGITUDE_OFFSET, NS_MSG_CONT_3_LONGITUDE_LEN, &dataChunk);
				scanFsmPtr->W2ACQrcvLog.Longitude = (uint32_t)(dataChunk);

				FrameGetChunk(scanMsg, NS_MSG_CONT_3_LATITUDE_OFFSET, NS_MSG_CONT_3_LATITUDE_LEN, &dataChunk);
				scanFsmPtr->W2ACQrcvLog.Latitude = (uint32_t)(dataChunk);
				break;
			case L2_MSG_SCAN_CONT_4:
				break;
			case L2_MSG_SCAN_CONT_5:
				break;
			default:
				DBG_ERROR("UNSURPPORTED message type [%d]. \n", intraW2AcqFrameMsgs[i]);
				Status = EIGNORE;
				goto failed;
			}
		}
		// Update net start time
		if(L2_GetNodeType(instPtr) != HEAD_NODE){
			instPtr->netStartTime = scanFsmPtr->W2ACQrcvLog.NetScanTime;
		}

		if(L2_GetNodeType(instPtr) == HEAD_NODE){
			if (IsNodeInList(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId) == false) {
				AppendNode(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId, (uint8_t) MEMBER_NODE);
			}
			// TODO: Update Node Info
		}

		if(L2_GetNodeType(instPtr) != HEAD_NODE && scanFsmPtr->W2ACQrcvLog.NetStage != NONE_STATE){
			// sync state with head node
			L2_NODE_STATE newState = scanFsmPtr->W2ACQrcvLog.NetStage;
			L2_EVENT stateUpEvent = {
				.event = L2_EV_MAIN_STATE_UPDATE,
				.msg = &newState
			};
			AssertL2Event(instPtr, &stateUpEvent);

			// check if net setup finished.
			if(L2_GetMainState(instPtr) == INIT_STATE && L2_isCxn2Net(instPtr) == false){
				L2_RTNE_CONFIG newRtneConfig = {0};
				L2_EVENT rtneUpEvent = {
					.event = L2_ROUTINE_UPDATE,
					.msg = &newRtneConfig
				};
				if(scanFsmPtr->W2ACQrcvLog.NetStage == INIT_STATE){
					newRtneConfig.rtneTable = initTable;
					newRtneConfig.rtnePrd = ARRAY_SIZE(initTable);
					AssertL2Event(instPtr, &rtneUpEvent);
				}
				else if(scanFsmPtr->W2ACQrcvLog.NetStage == INTRA_SEEK_STATE){
					newRtneConfig.rtneTable = intraSeekTable;
					newRtneConfig.rtnePrd = ARRAY_SIZE(intraSeekTable);
					AssertL2Event(instPtr, &rtneUpEvent);
				}
				L2_EVENT netSetupDoneEvent = {
					.event = L2_EV_NET_SETUP_DONE,
					.msg = NULL
				};
				AssertL2Event(instPtr, &netSetupDoneEvent);
			}
		}
		break;
	}
	default: {
		DBG_ERROR("Cannot handler message type [%d] properly.\n", L2_GetLastFrameType(instPtr));
		Status = EIGNORE;
		goto failed;
	}
	}
	bit_array_free(scanMsg);
	return SUCCESS;
failed:
	bit_array_free(scanMsg);
	return Status;
}

/*****************************************************************************/
/**
 * @brief   Initialize ScanNet FSM, allocate memory for FSM instance.
 *
 * @param   scanFsmPtr a pointer to scan net  finite state machine
 *
 * @return  SUCCESS Initialization successful.
 *          EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
 *
 ******************************************************************************/
int32_t ScanNet_Init(L2_SCAN_FSM** scanFSMPtr) {
	*scanFSMPtr = (L2_SCAN_FSM *) malloc(sizeof(L2_SCAN_FSM));
	if (*scanFSMPtr == NULL) {
		DBG_ERROR("Failed to allocate memory for scanFsmPtr.\n");
		return EALLOC;
	}
	ScanNet_Reset(*scanFSMPtr);
	return (SUCCESS);
}


/*****************************************************************************/
/**
 * @brief  Reset ScanNet FSM
 *
 * @param   scanFsmPtr a pointer to scan net  finite state machine
 *
 * @return  NONE
 *
 ******************************************************************************/
void ScanNet_Reset(L2_SCAN_FSM* scanFSMPtr) {
	ASSERT(scanFSMPtr != NULL);
	scanFSMPtr->isLocked = false;
	scanFSMPtr->isBmRcved = false;
	scanFSMPtr->isAcked = false;
	scanFSMPtr->scanCnt = 0;
	scanFSMPtr->bmTimeout = 0;
	scanFSMPtr->w2acqTimeout = 0;
	scanFSMPtr->scanStage = NONE_STAGE;
	scanFSMPtr->scanMode = INTRA_CLUSTER_MODE;
	// FIXME: Only for test
	scanFSMPtr->BMrcvLog.Height = 0;
	scanFSMPtr->BMrcvLog.Latitude = 0;
	scanFSMPtr->BMrcvLog.Longitude = 0;
	scanFSMPtr->BMrcvLog.EastSpeed = 0;
	scanFSMPtr->BMrcvLog.NorthSpeed = 0;
	scanFSMPtr->BMrcvLog.UpperSpeed = 0;
	scanFSMPtr->BMrcvLog.frameNum = 0;
	scanFSMPtr->BMrcvLog.slotNum = 0;
	scanFSMPtr->W2ACQrcvLog.Height = 0;
	scanFSMPtr->W2ACQrcvLog.Latitude = 0;
	scanFSMPtr->W2ACQrcvLog.Longitude = 0;
	scanFSMPtr->W2ACQrcvLog.EastSpeed = 0;
	scanFSMPtr->W2ACQrcvLog.NorthSpeed = 0;
	scanFSMPtr->W2ACQrcvLog.UpperSpeed = 0;
	scanFSMPtr->W2ACQrcvLog.NetStage = NONE_STATE;
	scanFSMPtr->W2ACQrcvLog.NetScanTime = 0;
}

/*****************************************************************************/
/**
 * @brief  Destroy ScanNet FSM, free memory
 *
 * @param   scanFsmPtr a pointer to scan net  finite state machine
 *
 * @return  NONE
 *
 ******************************************************************************/
void ScanNet_Destroy(L2_SCAN_FSM* scanFSMPtr) {
	free(scanFSMPtr);
}

/*****************************************************************************/
/**
 * @brief   Execute a member node scan net procedure.
 *
 * @param   instPtr	a pointer to linklayer layer instance
 *
 * @return  used slot (5ms slot).
 *
 ******************************************************************************/
int32_t ScanNet_Proc(L2_INST* instPtr) {
	L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
	if (L2_GetNodeType(instPtr) == HEAD_NODE) {
		DBG_ERROR("Only member node needs to execute ScanNet_Proc. \n");
		return NUM_SLOTS_NONE;
	}
	int32_t Status;
	DBG_INFO("-------- ScanNet Process --------\n");
	switch (scanFsmPtr->scanStage) {
	case NONE_STAGE:
		/* Start Scan. */
		StageUpdate(scanFsmPtr, INIT_SCAN_STAGE);
		Status = Scan_InitScanHandler(instPtr);
		break;
	case INIT_SCAN_STAGE:
		Status = Scan_InitScanHandler(instPtr);
		break;
	case BM_ADVT_STAGE:
		Status = Scan_BmAdvtHandler(instPtr);
		break;
	case WAY2_ACQ_STAGE:
		Status = Scan_2WAcqHandler(instPtr);
		break;
	default:
		DBG_ERROR("Unexpected scan net stage: %d.\n", scanFsmPtr->scanStage);
		Status = FAILURE;
		break;
	}
	return Status;
}

/*****************************************************************************/

/**
 * @brief   Execute a scan net Routine procedure.
 *
 * @param   rtneEntry  Routine configuration
 *
 * @return  used slot (5ms slot).
 *
 ******************************************************************************/
int acqSend = 0;
int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry) {
	ASSERT(instPtr != NULL);
	L2_TOR_TYPE torState;
	int32_t Status;
	DBG_INFO("-------- ScanNet Routine --------\n");

	if(rtneEntry->slotSeq != 0){
		return SQUENCE_SCAN_SLOTS;
	}

	if(L2_GetNodeType(instPtr) != HEAD_NODE){
		if (rtneEntry->dst0 == L2_GetNodeId(instPtr) || rtneEntry->dst0 == BROADCAST_ID){
			torState = L2_TOR_RX;
		}
		else if (rtneEntry->src0 == L2_GetNodeId(instPtr) || rtneEntry->src0 == BROADCAST_ID){
			torState = L2_TOR_TX;
		}
		else {
			torState = L2_TOR_SUSPEND;
		}
	}
	else{
		if (rtneEntry->dst0 == L2_GetNodeId(instPtr)){
			torState = L2_TOR_RX;
		}
		else if (rtneEntry->src0 == L2_GetNodeId(instPtr)){
			torState = L2_TOR_TX;
		}
		else {
			torState = L2_TOR_SUSPEND;
		}
	}

	switch (rtneEntry->slotType) {
	case SLOT_INTER_SCAN_ACQ:
		// TODO:
		L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_SCAN_FRAME);
		break;

	case SLOT_INTRA_SCAN_ACQ:
		if (L2_GetNodeType(instPtr) != HEAD_NODE && torState == L2_TOR_TX) {
			DBG_ERROR("Only head node can send init-scan Frame.\n");
			Status = FAILURE;
			goto routine_err;
		}
		L2_SetupNextSlot(instPtr, torState, L2_INTRA_SCAN_FRAME);
		break;

	case SLOT_INTER_SCAN_BM:
		// TODO:
		L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_BM_FRAME);
		break;

	case SLOT_INTRA_SCAN_BM:
		if (torState == L2_TOR_TX) {
			if (L2_GetNodeType(instPtr) != HEAD_NODE) {
				DBG_ERROR("Only head node can send BM Frame.\n");
				Status = FAILURE;
				goto routine_err;
			}
			memset(instPtr->sendPktData, 0xAA, INTRA_BM_FRAME_LENGTH_MAX);
			if(ScanPkt_Packing(instPtr, SCANPKT_INTRA_BM) != SUCCESS) {
				Status = FAILURE;
				goto routine_err;
			}
		}
		/* Antenna setting (reserved.) */
		L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_INTRA_BM_FRAME);
		break;
	case SLOT_INTER_SCAN_2W:
		// TODO:
		L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_W2_ACQ_FRAME);
		break;
	case SLOT_INTRA_SCAN_2W:
		if (torState == L2_TOR_TX) {
			if(L2_GetNodeType(instPtr) != HEAD_NODE && L2_isDataAvail(instPtr) == false) {
				/* Antenna setting (reserved.) */
				L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTRA_W2_ACQ_FRAME);
				break;
			}
			/* Prepare Request/ACK data. */
			memset(instPtr->sendPktData, 0xAA, (size_t) INTRA_W2ACQ_FRAME_LENGTH_MAX);
			/* Pack message words */
			if(ScanPkt_Packing(instPtr, SCANPKT_INTRA_2WACQ) != SUCCESS) {
				Status = FAILURE;
				goto routine_err;
			}
			W2ackStateUpdate(instPtr->scanNetFSM, true);
			acqSend ++;
		}
		/* Antenna setting (reserved.) */
		L2_SetupNextSlot(instPtr, torState, L2_INTRA_W2_ACQ_FRAME);
		break;
	default:
		DBG_ERROR("Cannot start a routine for slot type [%d] properly.\n", rtneEntry->slotType);
		Status = FAILURE;
		goto routine_err;
	}
	return SUCCESS;
routine_err:
	return Status;
}