/******************** (C) COPYRIGHT 2020 VIVO************************************
* File Name          : LapProcess.c
* Department         : Sensor Algorithm Team
* Current Version    : V1.0
* Author             : yuanlin_rjyb@vivo.com
* Date of Issued     : 2020.8.4
* Comments           : 跑圈模式优化
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include "LapProcess.h"

/* Global Variable Definition ------------------------------------------------*/
unsigned char circleFlag;
unsigned char adjcirFlag;
unsigned char lineFlag;
int circumference = 0;
int poorPoint = 0;
int matchIndex = -1;
double startTime = 0;
double distSum = 0;
double errorSum = 0;
double matchE = 0;
double firstLoc[2] = { 0 };
TrajSign circleTrajs = { 0 };
TrajSign pdr_refTrajs = { 0 };
extern int debugCount;

/* Function Declaration ------------------------------------------------------*/
/**---------------------------------------------------------------------
* Function    : lapInit
* Description : 跑圈优化功能初始化
* Date        : 2020/8/4 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void lapInit(){
	circleFlag = NO_CIRCLE;
	adjcirFlag = NADJ_CIRCLE;
	lineFlag = FALIED_LINE;
	circumference = 0;
	poorPoint = 0;
	startTime = 0;
	distSum = 0;
	errorSum = 0;
	matchIndex = -1;
	matchE = 0;
	memset(&firstLoc, 0, sizeof(firstLoc));
	memset(&circleTrajs, 0, sizeof(circleTrajs));
	memset(&pdr_refTrajs, 0, sizeof(pdr_refTrajs));
}

void lapProcess(gnss pgnss, lct_fs *result)
{
	double dist = -1;
	double refweight = 1.0;
	//double circleOrd;
	int loopFlag = 0;
	LatLng tmp;

	if (circleFlag == NO_CIRCLE)
	{
		pdr_cacheTrajs(pgnss, result);
	}

	if (circleFlag == FIND_CIRCLE)
	{
		loopFlag = pdr_findTun(&pdr_refTrajs, circleTrajs.points[0], circleTrajs.yaw[0]);
		if (loopFlag == 0)
		{
			memcpy(&pdr_refTrajs, &circleTrajs, sizeof(pdr_refTrajs));
			int num = pdr_adjustTraj(pdr_refTrajs.points, circumference);
			memset(&circleTrajs, 0, sizeof(circleTrajs));
			adjcirFlag = ADJ_CIRCLE;
			circleFlag = NO_CIRCLE;
			
			printf("LOOP_Ori\n");
		}
		else if (loopFlag == 1)
		{
			double newRefLoc[2 * TRAJ_POINT_COUNT][2] = { { 0 } };
			int count = pdr_fuseRefTraj(&circleTrajs, &pdr_refTrajs, newRefLoc);

			if (count < MIN_POINT_COUNT)
			{
				memset(&newRefLoc, 0, sizeof(newRefLoc));
				return;
			}

			int num = pdr_adjustTraj(newRefLoc, count);
			if (num < MIN_POINT_COUNT)
			{
				memset(&newRefLoc, 0, sizeof(newRefLoc));
				return;
			}

			pdr_refTrajs.weight += circleTrajs.weight;
			pdr_refTrajs.count = num;
			for (int i = 0; i < num; i++)
			{
				pdr_refTrajs.points[i][0] = newRefLoc[i][0];
				pdr_refTrajs.points[i][1] = newRefLoc[i][1];
			}
			memset(&circleTrajs, 0, sizeof(circleTrajs));
			adjcirFlag = ADJ_CIRCLE;
			circleFlag = NO_CIRCLE;
			printf("LOOP_Next\n");
		}
	}

	if (adjcirFlag == ADJ_CIRCLE)
	{
		tmp.lat = result->latitude;
		tmp.lon = result->longitudinal;
		dist = pdr_optiFromTrajs(pgnss, result);
		/*根据轨迹上对应点的error（matchE）确定轨迹上该点的可信度，matchE越大，可信度越低，refweight应越小*/
		refweight = refweight * (1 - matchE / 15);
		if (refweight < 0)
			refweight = 0;
		/*dist为点与轨迹上对应点的距离，越远ref权重应越小*/
		if (dist >= 0)
		{
			refweight = refweight * (1 - fabs(dist / DIST_THRESHOLD));
			result->latitude = refweight * result->latitude + (1 - refweight) * tmp.lat;
			result->longitudinal = refweight * result->longitudinal + (1 - refweight) * tmp.lon;
		}
	}
}

