/*****************************************************************************/
/**
 * Description: RTT algorithm.
 *
 * Author: FanHao
 * Date: 2023-12-10 20:43
 * LastEditors: FanHao
 * LastEditTime: 2023-12-10 20:43
 * Revisions: Rev 0.0
 * History:
 * [2023-12-10]: Initial version, split form time_sync.c
 *
 * Copyright(c) 2021 Beijing Institute of Technology.
 * Lab of Communication and Networking
 *
 ******************************************************************************/

/***************************** Include Files *********************************/
#include <string.h>
#include <stdlib.h>
#include "linklayer814.h"
#include "rtt.h"
#include "../time_ctrl/time_ctrl.h"
#include "../phy_ctrl/phy_ctrl.h"
#include "../error.h"
#include "../common.h"

/************************** Constant Definitions *****************************/
#define LIGHT_SPPED_AIR 299702547.0 // m/s
#define LIGHT_SPPED_VAC 299792458.0 // m/s
#define RTT_DOPPLER_NONE UINT32_MAX

const TIME_WORD RTT_TIME_NONE = 
{
	.int_part = UINT32_MAX,
	.frac_part = UINT32_MAX
};
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/

static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage);
static void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType);

/*****************************************************************************/
/**
 * @brief   Update corrent time-sync for RTT stage
 *
 * @param   scanFsmPtr  A pointer to scan net fsm.
 * @param   newSatge    New state.
 *
 * @return  NONE.
 *
 ******************************************************************************/
static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage)
{
	ASSERT(rttFsmPtr != NULL);
	ASSERT(newStage >= RTT_NONE_STAGE && newStage <= RTT_RSP2);
	DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStage, newStage);
	rttFsmPtr->rttStage = newStage;
}

/*****************************************************************************/
/**
 * @brief   Reset RTT FSM
 *
 * @param   rttFsmPtr	a pointer to rtt FSM instance.
 *
 * @return  NONE
 *
 ******************************************************************************/
void RTT_Reset(L2_RTT_FSM *rttFsmPtr)
{
	ASSERT(rttFsmPtr != NULL);
	memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD));
	rttFsmPtr->rttStage = RTT_REQ1;
	rttFsmPtr->isR1Valid = false;
	rttFsmPtr->isR2Valid = false;
	rttFsmPtr->isR3Valid = false;
	rttFsmPtr->isR4Valid = false;
	rttFsmPtr->timeStamp.int_part = 0;
	rttFsmPtr->timeStamp.frac_part = 0;
	rttFsmPtr->doppler = 0;
}

/*****************************************************************************/
/**
 * @brief   Destroy RTT FSM, free allocated memory.
 *
 * @param   rttFsmPtr	a pointer to rtt FSM instance.
 *
 * @return  NONE
 *
 ******************************************************************************/
void RTT_Destroy(L2_RTT_FSM *rttFsmPtr)
{
	free(rttFsmPtr);
}

/*****************************************************************************/
/**
 * @brief   Get the frame start time. If torType == L2_TOR_TX, get the next
 * 			slot start time, if torType == L2_TOR_RX get last frame start
 * 			time.
 *
 * @param   instPtr	a pointer to linklayer layer instance
 * @param   tmStamp	a pointer to store frame start time
 * @param   torType	tx/rx mode
 *
 * @return  NONE
 *
 ******************************************************************************/
void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType)
{
	ASSERT(instPtr != NULL);
	ASSERT(tmStamp != NULL);
	// Get the next frame start time
	if (torType == L2_TOR_TX)
	{
		tmStamp->int_part = L2_GetNextFrameNum(instPtr) * L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr);
		tmStamp->frac_part = 0;
	}
	// Get the last reiceive frame start time
	else if (torType == L2_TOR_RX)
	{
		PHY_INST *phyRef = instPtr->config.phyInstPtr;
		TIME_WORD timeStamp;
		int64_t timeDiff;
		Phy_GetLastReceivedFrameSyncTimeStamp(phyRef, &timeStamp);
		timeDiff = (uint64_t)timeStamp.frac_part;
		timeDiff += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX;
		Phy_GetLastReceivedSlotStartTimeStamp(phyRef, tmStamp);
		timeDiff -= ((uint64_t)tmStamp->int_part) * TIME_FRAC_MAX;
		timeDiff -= (uint64_t)tmStamp->frac_part;
		timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff;
		if (timeDiff < 0)
		{
			timeDiff += TIME_FRAC_MAX;
			tmStamp->int_part--;
			_nop_();
		}
		tmStamp->frac_part = (TIME_FRAC)timeDiff;
	}
	else
	{
		*tmStamp = RTT_TIME_NONE;
		DBG_ERROR("Unexpected tx/rx mode, setting dummy values.\n");
	}
}

