#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "error.h"
#include "unit_test_framework.h"
#include "time_ctrl/time_ctrl.h"
#include "phy_ctrl/phy_ctrl.h"
#include "linklayer/linklayer814.h"
#include "linklayer/msg_def.h"

#define TIME_INT_REG	(0x00000008>>2)
#define TIME_FRAC_REG	(0x0000000C>>2)
#define TIME_ADJ_INT	(0x0000001C>>2)
#define TIME_ADJ_FRAC	(0x00000020>>2)
#define TIME_ADJ_TRIG	(0x00000024>>2)
#define CNTSTEP_OPT		(0x00000028>>2)

TIMECTRL_INST* tmcInstPtr = NULL;
PHY_INST* phyInstPtr = NULL;
L2_INST* linkInstPtr = NULL;

uint32_t tmcReg[32] = {0};
uint32_t phycReg[512] = {0};

uint8_t bmTemplate[INTRA_BM_FRAME_LENGTH_MAX] __attribute__ ((aligned (8))) = {
	0x08, 0x21, 0x04, 0x06, 0x00, 0xF3, 0xA8, 0xCB, 
	0x00, 0xA0, 0x49, 0x00, 0x9A, 0x04, 0x13, 0x1E, 
	0x45, 0xE7, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA
};
uint8_t w2acqTemplate[INTRA_W2ACQ_FRAME_LENGTH_MAX] __attribute__ ((aligned (8))) = {
	0xCC, 0x71, 0x21, 0xA8, 0x0A, 0x00, 0x00, 0x00, 
	0x00, 0x1A, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 
	0xF0, 0xF0, 0x2A, 0x0F, 0x0F, 0xF0, 0x0F, 0x00, 
	0xFF, 0x00, 0xAA, 0x3A, 0xFF, 0x00, 0xFF, 0x00, 
	0x00, 0xFF, 0x00, 0xFF, 0x4A, 0xB2, 0xBA, 0xF0, 
	0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0x5A, 0x0F, 0x0F, 
	0x0F, 0x0F, 0x0F, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA
};
uint8_t mgmtTemplate[INTRA_MGMT_FRAME_LENGTH_MAX] __attribute__ ((aligned (8))) = {
	0xCC, 0xC3, 0x99, 0x99, 0x83, 0x99, 0x99, 0x99, 
	0x99, 0xA5, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xBA, 
	0xBB, 0x9B, 0x19, 0xCC, 0xCC, 0xDD, 0xDD, 0xEE, 
	0xEE, 0xEE, 0x99, 0x29, 0xAA, 0xAA, 0xAA, 0xAA, 
	0xBB, 0xBB, 0xBB, 0xBB, 0x39, 0x21, 0x98, 0xBB, 
	0xBB, 0xCC, 0xCC, 0xDD, 0xDD, 0x99, 0x99, 0x99, 
	0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
uint8_t rttTemplate[INTRA_RTT_FRAME_LENGTH_MAX] __attribute__ ((aligned (8))) = {
	0xDC, 0xA3, 0x10, 0x44, 0x98, 0x99, 0x99, 0x99, 
	0x99, 0x05, 0x00, 0x00, 0x00, 0x50, 0x32, 0x01, 
	0x00, 0x90, 0x99, 0xFF, 0xFF, 0xFF, 0x99, 0x99, 
	0x99, 0x99, 0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
uint8_t svcTemplate[INTRA_SVC_FRAME_LENGTH_MAX] __attribute__ ((aligned (8))) = {
	0xF0, 0xFF, 0x00, 0x21, 0x88, 0x00, 0x00, 0xF0,
	0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0x56, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x21, 0xE6, 0x00, 0x21, 0x88, 0x00, 0x00,
	0x11, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0xB5,
};


L2_CONFIG l2Config = {
	.myType = L2_NODE_HEAD,
	.isInterHead = false,
	.myId = 1,
	.clustId = 1,
	.headId = 1,
	.bkpHeadId = 1,
	.netScale = 2,
	.phyInstPtr = NULL,
	.tmcInstPtr = NULL
};

PHY_CONFIG l1Config = {
	.devId = 0,
	.txBramId = 0,
	.rxBramId = 0,
	.antBramId = 0,
	.baseAddr = (uintptr_t)phycReg
};

TIMECTRL_CONFIG tmcConfig = {
	.baseAddr = (uintptr_t)tmcReg,
	.reset = 10
};

inline uint32_t rand_range(uint32_t a, uint32_t b){
	ASSERT(b > a);
	return (rand() % (b + 1 - a) + a);
}

void linklayer_setup(void){
	int32_t Status;
	Status = TimeCtrl_Init(&tmcInstPtr, &tmcConfig);
	ASSERT_EQUAL(SUCCESS, Status);
	Status = Phy_InstInit(&phyInstPtr, &l1Config);
	ASSERT_EQUAL(SUCCESS, Status);

	// Update l2 config
	l2Config.phyInstPtr = phyInstPtr;
	l2Config.tmcInstPtr = tmcInstPtr;

	Status = Linklayer_Init(&linkInstPtr, &l2Config);
	ASSERT_EQUAL(SUCCESS, Status);
}

void linklayer_teardown(void){
	Linklayer_Destroy(linkInstPtr);
	Phy_Destroy(phyInstPtr);
	TimeCtrl_Destroy(tmcInstPtr);
}

void forge_bm_frame(uint8_t* buff, uint8_t nodeId, uint16_t clustId, uint16_t headId, uint64_t time) {
	BIT_ARRAY bitarr = {
		.words = (uint64_t*)buff,
		.num_of_bits = INTRA_BM_FRAME_LENGTH_MAX*8,
		.num_of_words = roundup_bits2words64(INTRA_BM_FRAME_LENGTH_MAX*8),
		.capacity_in_words = roundup_bits2words64(INTRA_BM_FRAME_LENGTH_MAX*8),
	};
	memcpy(buff, bmTemplate, INTRA_BM_FRAME_LENGTH_MAX);
	uint64_t dataChunk;
	dataChunk = (nodeId & 0x001F) | ((clustId & 0x001F) << 5);
	FrameInsertChunk(&bitarr, BM_MSG_SRC_ADDR_OFFSET, BM_MSG_SRC_ADDR_LEN, dataChunk);

	/* net management node id */
	dataChunk = (headId & 0x001F) | ((clustId & 0x001F) << 5);
	FrameInsertChunk(&bitarr, BM_MSG_NETMG_ADDR_OFFSET, BM_MSG_NETMG_ADDR_LEN, dataChunk);

	/* time unit number */
	dataChunk = time / L2_SLOTS_PER_FRAME;
	FrameInsertChunk(&bitarr, BM_MSG_TUNIT_NUM_OFFSET, BM_MSG_TUNIT_NUM_LEN, dataChunk);

	/* frame number */
	dataChunk = (time % L2_SLOTS_PER_FRAME) / 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 = time % L2_SLOTS_PER_SUBFRAME;
	FrameInsertChunk(&bitarr, BM_MSG_TSLOT_NUM_OFFSET, BM_MSG_TSLOT_NUM_LEN, dataChunk);

	// Append CRC
	L2_CrcAppend(buff, BM_MSG_CRC_OFFSET / 8, BM_MSG_CRC_OFFSET / 8, L2_CRC8_31);
}

void forge_w2acq_frame(uint8_t* buff, L2_NODE_TYPE nodeType, uint16_t srcId, uint16_t destId, 
						uint16_t clustId, uint32_t netStartTime, L2_NODE_STATE state) {
	BIT_ARRAY bitarr = {
		.words = (uint64_t*)buff,
		.num_of_bits = INTRA_W2ACQ_FRAME_LENGTH_MAX*8,
		.num_of_words = roundup_bits2words64(INTRA_W2ACQ_FRAME_LENGTH_MAX*8),
		.capacity_in_words = roundup_bits2words64(INTRA_W2ACQ_FRAME_LENGTH_MAX*8),
	};
	memcpy(buff, w2acqTemplate, INTRA_W2ACQ_FRAME_LENGTH_MAX);

	uint64_t dataChunk;
	/* message type */
	dataChunk = (nodeType == L2_NODE_HEAD) ? 0 : 1;
	FrameInsertChunk(&bitarr, NS_MSG_INI_TYPE_OFFSET, NS_MSG_INI_TYPE_LEN, dataChunk);

	/* source node id */
	dataChunk = (srcId & 0x001F) | ((clustId & 0x001F) << 5);
	FrameInsertChunk(&bitarr, NS_MSG_INI_SRC_ADDR_OFFSET, NS_MSG_INI_SRC_ADDR_LEN, dataChunk);

	/* destination node id */
	dataChunk = (destId & 0x001F) | ((clustId & 0x001F) << 5);
	FrameInsertChunk(&bitarr, NS_MSG_INI_LEN_OFFSET, NS_MSG_INI_LEN_LEN, dataChunk);
	/* current net stage */
	switch (state) {
	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 = netStartTime;
	FrameInsertChunk(&bitarr, NS_MSG_INI_SCAN_TIME_OFFSET, NS_MSG_INI_SCAN_TIME_LEN, dataChunk);

	// Update CRC
	L2_CrcAppend(buff, W2ACQ_FRAME_CRC_DLEN, W2ACQ_FRAME_CRC_POS, L2_CRC16_8005);
}

void forge_mgmt_frame(uint8_t* buff, L2_NODE_STATE state) {
	BIT_ARRAY bitarr = {
		.words = (uint64_t*)buff,
		.num_of_bits = INTRA_MGMT_FRAME_LENGTH_MAX*8,
		.num_of_words = roundup_bits2words64(INTRA_MGMT_FRAME_LENGTH_MAX*8),
		.capacity_in_words = roundup_bits2words64(INTRA_MGMT_FRAME_LENGTH_MAX*8),
	};
	memcpy(buff, mgmtTemplate, INTRA_MGMT_FRAME_LENGTH_MAX);

	uint64_t dataChunk;
	switch (state) {
	case L2_INIT_STATE:
		dataChunk = NET_INI_STAGE;
		break;
	case L2_NORMAL_STATE:
		dataChunk = NET_NORMAL_STAGE;
		break;
	case L2_INTRA_SEEK_STATE:
		dataChunk = NET_INTRA_SEEK_STAGE;
		break;
	case L2_INTER_SEEK_STATE:
		dataChunk = NET_INTER_SEEK_STAGE;
		break;
	case L2_SEEK_STATE:
		dataChunk = NET_SEEK_STAGE;
		break;
	default:
		dataChunk = NET_ERROR_STAGE;
		break;
	FrameInsertChunk(&bitarr, TK_MSG_INI_NET_STATE_OFFSET, TK_MSG_INI_NET_STATE_LEN, dataChunk);
	}
}

void forge_rtt_frame(uint8_t* buff, uint16_t srcId, uint16_t destId, 
					uint16_t clustId, const TIME_WORD* timeStamp, uint32_t doppler) {
	BIT_ARRAY bitarr = {
		.words = (uint64_t*)buff,
		.num_of_bits = INTRA_BM_FRAME_LENGTH_MAX*8,
		.num_of_words = roundup_bits2words64(INTRA_BM_FRAME_LENGTH_MAX*8),
		.capacity_in_words = roundup_bits2words64(INTRA_BM_FRAME_LENGTH_MAX*8),
	};
	memcpy(buff, rttTemplate, INTRA_BM_FRAME_LENGTH_MAX);
	uint64_t dataChunk;
	// INIT MSG
	// --source node address.
	dataChunk = (srcId & 0x001F) | ((clustId & 0x001F) << 5);
	FrameInsertChunk(&bitarr, RTT_REQ_SRC_ADDR_OFFSET, RTT_REQ_SRC_ADDR_LEN, dataChunk);
	// --destination node address.
	dataChunk = (destId & 0x001F) | ((clustId & 0x001F) << 5);
	FrameInsertChunk(&bitarr, RTT_REQ_DST_ADDR_OFFSET, RTT_REQ_DST_ADDR_LEN, dataChunk);

	// EXTD MSG
	// --Time tag Low
	dataChunk = timeStamp->frac_part;
	FrameInsertChunk(&bitarr, RTT_RSP_EX_TTAGL_OFFSET + RTT_MSG_LEN, RTT_RSP_EX_TTAGL_LEN, dataChunk);
	// --Time tag High.
	dataChunk = timeStamp->int_part;
	FrameInsertChunk(&bitarr, RTT_RSP_EX_TTAGH_OFFSET + RTT_MSG_LEN, RTT_RSP_EX_TTAGH_LEN, dataChunk);

	// CONT MSG
	// -- Doppler
	dataChunk = doppler;
	FrameInsertChunk(&bitarr, RTT_RSP_CONT_DOP_OFFSET + RTT_MSG_LEN * 2, RTT_RSP_CONT_DOP_LEN, dataChunk);
	return;
}

TEST(linklayer_master_routine) {
	int32_t Status;
	Linklayer_Start(linkInstPtr);
	TIME_WORD timeStampA, timeStampB, timeGap;
	uint32_t timeDiff;
	// 30 seconds
	for(int i = 0; i < 200*30; i++) {
		tmcReg[TIME_INT_REG]++;

		if (L2_GetNextTor(linkInstPtr) == L2_TOR_TX) 
		{
			if (L2_GetNextFrameType(linkInstPtr) == L2_INTRA_TSYNC_FRAME)
			{
				TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampA);
				// ASSERT_EQUAL(linkInstPtr->rttFSM->rttRecord.Tt1.int_part + linkInstPtr->netStartTime*L2_SLOTS_PER_FRAME, 
				// 				timeStampA.int_part);
				// ASSERT_EQUAL(linkInstPtr->rttFSM->rttRecord.Tt1.frac_part, timeStampA.frac_part);
				timeStampA.frac_part = rand()%1000; // 10us deviation.
				
			}
		}
		if (L2_GetCurrentTor(linkInstPtr) == L2_TOR_RX)
		{
			if(L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_W2_ACQ_FRAME) {
				forge_w2acq_frame((uint8_t*)phyInstPtr->recvDataAddr, L2_NODE_MEMBER, 2, 1, 1, -1, L2_INIT_STATE);
				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_W2ACQ_FRAME_LENGTH_MAX);
			}

			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_MGMT_FRAME)
			{
				forge_mgmt_frame((uint8_t*)phyInstPtr->recvDataAddr, L2_NORMAL_STATE);
				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_MGMT_FRAME_LENGTH_MAX);
			}
			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_TSYNC_FRAME)
			{
				timeDiff = Phy_GetTimeComp(L2_L2frame2L1frame(L2_INTRA_TSYNC_FRAME)) + (rand()-RAND_MAX)%1000;
				TimeCtrl_CalculateTimeGap(&timeStampA, &timeStampB, &timeGap);
				forge_rtt_frame((uint8_t*)phyInstPtr->recvDataAddr, 2, 1, 1, &timeGap , rand()%200000);
				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
				PhyWrite(phyInstPtr, FSTIME_H, L2_GetLastFrameNum(linkInstPtr));
				PhyWrite(phyInstPtr, FSTIME_L, timeDiff);
				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_MGMT_FRAME_LENGTH_MAX);
			}

			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_SVC_FRAME)
			{
				forge_mgmt_frame((uint8_t*)phyInstPtr->recvDataAddr, L2_NORMAL_STATE);
				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_MGMT_FRAME_LENGTH_MAX);
			}
		}
		else {
			PhyWrite(phyInstPtr, TXRX_STATE, 0x02020202);
		}

		Phy_UpdateState(phyInstPtr);

		Status = Linklayer_Proc(linkInstPtr);

		if((tmcReg[TIME_INT_REG]+1)/200 < linkInstPtr->netStartTime) {
			ASSERT_EQUAL(EIGNORE, Status);
		}
		else{
			ASSERT_EQUAL(SUCCESS, Status);
		}

		if(L2_isDataAvail(linkInstPtr)) {
			if(L2_GetLastFrameType(linkInstPtr) == L2_INTRA_W2_ACQ_FRAME) {
				ASSERT_EQUAL(1, L2_GetNodeListLen(linkInstPtr->nodeList));
				ASSERT_TRUE(L2_isNodeInList(linkInstPtr->nodeList, 2));
			}
		}
	}
}


TEST(linklayer_reconfig) {
	int32_t Status;
	Status = Linklayer_Reconfig(linkInstPtr, &l2Config);
	ASSERT_EQUAL(SUCCESS, Status);
}

TEST_SUITE(linklayer_unit_test) {
	TEST_SUITE_CONFIGURE(linklayer_setup, linklayer_teardown);
	RUN_TEST(linklayer_master_routine);
	RUN_TEST(linklayer_reconfig);
}

int main(void) {
	RUN_TEST_SUITE(linklayer_unit_test);
	TEST_REPORT();
	return TEST_EXIT_CODE;
}