#include "OrdLidar.h"

#include "KMachine.h"

int nPosX;
int nPosY;
int nPosZ;

int nPosZ1,nPosZ2;

int pCount1=0;
int pCount2=0;

int eCount1;
int eCount2;

int dCount1=0;
int dCount2=0;

int vCount1=0;
int vCount2=0;

int results[32];


void Uart3SendPacket(char * str, int len);
void Uart5SendPacket(char * str, int len);

//ÕýÏÒ±í£¬0 - 360 ´ú±í 0 - 2PI,½á¹û0 - 1000 ±íʾ0 - 1£¬±íÖÐÖ»ÓÐ0 - PI/2¼´1/4ÖÜÆÚµÄÊý¾Ý£¬ÆäËûµÄÊý¾ÝÓöԳƺ;µÏñµÃµ½¡£
const int SinTable[] =  
{
0,17,35, 52, 70, 87, 105, 122, 139, 156, 174, 191, 208, 225, 242, 259, 276, 292,
309, 326, 342, 358, 375, 391, 407, 423, 438, 454, 469, 485, 500, 515, 530, 545,
559, 574, 588, 602, 616, 629, 643, 656, 669, 682, 695, 707, 719, 731, 743, 755,
766, 777, 788, 799, 809, 819, 829, 839, 848, 857, 866, 875, 883, 891, 899, 906,
914, 921, 927, 934, 940, 946, 951, 956, 961, 966, 970, 974, 978, 982, 985, 988,
990, 993, 995, 996, 998, 999, 999, 1000, 1000,
1000, 999, 999, 998, 996, 995,
993, 990, 988, 985, 982, 978, 974, 970, 966, 961, 956, 951, 946, 940, 934, 927,
921, 914, 906, 899, 891, 883, 875, 866, 857, 848, 839, 829, 819, 809, 799, 788,
777, 766, 755, 743, 731, 719, 707, 695, 682, 669, 656, 643, 629, 616, 602, 588,
574, 559, 545, 530, 515, 500, 485, 469, 454, 438, 423, 407, 391, 375, 358, 342,
326, 309, 292, 276, 259, 242, 225, 208, 191, 174, 156, 139, 122, 105, 87, 70,
52, 35, 17, 0, -17, -35, -52, -70, -87, -105, -122, -139, -156, -174, -191,
-208, -225, -242, -259, -276, -292, -309, -326, -342, -358, -375, -391, -407,
-423, -438, -454, -469, -485, -500, -515, -530, -545, -559, -574, -588, -602,
-616, -629, -643, -656, -669, -682, -695, -707, -719, -731, -743, -755, -766,
-777, -788, -799, -809, -819, -829, -839, -848, -857, -866, -875, -883, -891,
-899, -906, -914, -921, -927, -934, -940, -946, -951, -956, -961, -966, -970,
-974, -978, -982, -985, -988, -990, -993, -995, -996, -998, -999, -999, -1000,
-1000, -1000, -999, -999, -998, -996, -995, -993, -990, -988, -985, -982, -978,
-974, -970, -966, -961, -956, -951, -946, -940, -934, -927, -921, -914, -906,
-899, -891, -883, -875, -866, -857, -848, -839, -829, -819, -809, -799, -788,
-777, -766, -755, -743, -731, -719, -707, -695, -682, -669, -656, -643, -629,
-616, -602, -588, -574, -559, -545, -530, -515, -500, -485, -469, -454, -438,
-423, -407, -391, -375, -358, -342, -326, -309, -292, -276, -259, -242, -225,
-208, -191, -174, -156, -139, -122, -105, -87, -70, -52, -35, -17
};



char StartCMD[2] = { 0xA5,0x60 };
char StopCMD[2] = { 0xA5,0x65 };


int StartAngle;
int EndAngle;

int OrdLidarStart(int nIdx)
{
	if (nIdx == 0 || nIdx == -1 ) {
		Uart3SendPacket(StartCMD,sizeof(StartCMD));
	}
	if (nIdx == 1 || nIdx == -1) {
		Uart5SendPacket(StartCMD,sizeof(StartCMD));
	}
		return 0;
};