void pdr_cacheTrajs(gnss pgnss, lct_fs *result) {
	double dist = 0;
	double tmpPoint[2] = { 0 };
	if (circleTrajs.count < TRAJ_POINT_COUNT) {
		circleTrajs.lastTime = pgnss.timestamps;
		if (circleTrajs.count == 0)
		{
			startTime = circleTrajs.lastTime;
			circleTrajs.points[0][0] = result->latitude * RADIAN_PER_DEG;
			circleTrajs.points[0][1] = result->longitudinal * RADIAN_PER_DEG;
			if (pgnss.yaw < 180)
			{
				circleTrajs.yaw[0] = -pgnss.yaw * RADIAN_PER_DEG;
			}
			else
			{
				circleTrajs.yaw[0] = 2 * PI - pgnss.yaw * RADIAN_PER_DEG;
			}
			//circleTrajs.yaw[0] = pgnss.yaw;
			circleTrajs.error[0] = pgnss.error;
			if (pgnss.vel > 1.0)
			{
				if (pdr_refTrajs.points[0][0] == 0 && pdr_refTrajs.points[0][1] == 0)
				{
					firstLoc[0] = circleTrajs.points[0][0];
					firstLoc[1] = circleTrajs.points[0][1];
				}
				circleTrajs.count++;
				errorSum += pgnss.error;
				if (pgnss.error > 10.5)
					poorPoint++;
			}
		}
		else{
			circleTrajs.points[circleTrajs.count][0] = result->latitude * RADIAN_PER_DEG;
			circleTrajs.points[circleTrajs.count][1] = result->longitudinal * RADIAN_PER_DEG;
			if (pgnss.yaw < 180)
			{
				circleTrajs.yaw[circleTrajs.count] = -pgnss.yaw * RADIAN_PER_DEG;
			}
			else
			{
				circleTrajs.yaw[circleTrajs.count] = 2 * PI - pgnss.yaw * RADIAN_PER_DEG;
			}
			//circleTrajs.yaw[circleTrajs.count] = pgnss.yaw;
			circleTrajs.error[circleTrajs.count] = pgnss.error;
			circleTrajs.count++;
			errorSum += pgnss.error;
			if (pgnss.error > 10.5)
				poorPoint++;
		}
	}

	if (circleTrajs.count > 0){
		tmpPoint[0] = result->latitude * RADIAN_PER_DEG;
		tmpPoint[1] = result->longitudinal * RADIAN_PER_DEG;

		int distff = (int)lap_calDistance(firstLoc, circleTrajs.points[0]);

		dist = lap_calDistance(tmpPoint, circleTrajs.points[0]);
		distSum += dist;
		double tmmp = (double)(poorPoint) / (double)(circleTrajs.count + 1);
		if (distff < 8 && dist < 5.25 && circleTrajs.lastTime - startTime > 150000 && (distSum / (double)(circleTrajs.count + 1) > 20))
		{
			if (circleTrajs.count < 200 || errorSum / (double)(circleTrajs.count + 1) > 10.0 || (double)(poorPoint) / (double)(circleTrajs.count + 1)> 0.28)
			{
				poorPoint = 0;
				distSum = 0;
				errorSum = 0;
				memset(&circleTrajs, 0, sizeof(circleTrajs));
			}
			else
			{
				circleFlag = FIND_CIRCLE;
				errorSum = 0;
				distSum = 0;
				circleTrajs.weight = 1.0;
				circumference = circleTrajs.count;
			}
		}
	}
}

