/*****************************************************************************/
/**
 * Description: Implementations of df linklayer scan net.
 *
 * Date: 2021-11-10 19:04
 * 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 "error.h"
#include "common.h"
#include "msg_def.h"
#include "link_layer.h"
#include "link_layer_types.h"
#include "../phy_layer/phy_layer.h"
#include "node_list.h"

/************************** Constant Definitions *****************************/

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

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

/************************** Variable Definitions *****************************/

/************************** Function Prototypes ******************************/
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
/*****************************************************************************/
/**
 * @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, L2_FRAME_TYPE type) {
	ASSERT(instPtr != NULL);
	int32_t Status;
	uint8_t *buff;
	uint32_t packLen;
	switch (type) {
		case L2_INFO_FRAME:{
			packLen = RSP_PACK_LEN;
			SysMonitor_GetSysinfo(&(instPtr->sysInfo));
			buff = malloc(packLen);
			if (buff == NULL) {
				DBG_ERROR("Not enough space, failed to pack frame...\n");
				return ENOMEM;
			}
			buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
			buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
			memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
			memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
			memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
			memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
			memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
			memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));

			Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
			if (Status != SUCCESS) {
				DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
				return Status;
			}
			if(CircQueuePush(instPtr->txQueue, buff) == false){
				DBG_ERROR("Cannot push packet to queue..\n");
				return EQUEUE;
			}
			break;
		}
		default :{
			DBG_ERROR("Unexpected frame type: %d.\n", type);
			return EPARAM;
			break;
		}
	}
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Initialize scan FSM.
 *
 * @param   instPtr
 *
 * @return
 *
 ******************************************************************************/
int32_t L2_Scan_Init(L2_INST *instPtr) {
	ASSERT(instPtr != NULL);
	instPtr->scanFsm.sacanCnt = 0;
	instPtr->scanFsm.beamNo = 0;
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Scan net packet handler.
 *
 * @param   msg A pointer to frame data buffer.
 *
 * @return
 *
 ******************************************************************************/
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
	ASSERT(instPtr != NULL);
	ASSERT(msg != NULL);
	int32_t Status;
	switch (instPtr->frame.last) {
	case L2_SCAN_FRAME: {
		if (instPtr->config.myType == L2_HEAD_NODE) {
			DBG_ERROR("Unexpected call from head node, Only None-Head Node can handle L2_SVC_FRAME.\n");
			Status = EUNEXP;
			break;
		}
		Status = AppendNode(instPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE);
		if (Status != SUCCESS) {
			DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
		}
		break;
	}
	case L2_INFO_FRAME: {
		if (instPtr->config.myType != L2_HEAD_NODE) {
			DBG_ERROR("Unexpected call from member node, Only Head Node can handle L2_SVC_FRAME.\n");
			Status = EUNEXP;
			break;
		}
		COORD coordTemp;
		float tmpVal;
		Status = AppendNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE);
		if (Status != SUCCESS) {
			DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
			break;
		}
		memcpy(&tmpVal, msg+L2_MSG_INFO_POSX, sizeof(float));
		coordTemp.x = tmpVal;
		memcpy(&tmpVal, msg+L2_MSG_INFO_POSY, sizeof(float));
		coordTemp.y = tmpVal;
		memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
		coordTemp.z = tmpVal;
		Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
		if (Status != SUCCESS) {
			DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
		}
		memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDX, sizeof(float));
		coordTemp.x = tmpVal;
		memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDY, sizeof(float));
		coordTemp.y = tmpVal;
		memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
		coordTemp.z = tmpVal;
		Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
		if (Status != SUCCESS) {
			DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
		}
		// Update Time deviation
		int64_t devi, deviTrunc;
		Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
		if (Status != SUCCESS) {
			DBG_ERROR("Failed to compute slot deviation.\n");
			UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0);
		}
		else {
			if(devi > INT32_MAX || devi < INT32_MIN) {
				DBG_ERROR("Illegal deviation value: %lld.\n", devi);
				deviTrunc = 0;
			}
			else if(devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN) {
				DBG_WARNING("Deviation value: %lld is out of adjustable range.\n", devi);
				deviTrunc = (int32_t)devi;
			}
			else {
				deviTrunc = (int32_t)devi;
			}
			UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
		}
		break;
	}
	default:
		DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last);
		Status = EUNEXP;
		break;
	}

	// Net setup finished.
	if(L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS){
		L2_EVENT l2Event;
		L2_RTNE_CONFIG newRtneConfig;

		// Assert Net-Setup Done.
		l2Event.event = L2_EV_NET_SETUP_DONE;
		l2Event.msg = NULL;
		AssertL2Event(instPtr, &l2Event);

		// Assert Routine Table Update...
		newRtneConfig.rtneTable = normalTable;
		newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable)*L2_SLOTS_PER_FRAME;
		newRtneConfig.rtneStep = 0;
		l2Event.event = L2_ROUTINE_UPDATE;
		l2Event.msg = &newRtneConfig;
		AssertL2Event(instPtr, &l2Event);
	}
	return Status;
}

