#include "YDLidar.h"

int nPosX;
int nPosY;
int nPosZ;

int nPosZ1,nPosZ2;

int pCount1=0;
int pCount2=0;

int dCount1=0;
int dCount2=0;

int vCount1=0;
int vCount2=0;


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
};
const int CosTable[] =
{
	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 ,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
};


const unsigned char sintab[256]={	//ÕýÏÒ±í£¬0 - 1024´ú±í 0 - 2PI,½á¹û0 - 255±íʾ0 - 1£¬±íÖÐÖ»ÓÐ0 - PI/2¼´1/4ÖÜÆÚµÄÊý¾Ý£¬ÆäËûµÄÊý¾ÝÓöԳƺ;µÏñµÃµ½¡£
 0, 1, 3, 4, 6, 7, 9, 10, 12, 14, 15, 17, 18, 20, 21, 23, 25, 26, 28, 29, 31, 32, 34, 36, 37, 39, 40, 42, 43, 45, 46,
 48, 49, 51, 53, 54, 56, 57, 59, 60, 62, 63, 65, 66, 68, 69, 71, 72, 74, 75, 77, 78, 80, 81, 83, 84, 86, 87, 89, 90,
 92, 93, 95, 96, 97, 99, 100, 102, 103, 105, 106, 108, 109, 110, 112, 113, 115, 116, 117, 119, 120, 122, 123, 124,
 126, 127, 128, 130, 131, 132, 134, 135, 136, 138, 139, 140, 142, 143, 144, 146, 147, 148, 149, 151, 152, 153, 155,
 156, 157, 158, 159, 161, 162, 163, 164, 166, 167, 168, 169, 170, 171, 173, 174, 175, 176, 177, 178, 179, 181, 182,
 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204,
 205, 206, 207, 208, 209, 210, 211, 211, 212, 213, 214, 215, 216, 217, 217, 218, 219, 220, 221, 221, 222, 223,
 224, 225, 225, 226, 227, 227, 228, 229, 230, 230, 231, 232, 232, 233, 234, 234, 235, 235, 236, 237, 237, 238,
 238, 239, 239, 240, 241, 241, 242, 242, 243, 243, 244, 244, 244, 245, 245, 246, 246, 247, 247, 247, 248, 248, 
 249, 249, 249, 250, 250, 250, 251, 251, 251, 251, 252, 252, 252, 252, 253, 253, 253, 253, 254, 254, 254, 254, 
 254, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,// 255 
};

	


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


int StartAngle;
int EndAngle;

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