/**************************************************************************
* Description ：对轨迹上的点进行排序，并调整点数
**************************************************************************/
int pdr_adjustTraj(double trajs[][2], int count)
{
	int i;
	int num = count;

	/*num大于的数值就是最后轨迹输出的点数*/
	while (num > pdr_min(TRAJ_POINT_COUNT, (int)floor(count * 0.7)))
	{
		double minDist = lap_calDistance(trajs[0], trajs[1]);
		int idx = 0;
		double tmpDist = 0;
		for (i = 1; i < num - 1; i++)
		{
			tmpDist = lap_calDistance(trajs[i], trajs[i + 1]);
			if (tmpDist < minDist)
			{
				minDist = tmpDist;
				idx = i;
			}
		}
		trajs[idx][0] = (trajs[idx][0] + trajs[idx + 1][0]) / 2;
		trajs[idx][1] = (trajs[idx][1] + trajs[idx + 1][1]) / 2;
		for (i = idx + 1; i < num - 1; i++)
		{
			memcpy(&trajs[i], &trajs[i + 1], sizeof(double)* 2);
		}
		num--;
	}
	return num;
}

/**---------------------------------------------------------------------
* Function    : getPointYaw
* Description : 计算两个点连线的yaw角
* Date        : 2020/7/27 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
double pdr_getPointYaw(double point1[], double point2[])
{
	S_EarthData earthData = pdr_earthParameter(point1[0]);
	double temp;
	temp = point2[1] * earthData.rnh;
	double lonDiff = (point2[1] - point1[1]) * earthData.rnh;
	double latDiff = (point2[0] - point1[0]) * earthData.rmh;
	return -atan2(lonDiff, latDiff);
}

/**************************************************************************
* Description ：根据隧道轨迹对融合定位点进行投影后处理
**************************************************************************/
double pdr_optiFromTrajs(gnss pgnss, lct_fs *lct_fusion)
{
	double projPoint[2] = { 0 };
	double tmpPoint[2] = { 0 };
	double yaw = 0;
	tmpPoint[0] = lct_fusion->latitude * RADIAN_PER_DEG;
	tmpPoint[1] = lct_fusion->longitudinal * RADIAN_PER_DEG;
	if (pgnss.yaw < 180)
	{
		yaw = -pgnss.yaw * RADIAN_PER_DEG;
	}
	else
	{
		yaw = 2 * PI - pgnss.yaw * RADIAN_PER_DEG;
	}

	pdr_pathMap(&pdr_refTrajs, tmpPoint, yaw, projPoint);

	double dist = lap_calDistance(tmpPoint, projPoint);

	double distThe = DIST_THRESHOLD;
	if (distThe > 50)
		distThe = 50;
	if (dist > distThe)
	{
		return -1;
	}

	lct_fusion->latitude = projPoint[0] * DEG_PER_RADIAN;
	lct_fusion->longitudinal = projPoint[1] * DEG_PER_RADIAN;

	return dist;
}

