/*****************************************************************************/
/**
 * 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 BM_LISTEN_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->isBmRecvd, state);
	scanFsmPtr->isBmRecvd = 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;
	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: dummy data, 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) == L2_NODE_HEAD) ? 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) == L2_NODE_HEAD) ? (L2_BROADCAST_ID | (L2_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) == L2_NODE_HEAD)
		{
			/* current net stage */
			switch (instPtr->mainState)
			{
			case L2_INIT_STATE:
				dataChunk = NET_INI_STAGE;
				break;
			case L2_SEEK_STATE:
				dataChunk = NET_INTER_SEEK_STAGE;
				break;
			case L2_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 FAILURE;
	}
	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->scanFSM;
	DBG_INFO("** Netsetup - InitScan Procedure (Attempts: %u)**\n", scanFsmPtr->scanCnt);

	if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
	{
		if (scanFsmPtr->isLocked == true)
		{
			StageUpdate(scanFsmPtr, L2_SCAN_BMADVT_STAGE);
			/* Antenna setting (reserved.) */
			L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
		} /* Otherwise remain L2_SCAN_INIT_STAGE */
		else
		{
			/* Antenna setting (reserved.) */
			L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
			scanFsmPtr->scanCnt++;
		}
	}
	else if (scanFsmPtr->scanMode == L2_SCAN_INTER_MODE)
	{
		if (scanFsmPtr->isLocked == true)
		{
			StageUpdate(scanFsmPtr, L2_SCAN_BMADVT_STAGE);
			/* Antenna setting (reserved.) */
			L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_BM_FRAME);
		} /* Otherwise remain L2_SCAN_INIT_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 FAILURE;
	}
	return SUCCESS;
}

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

/**
 * @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->scanFSM;
	DBG_INFO("** Netsetup - BmAdvt Procedure (Timeout: %u) **\n", scanFsmPtr->bmTimeout);

	// if received a valid bm frame.
	if (scanFsmPtr->isBmRecvd == true)
	{
		scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
		DBG_INFO("Received Bm frame, skipping 10ms for turn around...\n");
		// TODO: check functionality.
		StageUpdate(scanFsmPtr, L2_SCANT_2WAYACQ_STAGE);
		return SUCCESS;
	}
	// no valid bm frame was received.
	else
	{
		/* if time out reached, go back to init scan stage. */
		if (scanFsmPtr->bmTimeout > BM_LISTEN_TIMEOUT)
		{
			if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
			{
				/* Antenna setting (reserved.) */
				L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
			}
			else if (scanFsmPtr->scanMode == L2_SCAN_INTER_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 FAILURE;
			}
			DBG_WARNING("BM timeout, restart net scan from L2_SCAN_INIT_STAGE.\n");
			L2_Scan_Reset(scanFsmPtr);
		}
		else
		{
			if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
			{
				/* Antenna setting (reserved.) */
				L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
			}
			else if (scanFsmPtr->scanMode == L2_SCAN_INTER_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 FAILURE;
			}
			scanFsmPtr->bmTimeout++;
		}
		return SUCCESS;
	}
}

