/**
@file Src_Template.c
@brief this is a template code for safertos Developer
*/

/********************************************************************************************
Debug Switch Definition Section
********************************************************************************************/

/********************************************************************************************
Include File Section
********************************************************************************************/
#include "seqproc.h"
#include "log_creat.h"
/********************************************************************************************
Macro Definition Section
********************************************************************************************/
#define SeqProc_MAX_DELAY	SAFETYOS_MAX_DELAY
#define SeqProc_IMDT_START	( ( tick_t ) 0 )

/********************************************************************************************
Prototype Declaration Section
********************************************************************************************/
static base_t xSeqProc_SingleStep (xSeqProc_st* pxSeqProc,const void* arg);
/********************************************************************************************
Const Global Variable Definition Section
********************************************************************************************/

/********************************************************************************************
Global Variable Definition Section
********************************************************************************************/

/********************************************************************************************
Static Variable Definition Section
********************************************************************************************/

/********************************************************************************************
Structure Type Definition Section
********************************************************************************************/

/********************************************************************************************
Function Definition Section
********************************************************************************************/

void vSeqProc_InitProc (xSeqProc_st* pxSeqProc)
{
	pxSeqProc->xInProgress = -1;
	pxSeqProc->xWaitTick = SeqProc_MAX_DELAY;
	pxSeqProc->xComputing = xTaskGetTickCount();

	for (int  inprog = 0 ; inprog < SeqProc_MaxNestTimes; inprog++) {
		pxSeqProc->xfunc[inprog] = SAF_NULL;
		pxSeqProc->xStage[inprog] = SeqProc_StageNone;
	}
}

void vSeqProc_KillProc (xSeqProc_st* pxSeqProc)
{
	pxSeqProc->xInProgress = -1;
	pxSeqProc->xWaitTick = SeqProc_MAX_DELAY;
	pxSeqProc->xComputing = xTaskGetTickCount();

	for (int  inprog = 0 ; inprog < SeqProc_MaxNestTimes; inprog++) {
		pxSeqProc->xfunc[inprog] = SAF_NULL;
		pxSeqProc->xStage[inprog] = SeqProc_StageNone;
	}
}

base_t xSeqProc_Call(xSeqProc_st* pxSeqProc,SeqProcfunc proc,const void* arg)
{
	base_t inprog;
	inprog = pxSeqProc->xInProgress + 1;

	if (inprog < SeqProc_MaxNestTimes)
	{
		/* invoke this sub-procedure */
		pxSeqProc->xfunc[inprog] = proc;
		pxSeqProc->xStage[inprog] = 0;
		pxSeqProc->xInProgress = inprog;
		
		if (xSeqProc_SingleStep(pxSeqProc,arg))
		{
			vSeqProc_Exec(pxSeqProc);
		}
		else
		{
			;/*do nothing*/
		}
		return SeqProc_CallOk;
	}
	else 
	{
		vSeqProc_KillProc(pxSeqProc);
		return SeqProc_CallError;
	}
}

static base_t xSeqProc_SingleStep (xSeqProc_st* pxSeqProc,const void* arg)
{
	int inprog = pxSeqProc->xInProgress;
	SeqProcfunc proc  = pxSeqProc->xfunc[inprog];
	SeqProcStage stage = pxSeqProc->xStage[inprog];
	uint32_t cont = proc(stage, arg);

	switch (cont & 0xFFFFFF00U)//cont:0x7401
	{
		case SeqProc_Imdt:
			pxSeqProc->xStage[inprog] = (cont & 0x00FF);
			return 1;

		case SeqProc_Complete:
			pxSeqProc->xStage[inprog] = SeqProc_StageNone;
			pxSeqProc->xfunc[inprog] = SAF_NULL;
			pxSeqProc->xInProgress -= 1;
			pxSeqProc->xWaitTick = SeqProc_IMDT_START;
			return 0;

		case SeqProc_Error:
			vSeqProc_KillProc(pxSeqProc);
			return 0;
			
		default:  /* SeqProc_WaitTime */
			pxSeqProc->xStage[inprog] = (cont & 0x00FF);
			pxSeqProc->xWaitTick = ((cont - SeqProc_WaitTime) >> 8);//0x64
			pxSeqProc->xComputing = xTaskGetTickCount();
			return 0;
	}
}

void vSeqProc_Exec (xSeqProc_st* pxSeqProc)
{
	int cont;
	pxSeqProc->xComputing = xTaskGetTickCount();

	do {
		if (pxSeqProc->xInProgress >= 0) {
			cont = xSeqProc_SingleStep(pxSeqProc,0);
		}
		if (pxSeqProc->xInProgress < 0) {
			/* top-level procedure has returned, terminate this process */
			cont = 0;
			pxSeqProc->xWaitTick = SeqProc_MAX_DELAY;
		}
	} while (cont);
}

 void vSeqProc_CalibriTimeout (xSeqProc_st* pxSeqProc)
{
	tick_t start = pxSeqProc->xComputing;	
	
	if ((pxSeqProc->xWaitTick != SeqProc_MAX_DELAY )&&
	(pxSeqProc->xWaitTick != SeqProc_IMDT_START))
	{
		tick_t cur, diff;
		cur = xTaskGetTickCount();
		if (start <= cur)
				 diff = cur - start;
		else
				diff = cur + (0xFFFFFFFFU - start);
		
		if (diff < (tick_t) pxSeqProc->xWaitTick) {
				pxSeqProc->xWaitTick -= diff;
		}
		else {
				pxSeqProc->xWaitTick = SeqProc_IMDT_START;
		}

		pxSeqProc->xComputing = cur;
	}
}

int8_s cSeqProc_IsInProgress(xSeqProc_st* pxSeqProc)
{
	return  (pxSeqProc->xInProgress +1);
}