int OrdLidarStop(int nIdx){
	if (nIdx == 0 || nIdx == -1 ) {
		Uart3SendPacket(StopCMD,sizeof(StopCMD));
	}
	if (nIdx == 1 || nIdx == -1) {
		Uart5SendPacket(StopCMD,sizeof(StopCMD));
	}
	return 0;
};

char startFlag0 = 0;
char startFlag1 = 0;

#define VALID_DATA_MAX 400
stLidarDot validData0[400];
stLidarDot validData1[400];

int nValidCount0 = 0;
int nValidCount1 = 0;

#define ORG_SUB_DEGREE 100
#define SUB_DEGREE 64		//64

#define START_DEGREE 110
#define END_DEGREE 250

#define MAX_DISTANCE 500				//mm

//ÕûÊýÇóSINÖµ£¬´øÏßÐÔ²åÖµ¹¦ÄÜ£¬ ÊäÈë0 - 360*64 Ϊ 0 - 2PI,Ò»¸öÖÜÆÚ;Êä³ö-1000 ÖÁ +1000,´ú±í-1µ½+1;
int sini(int a)
{
	if (a <0) {a += 360 * SUB_DEGREE;}
	a = a % (360 * SUB_DEGREE);
	int b;
	b=a / SUB_DEGREE;
	int xx = a % SUB_DEGREE;
	if (b< 90 )
	{
		int d1= SinTable[b ];
		int d2 =SinTable[b+1 ];
		int d;
		d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
		
		return (d);
	}
	else if (b<180 )
	{	
		int c;
		c=180-b;
		
		int d1= SinTable[c ];
		int d2 =SinTable[c-1 ];
		int d;
		d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
		
		return (d);
	}
	
	else if (b<270)
	{
		int c;
		c=b-180;
		int d1= SinTable[c ];
		int d2 =SinTable[c+1 ];
		int d;
		d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
		
		return (-d);
	}
	else 
	{
		int c;
		c=360 -b;
		int d1= SinTable[c ];
		int d2 =SinTable[c-1 ];
		int d;
		d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
		
		return (-d);
	}
	
//	return a;
}

int cosi(int a)
{
	return sini(a+90*SUB_DEGREE);
}

