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

void peakCentering(
	hls::stream<RgnPix> 	&InRgnPnt,
	hls::stream<RgnPix> 	&OutCenteringPnt
)
{
#if !(VITIS_HLS)
#pragma HLS DATA_PACK variable=OutCenteringPnt
#pragma HLS DATA_PACK variable=InRgnPnt
#endif
#pragma HLS INTERFACE axis register both port=OutCenteringPnt
#pragma HLS INTERFACE axis register both port=InRgnPnt

	//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;
	ap_uint<12> WinNum = 0;
	RgnPix 	InData;
	InData = InRgnPnt.read();
	ap_uint<1> VSync = InData.Sync.range(1,1);
	if(0b0 == VSync)  //wait for syncFirst
		return;

	//1st: frameNo
	//frmNo(15,0) = InData.winStartX.range(15,0);
	//frmNo(31,16) = InData.y.range(15,0);
	OutCenteringPnt.write(InData);
	//2nd: timeStamp
	InData = InRgnPnt.read();
	//timeStamp(15,0) = InData.winStartX.range(15,0);
	//timeStamp(31,16) = InData.y.range(15,0);
	OutCenteringPnt.write(InData);
	//3rd:encInfo
	InData = InRgnPnt.read();
	//encInfo(15,0) = InData.winStartX.range(15,0);
	//encInfo(31,16) = InData.y.range(15,0);
	OutCenteringPnt.write(InData);
	//4th: frmW, frmH
	InData = InRgnPnt.read();
	//frmW(15,0) = InData.winStartX.range(15,0);
	//frmH(15,0) = InData.y.range(15,0);
	OutCenteringPnt.write(InData);
	//5th: frmX, WinNum
	InData = InRgnPnt.read();
	//frmX(15,0) = InData.winStartX.range(15,0);
	WinNum(11,0) = InData.LazerWinY.range(11,0);
	OutCenteringPnt.write(InData);
	//6th: frmY
	InData = InRgnPnt.read();
	//frmY(15,0) = InData.winStartX.range(15,0);
	OutCenteringPnt.write(InData);
	RgnPix zeroData = {0,0,0,0,0,0,0};
	//ping-pong buffer
	RgnPix HSyncData_d1 = {0,0,0,0,0,0,0};
	ap_uint<12> LazerWinX_d1 = 0;
	ap_uint<8> lineBuff_0[RGN_DATA_WIN_SIZE * 2];
#if VITIS_HLS
#pragma HLS BIND_STORAGE variable=lineBuff_0 type=RAM_2p
#else
//#pragma HLS RESOURCE variable=lineBuff_0 core=RAM_2P
#endif

	ap_uint<8> lineBuff_1[RGN_DATA_WIN_SIZE * 2];
#if VITIS_HLS
#pragma HLS BIND_STORAGE variable=lineBuff_0 type=RAM_2p
#else
//#pragma HLS RESOURCE variable=lineBuff_1 core=RAM_2P
#endif

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

	PeakRun linePk = {0,0,0};
	ap_uint<8> pkCenter = 0;

	ap_uint<8> lastData = 0;;
	PeakRun currPeak = {0,0,0};
	ap_uint<2> SM = 0b00;
	ap_uint<1> evenFlag = 0;
	ap_uint<5> pkRng_S, pkRng_E;
	for(int n = 0; n <= WinNum; n ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
		for(int i = 0; i <= 8; i ++)
		{
#pragma HLS PIPELINE II=1

			RgnPix InData;
			if(n < WinNum)
				InData = InRgnPnt.read();
			else
				InData = zeroData;

			if(0 == i)  //HSync
			{
				evenFlag = n%2;
				if(0 == linePk.len)
					pkCenter = RGN_DATA_WIN_SIZE/2;
				else
					pkCenter = linePk.start + (linePk.len >> 1);
				HSyncData_d1.LazerWinX = LazerWinX_d1 + pkCenter;

				if ((n > 0) &&( linePk.len>0))
					OutCenteringPnt.write(HSyncData_d1);
				HSyncData_d1 = InData;
				LazerWinX_d1 = InData.LazerWinX - RGN_DATA_WIN_SIZE;

				pkRng_S = InData.RltvRdx.range(3,0) + RGN_DATA_WIN_SIZE/2;
				pkRng_E = InData.RltvRdx.range(7,4) + RGN_DATA_WIN_SIZE/2;

				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 ++)
			{
				ap_uint<8> wrPos_0 = i * 2 - 2 + RGN_DATA_WIN_SIZE/2;
				ap_uint<8> wrPos_1 = i * 2 - 1 + RGN_DATA_WIN_SIZE/2;
				ap_uint<8> rdPos_0 = pkCenter - RGN_DATA_WIN_SIZE/2 + i* 2 -2;
				ap_uint<8> rdPos_1 = pkCenter - RGN_DATA_WIN_SIZE/2 + i* 2 -1;

				ap_uint<8> data_0 = InData.LazerWinX.range(7,0);
				ap_uint<8> data_1 = InData.LazerWinY.range(7,0);

				ap_uint<8> 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
				ap_uint<8> preCenter = currPeak.start + (currPeak.len >> 1);  //old peak center
				ap_uint<8> 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(7,0) = out_0;
				stmOut.LazerWinY(7,0) = out_1;
				if( (n > 0 ) && (pkCenter > 0) )
				{
					OutCenteringPnt.write(stmOut);
				}
			}
		}
	}

}