#include "SPCE061A.h"

#define M_TIMERA_EN       0x0001						
#define M_TIMERA_STOP	  0x0006						

unsigned int  M_TIMER_DELAY=400;						
unsigned int  M_LOW_DISTEN=75;
unsigned int  M_TIMER_OFFSET=30;
unsigned int  M_TIMER_SEND=25;
unsigned int  M_MUT_DATA=33500;

unsigned int uiIOB_20Hz_flag;							
unsigned int uiEXT_EN_flag;								
unsigned int uiResult1;
unsigned int uiMeasure_Index;							
unsigned int uiResult[4];							

unsigned int ResultOS(void);
void ErrorOS(void);
void InitMeasureData(void);

unsigned int uiError_Counter;						

void InitMeasure(void)
{

	*P_SystemClock = 0x000b;
	
	*P_IOB_Dir = 0x0008;
	*P_IOB_Attrib = 0x0008;
	*P_IOB_Data = 0x0004;
	
	*P_TimeBase_Setup=0x0001;
	
	*P_TimerB_Data = 0x0000;
	*P_TimerB_Ctrl = M_TIMERA_STOP;

	__asm("FIQ OFF");
	__asm("IRQ ON");
	
	InitMeasureData();
}

void InitMeasureData(void)
{
	uiIOB_20Hz_flag = 0;							
	uiEXT_EN_flag = 0;
	
	uiMeasure_Index = 0;
}



void User_Delay(unsigned int iTimers)
{
	unsigned int i;
	for(i=0;i<iTimers;i++)
	{
		__asm("nop");
	}
}
void US_Send200us(void)
{
	*P_TimerB_Data = 0x0000;							
	*P_IOB_Data=*P_IOB_Buffer|0x0008;
	*P_TimerB_Ctrl = M_TIMERA_EN;						
	User_Delay(M_TIMER_SEND);							
	*P_IOB_Data=*P_IOB_Buffer&0xfff7;					
	uiIOB_20Hz_flag = 1;								
}
unsigned int BeginMeasure(void)
{
	unsigned int uiExit_RQ;								
	unsigned int uiMeasure_Result;						
	unsigned int Temp = 0, Temp2 = 0;
	int i = 0, j = 0;
	
	*P_SystemClock = 0x000b;							
	uiExit_RQ = 1;
	
	uiMeasure_Index = 0;								
	*P_INT_Ctrl = *P_INT_Ctrl_New|0x0002;				
	while(uiExit_RQ)
	{
		if(uiIOB_20Hz_flag)								
		{
			if(*P_TimerB_Data>=M_TIMER_DELAY)			
			{											
				*P_INT_Clear = 0x0100;					
				*P_INT_Ctrl = *P_INT_Ctrl_New|0x0100;
				uiEXT_EN_flag = 1;
				uiIOB_20Hz_flag = 0;				
			}
		}
		if(*P_TimerB_Data>900)														
		{
			*P_INT_Ctrl = *P_INT_Ctrl_New&(~0x0100);	
			uiEXT_EN_flag = 0;
			*P_TimerB_Ctrl = M_TIMERA_STOP;				
			
			i = uiMeasure_Index;
			j = 0;
			Temp = 900;
			while(i)
			{
				Temp2 = uiResult[j];
				uiResult[j] = Temp;
				Temp = Temp2;
				j++;
				i--;
			}
		}
		
		if(uiMeasure_Index>4)							
		{
			uiMeasure_Index=0;
			uiMeasure_Result = ResultOS();				
			if(uiMeasure_Result<=M_LOW_DISTEN)
			{
				uiMeasure_Result = 0;					
			}
			uiExit_RQ=0;
			break;
		}
		*P_Watchdog_Clear = 0x0001;
	}
	return uiMeasure_Result;							
}


unsigned int ResultOS(void)
{
	unsigned long int uliResult2;	

	unsigned int i;
	if(uiResult[0]<=(M_TIMER_DELAY+M_TIMER_OFFSET))
	{
		return 0;
	}
	uiResult1 = (uiResult[1]+uiResult[2])/2-3;			
	uliResult2 = (unsigned long int)uiResult1*M_MUT_DATA;
	uliResult2 = uliResult2/96;
	uliResult2 = uliResult2/2;
	
	uliResult2 = uliResult2-20;
	uliResult2 = uliResult2/1000;
	
	
	uiResult1 = (unsigned int)uliResult2;
	for(i=0;i<4;i++)
	{
		uiResult[i]=0;
	}
	return uiResult1;
}


void ErrorOS(void)
{
	unsigned int i;
	i=0;
}