int YdLidarStop(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 200
stLidarDot validData0[200];
stLidarDot validData1[200];

int nValidCount0 = 0;
int nValidCount1 = 0;

#define SUB_DEGREE 64

#define START_DEGREE 135
#define END_DEGREE 225

#define MAX_DISTANCE 500				//mm

//ÕûÊýÇóSINÖµ£¬´øÏßÐÔ²åÖµ¹¦ÄÜ£¬ ÊäÈë0 - 360*64 Ϊ 0 - 2PI,Ò»¸öÖÜÆÚ;Êä³ö-1000 ÖÁ +1000,´ú±í-1µ½+1;
int sini(int a)
{
	
	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 YdLidarParsePkt(int nLidarIdx, stLidarDotsPkt * pLindarPkt, int len1)
{
	int iRet = 0;
	if (pLindarPkt->StSign != 0x55AA) return 0;		// check for start sign
	if ((pLindarPkt->CT & 0x01) != 0x00) return 0;		// check for CT bit0. 
	if (pLindarPkt->LSN<2) return 0;
	
	int startAngle = (pLindarPkt->FSA >> 1);	// degree * 64;
	int endAngle = (pLindarPkt->LSA >> 1); 		// degree * 64;
	if (startAngle >= endAngle)return 0;
	int nCount = pLindarPkt->LSN;	
	int diffAngle = (endAngle - startAngle);
	int eachAngle = diffAngle / (nCount - 1);	
	
	if (nLidarIdx == 0) 
	{
		pCount1++;
		for (int i = 0;i < nCount && i < 40 ;i++)
		{
			float intensity = pLindarPkt->LidarDotDatas[i].intensity;
			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)
			{
				int value = (pLindarPkt->LidarDotDatas[i].s2 << 6) + (pLindarPkt->LidarDotDatas[i].s1 >> 2); // distance
				
				int x = value * sini(angle - 90 * SUB_DEGREE );			// 	uM
				int y = value * cosi(angle - 90 * SUB_DEGREE );			//  uM
				
				//vectorX.push_back(x);
				//vectorY.push_back(y);
				validData0[nValidCount0].distance = value;
				validData0[nValidCount0].x = x;
				validData0[nValidCount0].y = y;
				if (nValidCount0 < 200 - 1 ) nValidCount0 ++;
				vCount1++;
			}

			if (angle > END_DEGREE * SUB_DEGREE)
			{
				if (startFlag0 == 1 && nValidCount0 > 0)
				{

						int min = 1000000, max = -1000000;
						int minX = 1000000;
						for (int i = 0;i < nValidCount0;i++)
						{
							if (validData0[i].distance < 5000) continue;
							
							if (validData0[i].y > 0)
							{
								if (validData0[i].y < min)
									min = validData0[i].y;
							}
							if (validData0[i].y < 0)
							{
								if (validData0[i].y > max)
									max = validData0[i].y;
							}
						}
						float avg=0;
						for (int i = 0;i < nValidCount0;i++)
						{
							if (validData0[i].x < minX) 
							{
								minX = validData0[i].x;
							}
						}
						if (min != 0 && max != 0)
						{
							avg = (min + max) / 2000;
						}
						
						nPosX = avg;
						nPosZ1 = minX/1000;
				}
				startFlag0 = 0;
				nValidCount0 = 0;
			}
		}
	}

	if (nLidarIdx == 1) 
	{
		pCount2++;
		for (int i = 0;i < nCount && i < 40 ;i++)
		{
			float intensity = pLindarPkt->LidarDotDatas[i].intensity;
			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)
			{
				int value = (pLindarPkt->LidarDotDatas[i].s2 << 6) + (pLindarPkt->LidarDotDatas[i].s1 >> 2);

				int x = value * sini(angle - 90 * SUB_DEGREE );			//	uM
				int y = value * cosi(angle - 90 * SUB_DEGREE );			//	uM //vectorX.push_back(x);
				//vectorY.push_back(y);
				validData1[nValidCount1].distance = value; 
				validData1[nValidCount1].x = x;
				validData1[nValidCount1].y = y;
				nValidCount1 ++;
				vCount2 ++;
			}

			if (angle > END_DEGREE * SUB_DEGREE)
			{
				if (startFlag1 == 1 && nValidCount1 > 0 )
				{
						int minDisIndex = 0;
						int minDistance = 99999999;
						
						int min = 1000000, max = -1000000;
						int minX = 1000000;
						for (int i = 0;i < nValidCount1;i++)
						{
							if (validData1[i].distance < 30) continue;		// skip too small points;
							
							if (validData1[i].distance < minDistance) {
								minDisIndex = i;
								minDistance = validData1[i].distance;
							}

							if (validData1[i].x < minX) 
							{
								minX = validData1[i].x;
							}
							
							if (validData1[i].y > 0)
							{
								if (validData1[i].y < min)
									min = validData1[i].y;
							}
							if (validData1[i].y < 0)
							{
								if (validData1[i].y > max)
									max = validData1[i].y;
							}
							
						}
						int avg=0;
						int leftcount = 0;
						int midcount = 0;
						int rightcount =0;
						for (int i = 0;i < nValidCount1;i++)
						{
							if (validData1[i].distance < 30) continue;		// skip too small points;
							// ÕÒÖмä¿Õ¶´;
							if (validData1[i].x > minX + 50000) {
											// points, 50mm higher that minX;
								if (leftcount>5 && rightcount <5) midcount++;
								
								continue;		//skip 
							}else {
									if (midcount ==0) {leftcount++;}
									else {rightcount++;}
							}
						}
						if (min != 0 && max != 0)
						{
							avg = (min + max) / 2000;
						}
						
						nPosY = avg;
						nPosZ1 = minX/1000;
						nPosY = validData1[minDisIndex].y /1000;
//						nPosZ2 = validData1[minDisIndex].x /1000;
						nPosZ2 = midcount;
				}
				startFlag1 = 0;
				nValidCount1 = 0;
			}
		}
	}	
	nPosZ = (nPosZ1 + nPosZ2) /2;
	
	return iRet;
}