/*****************************************************************************/
/**
 * Description: Implementations for 814 datalink service net.
 * Date: 2021-11-16 00:08
 * LastEditors: Fan Hao
 * LastEditTime: 2022-07-16 17:04
 * Revisions: Rev 0.1
 * History:
 * [2022-07-16]: New branch for DiJian.
 * [2021-11-16]: 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 "msg_def.h"
#include "../error.h"
#include "../common.h"


#ifdef _MPC_CTRL

#endif

/************************** Constant Definitions *****************************/
#define SQUENCE_SVC_SLOTS 0
#define INER_SVC_SLOTS   2
#define INRA_SVC_SLOTS   1
#define NONE_SVC_SLOTS   0
#define TEST_UDU_PDLEN	 16
#define TEST_UDU_LEN	 (UDUH_LEN/8 + TEST_UDU_PDLEN + PDU_CRC_LEN/8 )
/***************** Macros (Inline Functions) Definitions *********************/

/************************** Function Prototypes ******************************/
#ifdef _MPC_CTRL
#endif

/**************************** Type Definitions *******************************/

/************************** Variable Definitions *****************************/
#ifdef _MPC_CTRL

#endif

/*****************************************************************************/
/**
 * @brief   Packing UDU Header to a bit array.
 *
 * @param   bitarr		pointer to bit array buffer.
 *          dst			Destination node address.
 *          dataLen		Length of UDU data.
 *
 * @return	EPARAM
 * 			SUCCESS
 *
 ******************************************************************************/
