#include "JY901.h"

CJY901 ::CJY901 ()
{
	// minimum value filter, ignore small values near 0
	accInitLimit = 0.0001; // +- 0.005 m/s^2
	gyroInitLimit = 0.0001; // +- 0.002 rad/s
	angleInitLimit = 0.0001;
	// max difference in delta t
	accDiffLimit = 5.0; // 5.0 m/s^2 
	gyroDiffLimit = 2.7; // 0.3 rad/s ~~ 20 deg/s
	angleDiffLimit = 0.7; // 0.035 deg ~~ 2 deg
}

bool CJY901::CopeSerialData(char ucData[], int& start, int& end, unsigned short usLength)
{
	static unsigned char chrTemp[1000];
	static unsigned char ucRxCnt = 0;	
	static unsigned short usRxLength = 0;

	// backup last value
	for (size_t i = 0; i < 3; i++)
	{
		lastAcc.a[i] = stcAcc.a[i];
		lastGyro.w[i] = stcGyro.w[i];
		lastAngle.Angle[i] = stcAngle.Angle[i];
	}

	if(end>start && (start+usLength)>end){
		usLength=end-start+1;
		end += 1;
		end %= 1000;
		start = end;
	}else if(end<start){
		if((start+usLength)<=1000){
			//remain unchanged
		}else if((start+usLength)%1000>end){
			usLength = end + 1000 - start;
			end +=1;
			end %=1000;
			start = end;
		}
	}else if(start==end){
		return false;
	}
	// std::cout<<"start, end, len: "<<start<<" "<<end<<" "<<usLength<<std::endl;
    memcpy(chrTemp,ucData+start,usLength);
	usRxLength += usLength;
    while (usRxLength >= 11)
    {
        if (chrTemp[0] != 0x55)
        {
			usRxLength--;
			memcpy(&chrTemp[0],&chrTemp[1],usRxLength);                        
            continue;
        }
		switch(chrTemp[1])
		{
			case 0x50:	memcpy(&stcTime,&chrTemp[2],8);break;
			case 0x51:	memcpy(&stcAcc,&chrTemp[2],8);break;
			case 0x52:	memcpy(&stcGyro,&chrTemp[2],8);break;
			case 0x53:	memcpy(&stcAngle,&chrTemp[2],8);break;
			case 0x54:	memcpy(&stcMag,&chrTemp[2],8);break;
			case 0x55:	memcpy(&stcDStatus,&chrTemp[2],8);break;
			case 0x56:	memcpy(&stcPress,&chrTemp[2],8);break;
			case 0x57:	memcpy(&stcLonLat,&chrTemp[2],8);break;
			case 0x58:	memcpy(&stcGPSV,&chrTemp[2],8);break;
		}
		usRxLength -= 11;
		memcpy(&chrTemp[0],&chrTemp[11],usRxLength);                     
    }
	return checkData();
}

bool CJY901::checkData(){
	for (size_t i = 0; i < 3; i++)
	{
		// 值过小忽略
		if (fabs((float)stcAcc.a[i] / 32768 * 16 * 9.8) < accInitLimit && (i == 0 || i == 1))
		{
			// std::cout << "acc x or y too small" << std::endl;
			return false;
		}
		if (fabs((float)stcAcc.a[i] / 32768 * 16 - 1) * 9.8 < accInitLimit && i == 2)
		{
			// std::cout << "acc z too small" << std::endl;
			return false;
		}
		if (fabs((float)stcGyro.w[i] / 32768 * 2000 * M_PI / 180.0) < gyroInitLimit)
		{
			// std::cout << "omega too small" << std::endl;
			return false;
		}
		if (fabs((float)stcAngle.Angle[i] / 32768 * M_PI) < angleInitLimit)
		{
			// std::cout << "angle too small" << std::endl;
			return false;
		}

		// 值差异过大，舍弃
		if (fabs((float)(stcAcc.a[i] - lastAcc.a[i]) / 32768 * 16 * 9.8) > accDiffLimit)
		{
			// std::cout << "diff acc too big" << std::endl;
			return false;
		}
		if (fabs((float)(stcGyro.w[i] - lastGyro.w[i]) / 32768 * 2000 * M_PI / 180.0) > gyroDiffLimit)
		{
			// std::cout << "diff omega too big" << std::endl;
			return false;
		}
		if (fabs((float)(stcAngle.Angle[i] - lastAngle.Angle[i]) / 32768 * M_PI) > angleDiffLimit)
		{
			// std::cout << "diff angle too big" << std::endl;
			return false;
		}
	}
	return true;
}
// CJY901 JY901 = CJY901();