/*****************************************************************************/
/**
 * @brief   Execute a member node scan net procedure.
 *
 * @param   instPtr  A pointer to scan net fsm.
 *
 * @return  used slot (5ms slot).
 *
 ******************************************************************************/
int32_t L2_Scan_Proc(L2_INST *instPtr) {
	int32_t Status;
	/* instPtr->nextState == NORMAL_STATE indicates network set up procedure was finished.
	 * Skip 5ms for nodes for node configuration update.
	 */
	DBG_INFO("-------- Scan Process --------\n");
	if (instPtr->nextState == NORMAL_STATE) {
		DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n");
		if(instPtr->config.myType != L2_HEAD_NODE) {
			Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
			Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
			Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
			L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME);
		}
		else {
			DBG_ERROR("Illegal call from Head Node.\n");
		}
		return NUM_SLOTS_SKIP;
	}

	/* Head node implement a periodical scan pattern 
	 * | Tx 20ms | Rx 100ms | Tx 20ms | Rx 100ms ... ...
	 * Send pilot signal in Tx slots with narrow beam width.
	 * Wait for response in Rx slots with the same beam width and beam number.
	 * Increase beam number after a Tx-Rx procedure pair, reset beam number when 
	 * it reaches the boundary.
	 */
	if (instPtr->config.myType == L2_HEAD_NODE) {
		// Apply a scan procedure
		if (instPtr->scanFsm.sacanCnt % 2 == 0){
			// Antenna configure
			Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
			Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
			Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
			L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_SCAN_FRAME);
		}
		else {
			// Antenna configure
			Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
			Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
			Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
			L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INFO_FRAME);
			// Update Scan beam number
			instPtr->scanFsm.beamNo = (instPtr->scanFsm.beamNo + 1) % L1_NUM_ANT_NRRW_PATTERN;
		}
	}
	/* Member node listening the head node pilot signal with wide beam width.
	 * Once is received a pilot signal, physical layer will perform a response
	 * procedure automatically, witch should contains the basic node information 
	 * (position, speed, etc.).
	 * So that, a extra datagram with the up-to-date node information is attached
	 * to the listening configuration.
	 */
	else {
		uint8_t *sendData;
		// Pack node information.
		Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME);
		if (Status != SUCCESS) {
			DBG_INFO("Failed to pack response.\n");
		}
		if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
			memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
			free(sendData);
		}
		else {
			DBG_INFO("Failed to pop tx Queue.\n");
		}
		// Antenna configure
		Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
		Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
		Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_LISTEN);
		L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME);
	}

	// Increase scan counter (for debug purpose).
	instPtr->scanFsm.sacanCnt ++;
	return NUM_SLOTS_SKIP;
}

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

/**
 * @brief   Execute a scan net Routine procedure.
 *
 * @param   schedEntry  Routine configuration
 *
 * @return  used slot (5ms slot).
 *
 ******************************************************************************/
int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
	DBG_INFO("-------- Scan Routine --------\n");
	ASSERT(false); // Not support.
	return 0;
}