#include "peakCentering.h"
#include "stdlib.h"

void peakCentering(
	std::vector<RgnPix> 	&InRgnPnt,
	std::vector<RgnPix> 	&OutCenteringPnt
)
{
	//ap_uint<32> frmNo=0;
	//ap_uint<32> timeStamp=0;
	//ap_uint<32> encInfo=0;
	//ap_uint<16> frmW=0, frmH=0, frmX=0, frmY=0;
	ushort WinNum = 0;
	RgnPix 	InData;
	int readPtr = 0;
	InData = InRgnPnt[readPtr++];
	uchar VSync = InData.Sync & 0x02;
	if(0 == VSync)  //wait for syncFirst
		return;

	//1st: frameNo
	//frmNo(15,0) = InData.winStartX.range(15,0);
	//frmNo(31,16) = InData.y.range(15,0);
	OutCenteringPnt.push_back(InData);
	//2nd: timeStamp
	InData = InRgnPnt[readPtr++];
	//timeStamp(15,0) = InData.winStartX.range(15,0);
	//timeStamp(31,16) = InData.y.range(15,0);
	OutCenteringPnt.push_back(InData);
	//3rd:encInfo
	InData = InRgnPnt[readPtr++];
	//encInfo(15,0) = InData.winStartX.range(15,0);
	//encInfo(31,16) = InData.y.range(15,0);
	OutCenteringPnt.push_back(InData);
	//4th: frmW, frmH
	InData = InRgnPnt[readPtr++];
	//frmW(15,0) = InData.winStartX.range(15,0);
	//frmH(15,0) = InData.y.range(15,0);
	OutCenteringPnt.push_back(InData);
	//5th: frmX, WinNum
	InData = InRgnPnt[readPtr++];
	//frmX(15,0) = InData.winStartX.range(15,0);
	WinNum = InData.LazerWinY;
	OutCenteringPnt.push_back(InData);
	//6th: frmY
	InData = InRgnPnt[readPtr++];
	//frmY(15,0) = InData.winStartX.range(15,0);
	OutCenteringPnt.push_back(InData);
	RgnPix zeroData = {0,0,0,0,0,0,0};
	//ping-pong buffer
	RgnPix HSyncData_d1 = {0,0,0,0,0,0,0};
	uchar lineBuff_0[RGN_DATA_WIN_SIZE * 2];
//#pragma HLS RESOURCE variable=lineBuff_0 core=RAM_2P
	uchar lineBuff_1[RGN_DATA_WIN_SIZE * 2];
//#pragma HLS RESOURCE variable=lineBuff_1 core=RAM_2P

	//initialize buffer
	for(int m = 0; m < RGN_DATA_WIN_SIZE * 2; m ++)
	{
		lineBuff_0[m] = 0;
		lineBuff_1[m] = 0;
	}

	PeakRun linePk = {0,0,0};
	uchar pkCenter = 0;

	uchar lastData = 0;;
	PeakRun currPeak = {0,0,0};
	uchar SM = 0b00;
	uchar evenFlag = 0;
	uchar pkRng_S, pkRng_E;
	for(int n = 0; n <= WinNum; n ++)
	{
		for(int i = 0; i <= 8; i ++)
		{

			RgnPix InData;
			if(n < WinNum)
				InData = InRgnPnt[readPtr++];
			else
				InData = zeroData;

			if(0 == i)  //HSync
			{
				if ((n > 0) &&( linePk.len>0))
					OutCenteringPnt.push_back(HSyncData_d1);
				HSyncData_d1 = InData;

				pkRng_S = InData.RltvRdx & 0x0f;
				pkRng_S += RGN_DATA_WIN_SIZE / 2;
				InData.RltvRdx = InData.RltvRdx >> 4;
				pkRng_E = InData.RltvRdx & 0x0f;
				pkRng_E += RGN_DATA_WIN_SIZE / 2;

				evenFlag = n%2;
				if(0 == linePk.len)
					pkCenter = RGN_DATA_WIN_SIZE/2;
				else
					pkCenter = linePk.start + (linePk.len >> 1);
				linePk.start = 0;
				linePk.len = 0;
				linePk.value = 0;
				lastData = 0;
				currPeak.start = 0;
				currPeak.len = 0;
				currPeak.value = 0;
				SM = 0b00; //state machine: 0b00=init state; 0b01=rising; 0b10=falling
			}
			else//for(int i = 0; i < 8; i ++)
			{
				uchar wrPos_0 = i * 2 - 2 + RGN_DATA_WIN_SIZE/2;
				uchar wrPos_1 = i * 2 - 1 + RGN_DATA_WIN_SIZE/2;
				uchar rdPos_0 = pkCenter - RGN_DATA_WIN_SIZE/2 + i* 2 -2;
				uchar rdPos_1 = pkCenter - RGN_DATA_WIN_SIZE/2 + i* 2 -1;

				uchar data_0 = InData.LazerWinX & 0xff;
				uchar data_1 = InData.LazerWinY & 0xff;

				uchar out_0, out_1;
				if(0 == evenFlag)
				{
					lineBuff_0[wrPos_0] = data_0;
					lineBuff_0[wrPos_1] = data_1;
					out_0 = lineBuff_1[rdPos_0];
					out_1 = lineBuff_1[rdPos_1];
				}
				else
				{
					lineBuff_1[wrPos_0] = data_0;
					lineBuff_1[wrPos_1] = data_1;
					out_0 = lineBuff_0[rdPos_0];
					out_1 = lineBuff_0[rdPos_1];
				}
				//find peak
				uchar preCenter = currPeak.start + (currPeak.len >> 1);  //old peak center
				uchar preCenter_1 = currPeak.start + ((currPeak.len+1) >> 1);  //old peak center plus one
				switch(SM)
				{
				case 0b00:
					if(data_0 < data_1) //rising
					{
						currPeak.start = wrPos_1;
						currPeak.len = 1;
						currPeak.value = data_1;
						SM = 0b01;
					}
					else if(data_0 > data_1) //falling
						SM = 0b10;
					break;
				case 0b01:  //rising
					if(lastData < data_0)
					{
						if(data_0 < data_1)
						{
							currPeak.start = wrPos_1;
							currPeak.len = 1;
							currPeak.value = data_1;
						}
						else if(data_0 > data_1)  //change to falling
						{
							if( (wrPos_0 >= pkRng_S) && (wrPos_0 <= pkRng_E) && (data_0 >linePk.value ))
							{
								linePk.start = wrPos_0;
								linePk.len = 1;
								linePk.value = data_0;
							}
							SM = 0b10;
						}
						else
						{
							currPeak.start = wrPos_0;
							currPeak.len = 2;
							currPeak.value = data_0;
						}

					}
					else if(lastData > data_0)
					{
						//compare with exist
						if( (preCenter >= pkRng_S) && (preCenter <= pkRng_E) && (currPeak.value > linePk.value))
						{
							linePk = currPeak;
						}
						if(data_0 < data_1)
						{
							currPeak.start = wrPos_1;
							currPeak.len = 1;
							currPeak.value = data_1;
						}
						else
							SM = 0b10;  //change to falling
					}
					else  //lastData == data_0
					{
						if(data_0 < data_1)
						{
							currPeak.start = wrPos_1;
							currPeak.len = 1;
							currPeak.value = data_1;
						}
						else if(data_0 > data_1)
						{
							if( (preCenter_1 >= pkRng_S) && (preCenter_1 <= pkRng_E) && (currPeak.value > linePk.value))
							{
								linePk.start = currPeak.start;
								linePk.value = currPeak.value;
								linePk.len = currPeak.len + 1;
							}
							SM = 0b10;  //change to falling
						}
						else //data_0 == dta_1
							currPeak.len += 2;
					}
					break;
				case 0b10: //falling
					if(lastData >= data_0)
					{
						if(data_0 < data_1)
						{
							currPeak.start = wrPos_1;
							currPeak.len = 1;
							currPeak.value = data_1;
							SM = 0b01;  //rising
						}
					}
					else  //lastData < data_0
					{
						if(data_0 < data_1)
						{
							currPeak.start = wrPos_1;
							currPeak.len = 1;
							currPeak.value = data_1;
							SM = 0b01;  //rising
						}
						else if(data_0 == data_1)
						{
							currPeak.start = wrPos_0;
							currPeak.len = 2;
							currPeak.value = data_0;
							SM = 0b01;  //rising
						}
						else //data_0 > data_1
						{
							//data_0 is a peak
							if( (wrPos_0 >= pkRng_S) && (wrPos_0 <= pkRng_E) && (currPeak.value > linePk.value))
							{
								linePk.start = wrPos_0;
								linePk.value = data_0;
								linePk.len = 1;
							}
						}
					}
					break;
				default:
					break;
				}
				lastData = data_1; //prepare for next clock

				//data output: delay one line
				RgnPix stmOut = {0,0,0,0,0,0,0};
				stmOut.Sync = 0b00;
				stmOut.LazerWinX = out_0;
				stmOut.LazerWinY = out_1;
				if( (n > 0 ) && (pkCenter > 0) )
				{
					OutCenteringPnt.push_back(stmOut);
				}
			}
		}
	}
}