/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : PDRBase.c
* Current Version    : V2.0
* Author             : logzhan
* Date of Issued     : 2022.10.15
* Comments           : PDR基础功能函数
********************************************************************************/
#include <math.h>
#include "PDRBase.h"
#include "Location.h"
#include "LinearFit.h"
#include "Utils.h"
#include "CoordTrans.h"
#include "Kalman.h"

extern double GpsPos[HIST_GPS_NUM][3];

int PredictStep(StepPredict* stepPredict, double timestamp, unsigned long steps_last, float gnssVel)
{
	int step = 0;
	float mean_time = 400;
	double dis = 0;

	if (stepPredict->meanTime > 0)
		mean_time = stepPredict->meanTime;

	if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0))
	{
		stepPredict->fsum = stepPredict->fsum / stepPredict->fnum;
		dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0] - GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2)
			+ pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2));

		if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0)))
		{
			step = (int)((timestamp - stepPredict->lastTime) / mean_time);
			stepPredict->deltaStep += step;
		}

		stepPredict->lastTime = timestamp;
		stepPredict->fnum = 0;
		stepPredict->fsum = 0;
	}

	return step;
}


double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss)
{
	int i;
	double lla[3] = { 0.0 };
	double ned1[3] = { 0.0 };
	double temp[2][HIST_GPS_NUM] = { { 0.0 } };
	double angle[HIST_GPS_NUM - 1] = { 0.0 };

	double ned2[3] = { 0.0 };
	double maxn = 0;
	double maxe = 0;
	double minn = 0;
	double mine = 0;
	double meanx = 0;
	double meany = 0;

	float error = 0;
	float sst = 0;
	float r2 = 0;

	double range = 0;
	double yaw = 0;
	double tempYaw = 0;
	double diffAngle = 0;

	double para[3] = { 0 };
	int flg = 0;

	lla[0] = pgnss.lat;
	lla[1] = pgnss.lon;

	for (i = 0; i < HIST_GPS_NUM - 1; i++)
	{
		gpsPos[i][0] = gpsPos[i + 1][0];
		gpsPos[i][1] = gpsPos[i + 1][1];
		gpsPos[i][2] = gpsPos[i + 1][2];
	}
	// 经纬度WGS84转地心地固坐标系ECEF
	if ((lla[0] > 0) && (lla[1] > 0)){
		WGS842ECEF(lla, gpsPos[HIST_GPS_NUM - 1]);
	}else{
		gpsPos[HIST_GPS_NUM - 1][0] = 0;
		gpsPos[HIST_GPS_NUM - 1][1] = 0;
		gpsPos[HIST_GPS_NUM - 1][2] = 0;
		return -1;
	}

	if (pgnss.error > 40){
		return -1;
	}

	for (i = 0; i < HIST_GPS_NUM - 1; i++)
	{
		if ((fabs(gpsPos[i][0]) < 1e-6) && (fabs(gpsPos[i][1]) < 1e-6) && (fabs(gpsPos[i][2]) < 1e-6))
		{
			return -1;
		}
	}
	//地心地固坐标系ECEF转东北天坐标系NED
	ECEF2NED(gpsPos[HIST_GPS_NUM - 1], lla, ned1);

	for (i = HIST_GPS_NUM - 2; i >= 0; i--) {

		ECEF2NED(gpsPos[i], lla, ned2);

		temp[0][i] = ned1[0] - ned2[0];
		temp[1][i] = ned1[1] - ned2[1];

		meanx += temp[0][i] / HIST_GPS_NUM;
		meany += temp[1][i] / HIST_GPS_NUM;

		if (maxn > temp[0][i])
		{
			maxn = temp[0][i];
		}
		if (minn < temp[0][i])
		{
			minn = temp[0][i];
		}
		if (maxe > temp[1][i])
		{
			maxe = temp[1][i];
		}
		if (mine < temp[1][i])
		{
			mine = temp[1][i];
		}

		angle[i] = atan2(temp[1][i], temp[0][i]);
		if (angle[i] < 0)
			angle[i] += TWO_PI;
	}
	flg = leastDistanceLinearFit(temp[0], temp[1], HIST_GPS_NUM, para);
	if (flg < 0)
		return -1;

	for (i = HIST_GPS_NUM - 1; i >= 0; i--) {
		error += (float)(pow(para[0] * temp[0][i] + para[1] * temp[1][i] + para[2], 2) / (pow(para[0], 2) + pow(para[1], 2)));
		sst += (float)(pow(meanx - temp[0][i], 2) + pow(meany - temp[1][i], 2));
	}

	if (sst > 0)
	{
		r2 = 1 - error / sst;
	}
	range = sqrt(pow(maxn - minn, 2) + pow(maxe - mine, 2));
	//printf("%f,%f,%f\n", r2, error, range);
	if (r2 < 0.95 || range < 1.5 || error > 3.0)
		return -1;

	yaw = atan(-para[0] / para[1]);
	modAngle(&yaw, 0, TWO_PI);

	tempYaw = meanAngle(angle, HIST_GPS_NUM - 1);
	modAngle(&tempYaw, 0, TWO_PI);

	diffAngle = tempYaw - yaw;
	modAngle(&diffAngle, -PI, PI);

	if (fabs(diffAngle) > PI / 2)
		yaw += PI;
	modAngle(&yaw, 0, TWO_PI);

	return yaw;
}