static int UduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, uint8_t dst, uint32_t dataLen){
	ASSERT(bitarr != NULL);
	if(bitarr->num_of_bits < UDUH_LEN){
		return EPARAM;
	}
	uint64_t dataChunk = 0;
	memset(bitarr->words, 0xF0, (size_t)(UDUH_LEN/8));
	// priority
	dataChunk = 1;
	FrameInsertChunk(bitarr, UDUH_PRI_OFFSET, UDUH_PRI_LEN, dataChunk);
	// message type
	dataChunk = 2;
	FrameInsertChunk(bitarr, UDUH_MSG_TYPE_OFFSET, UDUH_MSG_TYPE_LEN, dataChunk);
	// data length
	dataChunk = dataLen;
	FrameInsertChunk(bitarr, UDUH_LENGTH_OFFSET, UDUH_LENGTH_LEN, dataChunk);
	// source node id
	dataChunk = (L2_GetNodeId(instPtr) & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
	FrameInsertChunk(bitarr, UDUH_SRC_ADDR_OFFSET, UDUH_SRC_ADDR_LEN, dataChunk);
	// destination node id
	dataChunk = (dst & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
	FrameInsertChunk(bitarr, UDUH_DST_ADDR_OFFSET, UDUH_DST_ADDR_LEN, dataChunk);
	// UDU Sequence number
	dataChunk = 0;
	FrameInsertChunk(bitarr, UDUH_UDU_SEQ_NUM_OFFSET, UDUH_UDU_SEQ_NUM_LEN, dataChunk);
	// retransmission times
	dataChunk = 0;
	FrameInsertChunk(bitarr, UDUH_RETRANS_NUM_OFFSET, UDUH_RETRANS_NUM_LEN, dataChunk);
	// number of slice
	dataChunk = 1;
	FrameInsertChunk(bitarr, UDUH_NUM_SLICE_OFFSET, UDUH_NUM_SLICE_LEN, dataChunk);
	// slice id
	dataChunk = 1;
	FrameInsertChunk(bitarr, UDUH_SLICE_ID_OFFSET, UDUH_SLICE_ID_LEN, dataChunk);
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Packing PDU Header to a bit array.
 *
 * @param   bitarr		pointer to bit array buffer.
 *          headerType	PDU header type.
 *          dst			Destination node address.
 *
 * @return	EPARAM
 * 			SUCCESS
 *
 ******************************************************************************/
static int PduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_PDU_HEADER_TYPE headerType, uint8_t dst){
	ASSERT(bitarr != NULL);
	if(headerType < L2_PDU_HEADER_0	|| headerType > L2_PDU_HEADER_1){
		return EPARAM;
	}
	uint64_t dataChunk = 0;
	if(headerType == L2_PDU_HEADER_0){
		if(bitarr->num_of_bits < PDUH0_LEN){
			return EPARAM;
		}
		memset(bitarr->words, 0xF0, (size_t)(PDUH0_LEN/8));
		// PDU type
		dataChunk = 0;
		FrameInsertChunk(bitarr, PDUH0_TYPE_OFFSET, PDUH0_TYPE_LEN, dataChunk);
		// frame length
		dataChunk = INTRA_SVC_FRAME_LENGTH_MAX;
		FrameInsertChunk(bitarr, PDUH0_LENGTH_OFFSET, PDUH0_LENGTH_LEN, dataChunk);
		// source node id
		dataChunk = (L2_GetNodeId(instPtr) & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
		FrameInsertChunk(bitarr, PDUH0_SRC_ADDR_OFFSET, PDUH0_SRC_ADDR_LEN, dataChunk);
		// destination node id
		dataChunk = (dst & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
		FrameInsertChunk(bitarr, PDUH0_DST_ADDR_OFFSET, PDUH0_DST_ADDR_LEN, dataChunk);
		// Sequence number
		dataChunk = 0;
		FrameInsertChunk(bitarr, PDUH0_SEQ_NUM_OFFSET, PDUH0_SEQ_NUM_LEN, dataChunk);
		// Append CRC
		L2_CrcAppend((uint8_t*)bitarr->words, PDUH0_HEADER_CRC_OFFSET/8, PDUH0_HEADER_CRC_OFFSET/8, L2_CRC8_31);
	}
	else if(headerType == L2_PDU_HEADER_1){
		if(bitarr->num_of_bits < PDUH1_LEN){
			return EPARAM;
		}
		memset(bitarr->words, 0xF0, (size_t)(PDUH1_LEN/8));
		// PDU type
		dataChunk = 1;
		FrameInsertChunk(bitarr, PDUH1_TYPE_OFFSET, PDUH1_TYPE_LEN, dataChunk);
		// frame length
		dataChunk = INTRA_SVC_FRAME_LENGTH_MAX;
		FrameInsertChunk(bitarr, PDUH1_LENGTH_OFFSET, PDUH1_LENGTH_LEN, dataChunk);
		// source node id
		dataChunk = (L2_GetNodeId(instPtr) & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
		FrameInsertChunk(bitarr, PDUH1_SRC_ADDR_OFFSET, PDUH1_SRC_ADDR_LEN, dataChunk);
		// destination node id
		dataChunk = (dst & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
		FrameInsertChunk(bitarr, PDUH1_DST_ADDR_OFFSET, PDUH1_DST_ADDR_LEN, dataChunk);
		// Slice tage...
		// Append CRC
		L2_CrcAppend((uint8_t*)bitarr->words, PDUH1_HEADER_CRC_OFFSET/8, PDUH1_HEADER_CRC_OFFSET/8, L2_CRC8_31);
	}
	else{
		return FAILURE;
	}
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Initialize service net FSM
 *
 * @param   serviceFSM  A pointer to service FSM instance.
 *
 * @return  SUCCESS Initialize successfully.
 *          Other       Initialize failed.
 *
 ******************************************************************************/
int32_t ServiceNet_Init(L2_SVC_FSM** serviceFSM) {
	(*serviceFSM) = (L2_SVC_FSM*) malloc(sizeof(L2_SVC_FSM));
	if (*serviceFSM == NULL) {
		return EALLOC;
	}
	ServiceNet_Reset(*serviceFSM);
	return SUCCESS;
}


void ServiceNet_Reset(L2_SVC_FSM* serviceFSM) {
	serviceFSM->isBisar = false;
	serviceFSM->isBisarEven = false;
	serviceFSM->bisarDst = L2_NONE_ID;
}

void ServiceNet_Destroy(L2_SVC_FSM* serviceFSM){
	free(serviceFSM);
}
/*****************************************************************************/
/**
 * @brief   Handle service received packet. Send data up to MPC
 *
 * @param   instPtr	a pointer to linklayer layer instance
 *
 * @return  SUCCESS message has been successfully handled.
 *          other       error occurred.
 *
 ******************************************************************************/
int32_t ServiceNet_PktHandler(L2_INST* instPtr) {
	// TODO:
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Service net FSM procedure.
 *
 * @param   instPtr	a pointer to linklayer layer instance
 * @param   rtneEntry	
 *
 * @return  Slot has been used.
 *
 ******************************************************************************/
int32_t ServiceNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry) {
	ASSERT(instPtr != NULL);
	int32_t Status;
	L2_TOR_TYPE torState;
	DBG_INFO("-------- ServiceNet Routine --------\n");

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

	// Determine TOR State.
	if(rtneEntry->src0 == L2_GetNodeId(instPtr)){
		torState = L2_TOR_TX;
	}
	else if(rtneEntry->dst0 == L2_GetNodeId(instPtr)){
		torState = L2_TOR_RX;
	}
	else{
		torState = L2_TOR_SUSPEND;
	}

	BIT_ARRAY* svcMsg = bit_array_create(L2_MSG_MAX);
	if(svcMsg == NULL){
		DBG_ERROR("Failed to allocate memory for svcMsg.\n");
		return FAILURE;
	}
	memset(instPtr->sendPktData, 0xCC, (size_t)(INTRA_SVC_FRAME_LENGTH_MAX));
	switch(rtneEntry->slotType){
	case L2_SLOT_INTER_SVC:
		L2_SetupNextSlot(instPtr, torState, L2_INTER_SVC_FRAME);
		break;
	case L2_SLOT_INTRA_SVC:
		if(torState == L2_TOR_TX){
			//TODO: 
			memset(instPtr->sendPktData, 0xAA, INTRA_SVC_FRAME_LENGTH_MAX);
			// Pack PDU header
			if(PduHeader_Packing(instPtr, svcMsg, L2_PDU_HEADER_0, rtneEntry->dst0) != SUCCESS){
				DBG_ERROR("Failed to packing PduHeader.\n");
				Status = FAILURE;
				goto routine_err;
			}
			memcpy(instPtr->sendPktData, svcMsg->words, (size_t)(PDUH0_LEN/8));
			// Pack UDU Header
			if(UduHeader_Packing(instPtr, svcMsg, rtneEntry->dst0, 
					INTRA_SVC_FRAME_LENGTH_MAX-(PDUH0_LEN+MSG_KEY_LENGTH_LEN)/8) 
				!= SUCCESS){
				DBG_ERROR("Failed to packing UduHeader.\n");
				Status = FAILURE;
				goto routine_err;
			}
			memcpy(instPtr->sendPktData + ((PDUH0_LEN+MSG_KEY_LENGTH_LEN)/8), svcMsg->words, (size_t)(UDUH_LEN/8));
			// For ZhiQiang
			// Payload Byte3, bit0~2: message class, bit3~7 message subclass
			// FIXME: message class tag 0x01, only for test.
			memset(instPtr->sendPktData+((PDUH0_LEN+MSG_KEY_LENGTH_LEN+UDUH_LEN)/8) + 3, 0x01, 1);
			L2_CrcAppend(instPtr->sendPktData + ((PDUH0_LEN+MSG_KEY_LENGTH_LEN)/8),
					INTRA_SVC_FRAME_LENGTH_MAX-(PDUH0_LEN+MSG_KEY_LENGTH_LEN+PDU_CRC_LEN)/8,
					INTRA_SVC_FRAME_LENGTH_MAX-(PDUH0_LEN+MSG_KEY_LENGTH_LEN+PDU_CRC_LEN)/8,
					L2_CRC16_8005
			);
		}
		L2_SetupNextSlot(instPtr, torState, L2_INTRA_SVC_FRAME);
		break;
	default:
		DBG_ERROR("UNKNOWN slot type [%d].\n", rtneEntry->slotType);
		DBG_ERROR("Failed to packing PduHeader.\n");
		Status = FAILURE;
		goto routine_err;
	}
	bit_array_free(svcMsg);// free memory
	return SUCCESS;
routine_err:
	bit_array_free(svcMsg);// free memory
	return Status;
}