#include "LindarPos.h"
#include "OrdLidar.h"
#include "KMachine.h"

int ProcessPos(int nLidarIdx, stLidarDot * pLindarDots, int nCount)
{
	int nValidCount0 = nCount;
	int minDisIndex = 0;		int minDistance = 55555;
	int minZ = 10000; int minZIndex= -1;
	int planeCount =0;			// ×îµ×²ãƽÃæµÄµãÊýÁ¿¡£
	int firstmin = 1000,firstmax = -1000;
	int secondmin = 1000, secondmax = -1000;

	int firstCount=0; int secondCount =0;			int midCount =0;
	results[0]=0;
	// Çó minZ ºÍ minDistance;
	for (int j = 0;j < nValidCount0;j++){
		if (pLindarDots[j].distance < 50) continue;		// skip too small points;
		if (pLindarDots[j].y < 40) continue;		// skip too small points;							
		if (pLindarDots[j].x < -200 || pLindarDots[j].x > 500) continue;							
		if (pLindarDots[j].y < minZ) {	minZIndex = j; minZ = pLindarDots[j].y;	}
		if (pLindarDots[j].distance < minDistance) {
			minDisIndex = j; minDistance = pLindarDots[j].distance;
		}	
	}

	for (int j = nValidCount0 -1 ;j>=0 && minZ < 500;j--)
	{
		int x = pLindarDots[j].x;
		int y = pLindarDots[j].y;
		int d = pLindarDots[j].distance;		
		
		if (d < 50) continue;		// skip too small points;
		if (y < 40) continue;		// skip too small points;												
		if (x < -200 || x > 500) continue;									
		// Ö»²éÕÒ µ×²¿ 50mm Êý¾Ý

		//results[0]++;							
		if (y >= minZ + 50) {
			if ((firstCount >0 && secondCount == 0) 
				|| (secondCount >0 && firstCount == 0)) {
				midCount++;
			}
			continue;
		}

		if (planeCount < 40)	{
			KMem.WDT[40+2 * planeCount]=d;
			KMem.WDT[40+2 * planeCount +1 ]=x;
		}
			
		planeCount++;
		if (x > -200 && x < 500) 
			{
			if (x < firstmin)  firstmin = x;
			if (x > secondmax) secondmax = x;
		}
			// ºóÑØ
		if (x > -200 && x < 100 && y < minZ + 40 )
		{
			if (x > firstmax)  firstmax = x;
			firstCount++;
		}
			// Ç°ÑØ
		if (x > 200 && x < 500 && y < minZ + 40 )
		{
			if (x < secondmin)  secondmin = x;
			secondCount ++;
		}

	}
	results[0]=planeCount;
	results[1]=firstCount;
	results[2]=midCount;
	results[3]=secondCount;

	results[4]=firstmin;
	results[5]=firstmax;
	results[6]=secondmin;
	results[7]=secondmax;
	int avg=0;
	if (firstCount > 0 && secondCount > 0 && midCount > 0 ){
		avg = (secondmin + firstmax) / 2;
	}else if (planeCount>10) {
		avg = (firstmin + secondmax) /2;
	}

	nPosX = (nPosX *3 + avg) /4;
	if (minZ < 9999) nPosZ1 = (minZ + nPosZ1 * 3)/4;	
	return 0;
	
	
	
}