int OrdLidarParsePkt(int nLidarIdx, OradarLidarFrame * pLindarPkt, int len1)
{
	int iRet = 0;
	if (pLindarPkt->header != 0x54) return 0;		// check for start sign
	int nDotNum = pLindarPkt->ver_len &0x1f;
	if (len1 != nDotNum*3 +11) return -1;
	if (nDotNum<2) return 0;
	
	int startAngle = (pLindarPkt->start_angle) * SUB_DEGREE / ORG_SUB_DEGREE;	// degree * 100;
	int endAngle = (pLindarPkt->end_angle ) * SUB_DEGREE / ORG_SUB_DEGREE; 		// degree * 100;
	if (startAngle >= endAngle)	{eCount1++; return 0;}
	int diffAngle = (endAngle - startAngle);
	int eachAngle = diffAngle / (nDotNum - 1);	
	
	if (nLidarIdx == 0) 
	{
		pCount1++;
		for (int i = 0;i < nDotNum && i < 40 ;i++)
		{
			unsigned char confidence = pLindarPkt->point[i].confidence;
			int angle = eachAngle * i + startAngle;	// degree * 64
			dCount1++;
			
			if (angle < START_DEGREE * SUB_DEGREE )	startFlag0 = 1;
			if (angle >= START_DEGREE * SUB_DEGREE && angle < END_DEGREE * SUB_DEGREE) // &&x>100000&&x<250000)
			{
				if (pLindarPkt->point[i].confidence < 30) continue;
				int value = pLindarPkt->point[i].distance; // distance
				
				int x = value * sini(angle  )/1000;			// 	mm
				int z = -value * cosi(angle )/1000;			//  mm
				
				//vectorX.push_back(x);
				//vectorY.push_back(y);
				validData0[nValidCount0].distance = value;
				validData0[nValidCount0].x = x;
				validData0[nValidCount0].y = z;
				if (nValidCount0 < VALID_DATA_MAX - 1 ) nValidCount0 ++;
				//vCount1++;
			}

			if (angle > END_DEGREE * SUB_DEGREE)
			{
				if (startFlag0 == 1 && nValidCount0 > 0)
				{
						vCount1 = nValidCount0;
					ProcessPos(0,validData0,nValidCount0);
					

				}
				startFlag0 = 0;
				nValidCount0 = 0;
			}
		}
	}

	if (nLidarIdx == 1) 
	{
		pCount2++;
		for (int i = 0;i < nDotNum && i < 40 ;i++)
		{
			unsigned char confidence = pLindarPkt->point[i].confidence;
			int angle = eachAngle * i + startAngle;

			dCount2++;
			
			if (angle < START_DEGREE * SUB_DEGREE)	startFlag1 = 1;
			if (angle >= START_DEGREE * SUB_DEGREE && angle < END_DEGREE * SUB_DEGREE ) // &&x>100000&&x<250000)
			{
				if (confidence < 30) continue;				
				int value = pLindarPkt->point[i].distance; // distance

				int x = value * sini(angle - 180 * SUB_DEGREE )/1000;			//	mm
				int y = value * cosi(angle - 180 * SUB_DEGREE )/1000;			//	mm //vectorX.push_back(x);
				//vectorY.push_back(y);
				validData1[nValidCount1].distance = value; 
				validData1[nValidCount1].x = x;
				validData1[nValidCount1].y = y;
				if (nValidCount1 < VALID_DATA_MAX - 1 ) nValidCount1 ++;				
				//vCount2 ++;
			}

			if (angle > END_DEGREE * SUB_DEGREE)
			{
				if (startFlag1 == 1 && nValidCount1 > 0 )
				{
						vCount2 = nValidCount1;

						int minDisIndex = 0;	int minDistance = 55555;
						int minZ = 9999; 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;
					
						// Çó minZ ºÍ minDistance;
						for (int j = 0;j < nValidCount1;j++){
							if (validData1[j].distance < 50) continue;		// skip too small points;
							if (validData1[j].y < 40) continue;		// skip too small points;													
							if (validData1[j].x < -200 || validData1[j].x > 500) continue;									
							if (validData1[j].y < minZ) {	minZIndex = j; minZ = validData1[j].y;	}
							if (validData1[j].distance < minDistance) {
								minDisIndex = j; minDistance = validData1[j].distance;
							}	
						}
						
						for (int j = 0;j < nValidCount1 && minZ<500;j++)
						{
							int x = validData1[j].x;
							int y = validData1[j].y;
							int d = validData1[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 Êý¾Ý
				
							if (y > minZ + 50) {
								if ((firstCount >0  && secondCount ==0) 
									|| (secondCount >0 && firstCount ==0)) {
									midCount++;
								}
								continue;
							}
							planeCount++;
							if (x > -200 && x < 500) 
								{
								if (x < firstmin)  firstmin = x;
								if (x > secondmax) secondmax = x;
							}							
							
								// ºóÑØ
							if (x > -200 && x < 100)
							{
								if (validData1[j].x > firstmax)  firstmax =x;
								firstCount++;
							}
								// Ç°ÑØ
							if (x > 200 && x < 500 )
							{
								if (x < secondmin)  secondmin = x;
								secondCount++;
							}

						}
						int avg=0;
						if (firstCount > 0 && secondCount > 0 && midCount > 0 ){
							avg = (secondmin + firstmax) / 2;
						}else if (planeCount>10) {
							avg = (firstmin + secondmax) /2;
						}
					
						nPosY = (nPosY *3 + avg) /4;
						if (minZ < 9999) {
						nPosZ2 = (minZ + nPosZ2 *3)/4;
						} else  {
					//		results[4]= nValidCount1;
					//		results[5]= minDistance;
							
						}
//						nPosY = validData1[minDisIndex].x /1000;
//						nPosZ2 = validData1[minDisIndex].x /1000;
//						nPosZ2 = midcount;
				}
				startFlag1 = 0;
				nValidCount1 = 0;
			}
		}
	}	
	nPosZ = ((nPosZ1 + nPosZ2) + nPosZ *2)/4;
	
//	nPosX = pCount1;
//	nPosY = pCount2;
	return iRet;
}