/**************************************************************************
* Description ：点到轨迹的投影
**************************************************************************/
void pdr_pathMap(TrajSign* trajs, double point[2], double yaw, double projPoint[2])
{
	lineFlag = IN_LINE;
	double dist = 1000.f;
	//int matchIndex = -1;
	double distTemp = 0;
	double yawThes = 60 * RADIAN_PER_DEG;
	matchIndex = -1;
	for (int i = 0; i < trajs->count; i++) {
		distTemp = lap_calDistance(point, trajs->points[i]);
		if (distTemp < dist) {
			dist = distTemp;
			matchIndex = i;
		}
	}

	if (matchIndex < 0) {
		lineFlag = FALIED_LINE;
		matchE = 100;
		projPoint[0] = point[0];
		projPoint[1] = point[1];
		return;
	}

	// 计算最邻近匹配点和邻近点前后两个点，计算距离
	int secIndex = 0;
	double lineYaw = 0;
	if (matchIndex == 0)
	{
		secIndex = matchIndex + 1;
	}
	else if (matchIndex == trajs->count - 1)
	{
		secIndex = matchIndex;
		matchIndex = secIndex - 1;
	}
	else
	{
		double distTemp1 = lap_calDistance(point, trajs->points[matchIndex - 1]);
		double distTemp2 = lap_calDistance(point, trajs->points[matchIndex + 1]);

		double yaw1 = pdr_getPointYaw(trajs->points[matchIndex - 1], trajs->points[matchIndex]);
		double yaw2 = pdr_getPointYaw(trajs->points[matchIndex], trajs->points[matchIndex + 1]);
		secIndex = matchIndex + 1;
		if (fabs(pdr_radianDifferent(yaw, yaw1)) < yawThes && fabs(pdr_radianDifferent(yaw, yaw2)) < yawThes)
		{
			if (distTemp1 < distTemp2)
			{
				secIndex = matchIndex;
			}
		}
		else if (fabs(pdr_radianDifferent(yaw, yaw1)) < yawThes)
		{
			secIndex = matchIndex;
		}
		else if (fabs(pdr_radianDifferent(yaw, yaw2)) < yawThes)
		{
			secIndex = matchIndex + 1;
		}
		else
		{
			lineFlag = FALIED_LINE;
		}
		matchIndex = secIndex - 1;
	}

	lineYaw = pdr_getPointYaw(trajs->points[matchIndex], trajs->points[secIndex]);
	if (fabs(pdr_radianDifferent(yaw, lineYaw)) > yawThes)
	{
		lineFlag = FALIED_LINE;
	}

	double yawtmp = pdr_getPointYaw(trajs->points[matchIndex], trajs->points[secIndex]);
	ProjPointOfLatLng_cir(trajs->points[matchIndex], yawtmp, point, projPoint);

	if (trajs->yaw[matchIndex] != 0 && fabs(trajs->yaw[matchIndex] - yaw) > 1.5 * PI)
	{
		matchE = trajs->error[matchIndex] * 10;
	}

	matchE = trajs->error[matchIndex];
	return;
}

/**---------------------------------------------------------------------
* Function    : findTun
* Description ：判断与之前存储的轨迹是否匹配
* Date        : 2020/8/6 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
int pdr_findTun(TrajSign* refTrajs, double point[], double heading)
{
	int ind = 0;
	double dist = 0;
	double minDist = 1e6;

	for (int j = 0; j < refTrajs->count; j++)
	{
		dist = lap_calDistance(refTrajs->points[j], point);
		if (dist < minDist)
		{
			double yaw;
			if (j == 0)
				yaw = pdr_getPointYaw(refTrajs->points[j], refTrajs->points[j + 1]);
			else if (j == refTrajs->count - 1)
				yaw = pdr_getPointYaw(refTrajs->points[j - 1], refTrajs->points[j]);
			else
			{
				yaw = pdr_getPointYaw(refTrajs->points[j - 1], refTrajs->points[j]);
				yaw += pdr_radianDifferent(yaw, pdr_getPointYaw(refTrajs->points[j - 1], refTrajs->points[j])) / 2;
			}

			if (fabs(pdr_radianDifferent(yaw, heading)) < 60 * RADIAN_PER_DEG)
			{
				minDist = dist;
				ind = 1;
			}
		}
	}

	if (minDist < 100)
	{
		return ind;
	}
	else
	{
		return 0;
	}
}

/**************************************************************************
* Description ：两条轨迹融合为一条
**************************************************************************/
int pdr_fuseRefTraj(TrajSign* tunnelTrajs, TrajSign* refTrajs, double newRefLoc[][2])
{
	if (tunnelTrajs->weight <= 0 || refTrajs->weight <= 0)
		return -1;

	double refWeight = refTrajs->weight / (tunnelTrajs->weight + refTrajs->weight);

	int i, j;
	for (i = 0; i < refTrajs->count - 1; i++)
	{
		int idx = -1;
		double tmpDist = 0;
		double minDist = 100;
		for (j = 0; j < tunnelTrajs->count - 1; j++)
		{
			tmpDist = lap_calDistance(refTrajs->points[i], tunnelTrajs->points[j]);
			if (tmpDist < minDist)
			{
				minDist = tmpDist;
				idx = j;
			}
		}
		if (minDist < 10)
		{
			newRefLoc[i][0] = (refTrajs->points[i][0] * refWeight + tunnelTrajs->points[idx][0] * (1 - refWeight));
			newRefLoc[i][1] = (refTrajs->points[i][1] * refWeight + tunnelTrajs->points[idx][1] * (1 - refWeight));
		}
		else{
			if ((refTrajs->error[i] - tunnelTrajs->error[i]) > 2)
			{
				double weight = refWeight * 4 / (refTrajs->error[i] - tunnelTrajs->error[i]);
				newRefLoc[i][0] = (refTrajs->points[i][0] * weight + tunnelTrajs->points[idx][0] * (1 - weight));
				newRefLoc[i][1] = (refTrajs->points[i][1] * weight + tunnelTrajs->points[idx][1] * (1 - weight));

			}
			else{
				newRefLoc[i][0] = refTrajs->points[i][0];
				newRefLoc[i][1] = refTrajs->points[i][1];

			}
		}
	}
	return refTrajs->count;
}