/**----------------------------------------------------------------------
* Function    : StateRecognition
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳  0: 平稳
* Date        : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void StateRecognition(float* acc, Classifer_t* sys) {

}



void calStepLastTime(StepPredict* stepPredict, double timestamp, unsigned long steps_last,
	unsigned long steps)
{
	float tmpTime = 0;
	if (stepPredict->lastTime > 0 && stepPredict->deltaStep == 0 && (steps - steps_last) > 0)
	{
		tmpTime = (float)((timestamp - stepPredict->lastTime) / (steps - steps_last));
		if (tmpTime > 300 && tmpTime < 800)
		{
			if (stepPredict->meanTime > 0) {
				stepPredict->meanTime = stepPredict->meanTime * 0.5f + tmpTime * 0.5f;
			}
			else {
				stepPredict->meanTime = tmpTime;
			}
		}
	}

	stepPredict->fnum = 0;
	stepPredict->fsum = 0;

	stepPredict->lastTime = timestamp;
	stepPredict->deltaStep = 0;

}

/**----------------------------------------------------------------------
* Function    : InsLocationPredict
* Description : PDR惯导位置更新
* Date        : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候
*               会造成PDR前后位置不够均匀
*---------------------------------------------------------------------**/
void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu,
	GNSS_t* pgnss, EKFPara_t* kf) {
	int step;
	long deltaStep;
	double stepLength = 0.7;

	stepPredict->fnum++;
	stepPredict->fsum += pdr->motionFreq;

	ulong  steps = pdr->steps;
	ulong  lastSteps = pdr->lastSteps;

	if (steps - lastSteps > 0 && pdr->motionFreq != 0.0) {
		deltaStep = steps - lastSteps - stepPredict->deltaStep;

		// 应该是防止计步器异常情况，一次计步特别多
		if (deltaStep > 5)deltaStep = 5;

		if (stepPredict->meanTime > 0)
		{
			step = (int)((imu->gyr.time - stepPredict->lastTime) / stepPredict->meanTime + 0.5);

			if (step > deltaStep && step <= 3)deltaStep = step;
		}
		if (lastSteps == 0)deltaStep = 0;

		if (pdr->heading > 0) {
			EKFStatePredict(kf, stepLength, pdr, deltaStep);
			pdr->fusionPdrFlg = PDR_TRUE;
		}
		calStepLastTime(stepPredict, imu->gyr.time, lastSteps, steps);
	}
	else {
		// 有时候，计步器可能考虑到稳定性暂时不更新步数，因此需要利用GNSS速度预测步数，达到
		// 惯导更新位置的目的
		int PreStep = PredictStep(stepPredict, imu->gyr.time, lastSteps, pgnss->vel);
		if (pdr->heading > 0) {
			EKFStatePredict(kf, stepLength, pdr, PreStep);
			if (PreStep > 0)pdr->fusionPdrFlg = PDR_TRUE;
		}
	}

	// 如果计步器可以保持较高的实时性，就可以采用下面的方法直接计算
	//if (pdr->heading > 0) {
	//	deltaStep = steps - lastSteps;
	//	EKFStatePredict(kf, stepLength, pdr, deltaStep);
	//	pdr->fusionPdrFlg = PDR_TRUE;
	//}
}