/*****************************************************************************/
/**
 * @brief   Initialize RTT FSM, allocate memory for FSM instance.
 *
 * @param   rttFsmPtr
 *
 * @return  SUCCESS Initialization successful.
 *          EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
 *
 ******************************************************************************/
int32_t RTT_Init(L2_RTT_FSM **rttFsmPtr)
{
	(*rttFsmPtr) = (L2_RTT_FSM *)malloc(sizeof(L2_RTT_FSM));
	if (*rttFsmPtr == NULL)
	{
		free(*rttFsmPtr);
		return EALLOC;
	}
	RTT_Reset(*rttFsmPtr);
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Update RTT FSM accordingly.
 *
 * @param   instPtr	a pointer to linklayer layer instance
 * @param	torType tx/rx mode type
 *
 * @return  SUCCESS
 * 			error code
 *
 ******************************************************************************/
int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
{
	ASSERT(instPtr != NULL);
	L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr);
	RTT_RECORD *rttRecord = &(rttFsmPtr->rttRecord);
	PHY_INST *phyRef = instPtr->config.phyInstPtr;

	DBG_INFO("-------- RTT FSM Update --------\n");
	// Stage trans
	// For Head Node: Tx->Rx->Tx->Rx
	if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
	{
		switch (rttFsmPtr->rttStage)
		{
		case RTT_REQ1:
			if (torType == L2_TOR_TX)
			{
				DBG_INFO("Prepare data for RTT REQ-1\n");
				// Update Tt1
				RTT_GetFrameStart(instPtr, &(rttRecord->Tt1), torType);
				DBG_INFO("Tt1-timeL:%u.\n", rttRecord->Tt1.int_part);
				DBG_INFO("Tt1-timeH:%u.\n", rttRecord->Tt1.frac_part);

				// Prepare data for sending...
				rttFsmPtr->timeStamp = rttRecord->Tt1;
				rttFsmPtr->doppler = RTT_DOPPLER_NONE;
				RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
			}
			else
			{
				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
			}
			break;
		case RTT_RSP1:
			if (torType == L2_TOR_RX)
			{
				if (L2_isDataAvail(instPtr) == true)
				{
					DBG_INFO("RTT RSP-1 have been received.\n");
					DBG_INFO("Preparing data for RTT REQ-2.\n");
					rttFsmPtr->isR2Valid = true;
					// Update doppler
					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
					// Update Tr2
					RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType);
					// Calculate TroundA
					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA));
				}
				else
				{
					DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStage);
					DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n");
					rttFsmPtr->isR2Valid = false;
					rttRecord->dopplerR2 = RTT_DOPPLER_NONE;
					rttRecord->Tr2 = RTT_TIME_NONE;
					rttRecord->TroundA = RTT_TIME_NONE;
				}
				DBG_INFO("dopplerR2:%d.\n", rttRecord->dopplerR2);
				DBG_INFO("Tr2-timeL:%u.\n", rttRecord->Tr2.frac_part);
				DBG_INFO("Tr2-timeH:%u.\n", rttRecord->Tr2.int_part);
				DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part);
				DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
				RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
			}
			else
			{
				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
			}
			break;
		case RTT_REQ2:
			if (torType == L2_TOR_TX)
			{
				DBG_INFO("Prepare data for RTT REQ-2\n");
				// Update Tt3
				RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType);

				if (rttFsmPtr->isR2Valid)
				{
					// Calculate reply A
					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA));
				}
				else
				{
					DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
					DBG_WARNING("Set TreplyA to dummy value.\n");
					rttRecord->TreplyA = RTT_TIME_NONE;
				}
				DBG_INFO("Tt3-timeL:%u.\n", rttRecord->Tt3.frac_part);
				DBG_INFO("Tt3-timeH:%u.\n", rttRecord->Tt3.int_part);
				DBG_INFO("TreplyA-timeL:%u.\n", rttRecord->TreplyA.frac_part);
				DBG_INFO("TreplyA-timeH:%u.\n", rttRecord->TreplyA.int_part);

				// Prepare data for sending.
				rttFsmPtr->timeStamp = rttRecord->TroundA;
				rttFsmPtr->doppler = rttRecord->dopplerR2;
				RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
			}
			else
			{
				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
			}
			break;
		case RTT_RSP2:
			if (torType == L2_TOR_RX)
			{
				if (L2_isDataAvail(instPtr) == true)
				{
					DBG_INFO("RTT RSP-2 have been received.\n");
					rttFsmPtr->isR4Valid = true;
					// Update doppler
					double doppler = Phy_GetCoarseFreqEst(phyRef);
					rttRecord->dopplerR2 = (int32_t)doppler;
					// Update Tr4
					RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType);
					// Calculate TroundA
					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA));
				}
				else
				{
					DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
					DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n");
					rttFsmPtr->isR4Valid = false;
					rttRecord->dopplerR4 = RTT_DOPPLER_NONE;
					rttRecord->Tr4 = RTT_TIME_NONE;
					rttRecord->TroundA = RTT_TIME_NONE;
				}
				DBG_INFO("dopplerR4:%d.\n", rttRecord->dopplerR4);
				DBG_INFO("Tr4-timeL:%u.\n", rttRecord->Tr4.frac_part);
				DBG_INFO("Tr4-timeH:%u.\n", rttRecord->Tr4.int_part);
				DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part);
				DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
				RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
				DBG_INFO("RTT procedure end...\n");
			}
			else
			{
				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
			}
			break;
		default:
			DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
			break;
		}
	}
	// For Member Node: Rx->Tx->Rx->Tx
	else
	{
		switch (rttFsmPtr->rttStage)
		{
		case RTT_REQ1:
			if (torType == L2_TOR_RX)
			{
				if (L2_isDataAvail(instPtr) == true)
				{
					DBG_INFO("RTT REQ-1 have been received.\n");
					DBG_INFO("Preparing data for RTT RSP-1.\n");
					rttFsmPtr->isR2Valid = true;
					// Update doppler
					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
					// Update Tr1
					RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType);
				}
				else
				{
					DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
					DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n");
					rttFsmPtr->isR1Valid = false;
					rttRecord->dopplerR1 = RTT_DOPPLER_NONE;
					rttRecord->Tr1 = RTT_TIME_NONE;
				}
				DBG_INFO("dopplerR1:%d.\n", rttRecord->dopplerR1);
				DBG_INFO("Tr1-timeL:%u.\n", rttRecord->Tr1.frac_part);
				DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.int_part);
				RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
			}
			else
			{
				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
			}
			break;
		case RTT_RSP1:
			if (torType == L2_TOR_TX)
			{
				DBG_INFO("Prepare data for RTT RSP-1\n");
				// Update Tt2
				RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType);
				if (rttFsmPtr->isR1Valid)
				{
					// Calculate reply B
					TimeCtrl_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB));
				}
				else
				{
					rttRecord->TreplyB = RTT_TIME_NONE;
					DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
					DBG_WARNING("Set ReplayB to dummy value.\n");
				}
				DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.frac_part);
				DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.int_part);
				DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
				DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part);

				// Prepare data for sending.
				rttFsmPtr->timeStamp = rttRecord->TreplyB;
				rttFsmPtr->doppler = rttRecord->dopplerR1;
				RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
			}
			else
			{
				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
			}
			break;
		case RTT_REQ2:
			if (torType == L2_TOR_RX)
			{
				if (L2_isDataAvail(instPtr) == true)
				{
					DBG_INFO("RTT REQ-2 have been received.\n");
					DBG_INFO("Preparing data for RTT RSP-2.\n");
					rttFsmPtr->isR3Valid = true;
					// Update doppler
					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
					// Update Tr3
					RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType);
					// Calculate TroundB
					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB));
				}
				else
				{
					DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
					DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n");
					rttFsmPtr->isR3Valid = false;
					rttRecord->dopplerR3 = RTT_DOPPLER_NONE;
					rttRecord->TroundB = RTT_TIME_NONE;
				}
				DBG_INFO("dopplerR3:%d.\n", rttRecord->dopplerR3);
				DBG_INFO("Tr3-timeL:%u.\n", rttRecord->Tr3.frac_part);
				DBG_INFO("Tr3-timeH:%u.\n", rttRecord->Tr3.int_part);
				DBG_INFO("TroundB-timeL:%u.\n", rttRecord->TroundB.frac_part);
				DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.int_part);
				RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
			}
			else
			{
				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
			}
			break;
		case RTT_RSP2:
			if (torType == L2_TOR_TX)
			{
				DBG_INFO("Prepare data for RTT RSP-2\n");
				// Update Tt4
				RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType);
				if (rttFsmPtr->isR3Valid)
				{
					// Calculate reply B
					TimeCtrl_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB));
					// TODO: implement a time adjustment.
				}
				else
				{
					DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
					DBG_WARNING("Set ReplayB to dummy value.\n");
					rttRecord->TreplyB = RTT_TIME_NONE;
				}
				DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.frac_part);
				DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.int_part);
				DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
				DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part);

				// Prepare data for sending.
				rttFsmPtr->timeStamp = rttRecord->TreplyB;
				rttFsmPtr->doppler = rttRecord->dopplerR3;
				RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
				DBG_INFO("RTT procedure end...\n");
			}
			else
			{
				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
			}
			break;
		default:
			DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
			break;
		}
	}
	return SUCCESS;
}