/*****************************************************************************/
/**
 * @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->scanFSM;
	DBG_INFO("** Netsetup - 2WAcq Procedure (Timeout: %u) **\n", scanFsmPtr->w2acqTimeout);
	L2_FRAME_TYPE frameType = L2_NONE_FRAME;

	if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
	{
		frameType = L2_INTRA_W2_ACQ_FRAME;
	}
	else if (scanFsmPtr->scanMode == L2_SCAN_INTER_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 L2_SCAN_INIT_STAGE.\n");
		L2_Scan_Reset(scanFsmPtr);
	}
	else
	{
		scanFsmPtr->w2acqTimeout++; // increase counter.
	}
	// using pull mode to receive 2wacq frame
	// since first time sync procedure is accomplished in BM stage.
	Phy_SetMode(instPtr->config.phyInstPtr, PHY_MODE_PULL);
	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 L2_Scan_PktHandler(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	uint32_t Status;
	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
	uint64_t dataChunk;

	DBG_INFO("Scan net handling packet...\n");

	BIT_ARRAY *scanMsg = bit_array_create(L2_MSG_MAX);
	if (scanMsg == NULL)
	{
		DBG_INFO("Failed to start handling process...\n");
		return EALLOC;
	}
	switch (L2_GetLastOptFrameType(instPtr))
	{
	case L2_INTER_SCAN_FRAME:
	case L2_INTRA_SCAN_FRAME:
		// INTRA_BM_FRAME could be received if current frame type is L2_INTRA_SCAN_FRAME
		// in net setup stage (for member node).
		// When SCAN frame was considered as received, handle the frame as BM frame.
		LockStateUpdate(scanFsmPtr, true);
		StageUpdate(scanFsmPtr, L2_SCAN_BMADVT_STAGE);
	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) != L2_NODE_HEAD)
		{

			NODE_ENTRY *nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId);
			if (nodeEntry == NULL)
			{
				nodeEntry = L2_AppendNode(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId, L2_NODE_HEAD);
				// TODO: Insert other node information.
			}
			else
			{
				// TODO: Update node information.
			}

			if (L2_isCxn2Net(instPtr) == false)
			{
				// TODO: Check
				L2_TimeAlign(instPtr, scanFsmPtr->BMrcvLog.frameNum, scanFsmPtr->BMrcvLog.slotNum, true);
			}
			// 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 = L2_INIT_STATE;
					break;
				case NET_NORMAL_STAGE:
					scanFsmPtr->W2ACQrcvLog.NetStage = L2_NORMAL_STATE;
					break;
				case NET_INTRA_SEEK_STAGE:
					scanFsmPtr->W2ACQrcvLog.NetStage = L2_INTRA_SEEK_STATE;
					break;
				case NET_INTER_SEEK_STAGE:
					scanFsmPtr->W2ACQrcvLog.NetStage = L2_INTER_SEEK_STATE;
					break;
				case NET_SEEK_STAGE:
					scanFsmPtr->W2ACQrcvLog.NetStage = L2_SEEK_STATE;
					break;
				default:
					scanFsmPtr->W2ACQrcvLog.NetStage = L2_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;
			}
		}

		NODE_ENTRY *nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId);
		if (nodeEntry == NULL)
		{
			nodeEntry = L2_AppendNode(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId, L2_NODE_MEMBER);
			// TODO: Insert other node information.
		}
		else
		{
			// TODO: Update node information.
		}

		if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
		{
			// check if net setup finished.
			if (L2_GetMainState(instPtr) == L2_INIT_STATE && L2_isCxn2Net(instPtr) == false)
			{

				// Assert net setup done event...
				uint32_t netStartTime = scanFsmPtr->W2ACQrcvLog.NetScanTime;
				L2_EVENT netSetupDoneEvent = {
					.event = L2_EV_NET_SETUP_DONE,
					.msg = &netStartTime};
				AssertL2Event(instPtr, &netSetupDoneEvent);

				// Update time sync since netStartTime updated.
				L2_TimeAlign(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true);

				// Assert update routine event...
				L2_RTNE_CONFIG newRtneConfig = {0};
				L2_EVENT rtneUpEvent = {
					.event = L2_ROUTINE_UPDATE,
					.msg = &newRtneConfig};
				if (scanFsmPtr->W2ACQrcvLog.NetStage == L2_INIT_STATE)
				{
					newRtneConfig.rtneTable = initTable;
					newRtneConfig.rtnePrd = ARRAY_SIZE(initTable);
					AssertL2Event(instPtr, &rtneUpEvent);
				}
				else if (scanFsmPtr->W2ACQrcvLog.NetStage == L2_INTRA_SEEK_STATE)
				{
					newRtneConfig.rtneTable = intraSeekTable;
					newRtneConfig.rtnePrd = ARRAY_SIZE(intraSeekTable);
					AssertL2Event(instPtr, &rtneUpEvent);
				}
			}

			// 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);
		}
		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 L2_Scan_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;
	}
	L2_Scan_Reset(*scanFSMPtr);
	return (SUCCESS);
}

/*****************************************************************************/
/**
 * @brief  Reset ScanNet FSM
 *
 * @param   scanFsmPtr a pointer to scan net  finite state machine
 *
 * @return  NONE
 *
 ******************************************************************************/