/**---------------------------------------------------------------------
* Function    : point2line
* Description : 判断点的相对位置
* Date        : 2020/8/6 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
unsigned char  pdr_point2line(double point[], double lineStart[], double lineEnd[], double projLoc[])
{
	unsigned char flg = IN_LINE;
	double vect_a[2] = { 0 };
	double vect_b[2] = { 0 };
	double yaw = pdr_getPointYaw(lineStart, lineEnd);
	ProjPointOfLoop(lineStart, yaw, point, projLoc);

	vect_a[0] = projLoc[0] - lineStart[0];
	vect_a[1] = projLoc[1] - lineStart[1];
	vect_b[0] = projLoc[0] - lineEnd[0];
	vect_b[1] = projLoc[1] - lineEnd[1];

	if ((vect_a[0] * vect_b[0] + vect_a[1] * vect_b[1]) > 0)
	{
		double distTemp1 = lap_calDistance(lineStart, projLoc);
		double distTemp2 = lap_calDistance(lineEnd, projLoc);

		if (distTemp1 < distTemp2) {
			flg = CLOSE_START;
		}
		else {
			flg = CLOSE_END;
		}
	}
	return flg;
}

/**---------------------------------------------------------------------
* Function    : ProjPointOfLoop
* Description : 将点投影到两个相邻点所成的直线上
* Date        : 2020/8/6 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void ProjPointOfLoop(double point1[], double yaw, double point2[], double result[])
{
	if (fabs(fabs(yaw) - PI / 2) < DOUBLE_ZERO)
	{
		result[0] = point1[0];
		result[1] = point2[1];
		return;
	}

	S_EarthData earthData = pdr_earthParameter(point1[0]);
	double k = -tan(yaw);
	double lonDiff = (point2[1] - point1[1]) * earthData.rnh;
	double latDiff = (point2[0] - point1[0]) * earthData.rmh;

	double x = (k*latDiff + k * k * lonDiff) / (1 + k * k);
	double y = (latDiff + k * lonDiff) / (1 + k * k);

	result[1] = point1[1] + x / earthData.rnh;
	result[0] = point1[0] + y / earthData.rmh;
}

/*
function: 跑圈功能使用的距离计算。
*/
double lap_calDistance(double lla1[2], double lla2[2])
{
	S_EarthData earthData = pdr_earthParameter(lla1[0]);
	double latDiff = earthData.rmh*(lla1[0] - lla2[0]);
	double lonDiff = earthData.rnh*(lla1[1] - lla2[1]);
	double distTemp = sqrt(latDiff*latDiff + lonDiff * lonDiff);
	return distTemp;
}