void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr)
{
	ASSERT(scanFSMPtr != NULL);
	scanFSMPtr->isLocked = false;
	scanFSMPtr->isBmRecvd = false;
	scanFSMPtr->isAcked = false;
	scanFSMPtr->scanCnt = 0;
	scanFSMPtr->bmTimeout = 0;
	scanFSMPtr->w2acqTimeout = 0;
	scanFSMPtr->scanStage = L2_SCAN_NONE_STAGE;
	scanFSMPtr->scanMode = L2_SCAN_INTRA_MODE;
	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 = L2_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 L2_Scan_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 L2_Scan_Proc(L2_INST *instPtr)
{
	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
	if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
	{
		DBG_ERROR("Only member node needs to execute L2_Scan_Proc. \n");
		return FAILURE;
	}
	int32_t Status;
	DBG_INFO("-------- ScanNet Process --------\n");
	switch (scanFsmPtr->scanStage)
	{
	case L2_SCAN_NONE_STAGE:
		/* Start Scan. */
		StageUpdate(scanFsmPtr, L2_SCAN_INIT_STAGE);
		Status = Scan_InitScanHandler(instPtr);
		break;
	case L2_SCAN_INIT_STAGE:
		Status = Scan_InitScanHandler(instPtr);
		break;
	case L2_SCAN_BMADVT_STAGE:
		Status = Scan_BmAdvtHandler(instPtr);
		break;
	case L2_SCANT_2WAYACQ_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 L2_Scan_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) != L2_NODE_HEAD)
	{
		if (rtneEntry->dst0 == L2_GetNodeId(instPtr) || rtneEntry->dst0 == L2_BROADCAST_ID)
		{
			torState = L2_TOR_RX;
		}
		else if (rtneEntry->src0 == L2_GetNodeId(instPtr) || rtneEntry->src0 == L2_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 L2_SLOT_INTER_SCAN_ACQ:
		// TODO:
		L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_SCAN_FRAME);
		break;

	case L2_SLOT_INTRA_SCAN_ACQ:
		if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && 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 L2_SLOT_INTER_SCAN_BM:
		// TODO:
		L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_BM_FRAME);
		break;

	case L2_SLOT_INTRA_SCAN_BM:
		if (torState == L2_TOR_TX)
		{
			if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
			{
				DBG_ERROR("Only head node can send BM Frame.\n");
				Status = FAILURE;
				goto routine_err;
			}
			memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
			if (ScanPkt_Packing(instPtr, SCANPKT_INTRA_BM) != SUCCESS)
			{
				Status = FAILURE;
				goto routine_err;
			}
		}
		/* Antenna setting (reserved.) */
		L2_SetupNextSlot(instPtr, torState, L2_INTRA_BM_FRAME);
		break;
	case L2_SLOT_INTER_SCAN_2W:
		// TODO:
		L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_W2_ACQ_FRAME);
		break;
	case L2_SLOT_INTRA_SCAN_2W:
		if (torState == L2_TOR_TX)
		{
			if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && L2_isDataAvail(instPtr) == false)
			{
				// skip...
				break;
			}
			/* Prepare Request/ACK data. */
			memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
			/* Pack message words */
			if (ScanPkt_Packing(instPtr, SCANPKT_INTRA_2WACQ) != SUCCESS)
			{
				Status = FAILURE;
				goto routine_err;
			}
			W2ackStateUpdate(instPtr->scanFSM, 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;
}