/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : pdr_util.c
* Department         : Sensor Algorithm Team
* Current Version    : V1.2
* Author             : logzhan
* Date of Issued     : 2020.7.4
* Comments           : PDR 工具类函数支持
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "PDRBase.h"
#include "Utils.h"
#include "CoordTrans.h"

const float FLT_THRES = 0.000001f;                    // 浮点数最小阈值

/* Function Declaration ------------------------------------------------------*/
/**----------------------------------------------------------------------
* Function    : mean
* Description : 对输入double数组求均值
* Review      : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
 *              不同函数
* Date        : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
double mean(double *data, int len) {
	int i;
	double value = 0;
	for (i = 0; i < len; i++) {
		value += data[i] / len;
	}
	return value;
}
/**----------------------------------------------------------------------
* Function    : fmean
* Description : 对输入float数组求均值
* Review      : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
 *              不同函数
* Date        : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
float fmean(float *data, int len)
{
	float value = 0;
	for (int i = 0; i < len; i++){
		value += data[i] / len;
	}
	return value;
}

double meanAngle(double* angle, int len)
{
	double baseAngle = angle[0];
	double diff = 0.0;
	double sum = 0.0;
	double value = 0;

	if (len <= 0)return 0;

	for (int i = 0; i < len; i++)
	{
		diff = angle[i] - baseAngle;
		if (diff > PI)
		{
			diff = diff - TWO_PI;
		}
		else if (diff < -PI)
		{
			diff = diff + TWO_PI;
		}

		sum += (baseAngle + diff);
	}

	value = sum / len;

	modAngle(&value, 0, TWO_PI);
	return value;
}

void modAngle(double * angle, double min, double max)
{
	double value = *angle;
	double range = max - min;
	while (value > max)value = value - range;
	while (value < min)value = value + range;
	*angle = value;
}

float stdf(float* data, int len)
{
	int i;
	float meanValue = fmean(data, len);
	float value = 0;
	for (i = 0; i < len; i++)
	{
		value += (data[i] - meanValue) * (data[i] - meanValue) / len;
	}
	return (float)sqrt(value);
}

/**----------------------------------------------------------------------
* Function    : pdr_invSqrt
* Description : 计算1/sqrt(x)的快速的算法
* Date        : 2020/6/30 logzhan
*---------------------------------------------------------------------**/
float InvSqrt(float x) {
	float xHalf = 0.5f * x;
	int i = *(int *)&x;
	i = 0x5f3759df - (i >> 1);
	x = *(float *)&i;
	x = x * (1.5f - xHalf * x * x);
	return x;
}

/**---------------------------------------------------------------------
* Function    : Vec3Norm
* Description : 三维浮点数向量归一化
* Date        : 2022/11/1 logzhan
*---------------------------------------------------------------------**/
void Vec3Norm(float* vx, float* vy, float* vz)
{
	float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
		(*vz) * (*vz)));
	// 防止出现模为0的情况
	if (norm > FLT_THRES) {
		norm = 1 / norm;
		(*vx) *= norm;   (*vy) *= norm;   (*vz) *= norm;
	}
}



/**----------------------------------------------------------------------
* Function    : Motion2TypeStr
* Description : 把用户运动类型转换为字符串输出
* Date        : 2022/9/16 logzhan
*---------------------------------------------------------------------**/
const char* Motion2TypeStr(int type)
{
	if (type == PDR_MOTION_TYPE_STATIC) {
		return "STATIC";
	}
	else if (type == PDR_MOTION_TYPE_IRREGULAR) {
		return "IRREGULAR";
	}
	else if (type == PDR_MOTION_TYPE_HANDHELD) {
		return "HANDLED";
	}
	else if (type == PDR_MOTION_TYPE_SWINGING) {
		return "SWINGING";
	}
	return "UNKOWN";
}

/**----------------------------------------------------------------------
* Function    : qnb2att
* Description : 四元数转欧拉角
* Date        : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
void Qnb2Att(float* q, double* attitude)
{
	double q11 = q[0] * q[0];   double q13 = q[0] * q[2];
	double q12 = q[0] * q[1];	double q14 = q[0] * q[3];
	double q22 = q[1] * q[1];   double q24 = q[1] * q[3];
	double q23 = q[1] * q[2];	double q33 = q[2] * q[2];
	double q34 = q[2] * q[3];   double q44 = q[3] * q[3];

	if (fabs(q34 + q12) <= 0.5) {
		attitude[0] = asin(2 * (q34 + q12));
	}
	attitude[1] = atan2(-2 * (q24 - q13), q11 - q22 - q33 + q44);
	attitude[2] = atan2(-2 * (q23 - q14), q11 - q22 + q33 - q44);
}



void GetCovMatrix(double *x, double *y, double cov[2][2], int n) {
	int i;
	double sum_x = 0, sum_y = 0, sum_xy = 0;
	double m_x = 0, m_y = 0;
	m_x = mean(x, n);
	m_y = mean(y, n);
	for (i = 0; i < n; i++) {
		sum_xy += (x[i] - m_x)*(y[i] - m_y);
		sum_x  += (x[i] - m_x)*(x[i] - m_x);
		sum_y  += (y[i] - m_y)*(y[i] - m_y);
	}
	cov[0][0] = sum_x / n;
	cov[0][1] = sum_xy / n;
	cov[1][0] = sum_xy / n;
	cov[1][1] = sum_y / n;
}

int Jacobi(double a[][2], double p[][2], int n, double eps, int T)
{
	int i, j, k;
	double max = a[0][1];
	int row = 0;
	int col = 0;
	int ite_num = 0;
	double theta = 0;
	double aii = 0;
	double ajj = 0;
	double aij = 0;
	double sin_theta = 0;
	double cos_theta = 0;
	double sin_2theta = 0;
	double cos_2theta = 0;
	double arowk = 0;
	double acolk = 0;
	double pki = 0;
	double pkj = 0;

	for (i = 0; i < n; i++)
		p[i][i] = 1;
	while (1)
	{
		max = fabs(a[0][1]);
		row = 0;
		col = 1;
		for (i = 0; i < n; i++)
		for (j = 0; j < n; j++)
		if (i != j && fabs(a[i][j])>max)
		{
			max = fabs(a[i][j]);
			row = i;
			col = j;
		}
		if (max < eps)
		{
			//printf("max : %lf \n", max);
			//printf("t : %lf \n", ite_num);

			return 1;
		}

		if (ite_num>T)
		{
			//printf("max : %lf \n", max);
			//printf("t : %lf \n", ite_num);
			return 0;
		}

		if (a[row][row] == a[col][col])
			theta = PI / 4;
		else
			theta = 0.5*atan(2 * a[row][col] / (a[row][row] - a[col][col]));
		aii = a[row][row];
		ajj = a[col][col];
		aij = a[row][col];
		sin_theta = sin(theta);
		cos_theta = cos(theta);
		sin_2theta = sin(2 * theta);
		cos_2theta = cos(2 * theta);
		a[row][row] = aii * cos_theta*cos_theta + ajj * sin_theta*sin_theta + aij * sin_2theta;
		a[col][col] = aii * sin_theta*sin_theta + ajj * cos_theta*cos_theta - aij * sin_2theta;
		a[row][col] = 0.5*(ajj - aii)*sin_2theta + aij * cos_2theta;
		a[col][row] = a[row][col];
		for (k = 0; k < n; k++)
		{
			if (k != row && k != col)
			{
				arowk = a[row][k];
				acolk = a[col][k];
				a[row][k] = arowk * cos_theta + acolk * sin_theta;
				a[k][row] = a[row][k];
				a[col][k] = acolk * cos_theta - arowk * sin_theta;
				a[k][col] = a[col][k];
			}
		}
		if (ite_num == 0)
		{
			p[row][row] = cos_theta;
			p[row][col] = -sin_theta;
			p[col][row] = sin_theta;
			p[col][col] = cos_theta;
		}
		else
		{
			for (k = 0; k < n; k++)
			{
				pki = p[k][row];
				pkj = p[k][col];
				p[k][row] = pki * cos_theta + pkj * sin_theta;
				p[k][col] = pkj * cos_theta - pki * sin_theta;
			}
		}
		ite_num++;
	}
}

static int isRealLat(double lat){ return (lat < 90) && (lat > -90); }
static int isRealLon(double lon){ return (lon < 180) && (lon > -180); }

/*
function: 计算地图上两个由经纬度表示点间的直线距离。
param @ pointA : 点A的经纬度
param @ pointB : 点B的经纬度
return @ dis : A、B点间的距离；若小于0则说明A或B点的经纬度输入有误。
*/
double CalDistance(LatLng_t pointA, LatLng_t pointB)
{
    double dis = -1;
    double lla1[3] = { 0 };
    double lla2[3] = { 0 };
    double ned1[3] = { 0 };
    double ned2[3] = { 0 };
    double diff_ned[3] = { 0 };

    lla1[0] = pointA.lat * RADIAN_PER_DEG;
    lla1[1] = pointA.lon * RADIAN_PER_DEG;
    lla2[0] = pointB.lat * RADIAN_PER_DEG;
    lla2[1] = pointB.lon * RADIAN_PER_DEG;

    if (isRealLat(pointA.lat) && isRealLon(pointA.lon)
        && isRealLat(pointB.lat) && isRealLon(pointB.lon))
    {
        WGS842NED(lla1, lla1, ned1);
        WGS842NED(lla2, lla1, ned2);
        diff_ned[0] = ned1[0] - ned2[0];
        diff_ned[1] = ned1[1] - ned2[1];
        diff_ned[2] = ned1[2] - ned2[2];

        dis = sqrt(pow(diff_ned[0], 2) + pow(diff_ned[1], 2) + pow(diff_ned[2], 2));
    }
    return dis;
}



LatLng_t ProjPointOfLatLng(LatLng_t point, LatLng_t linePointA, LatLng_t linePointB)
{

    /* local variables declaration */
    double gradxB_P;			/* linePointB 向 point 的 x 梯度         */
    double gradxB_A;			/* linePointA 向 linePointB 的 x 梯度    */
    double gradyB_A;			/* linePointA 向 linePointB 的 y 梯度    */
    double normPow;				/* linePointA 向 linePointB 梯度的平方和 */
    double scalarMultip;		/* gradxB_A * gradyB_A 的标量乘法        */
    LatLng_t resProjPoint;	/* 结果点坐标 */

    /* block process */
    gradxB_A = linePointA.lat - linePointB.lat;
    gradyB_A = linePointA.lon - linePointB.lon;
	if ((fabs(gradxB_A) < DBL_EPSILON) && (fabs(gradyB_A) < DBL_EPSILON))
    {
        /* 当linePointA 和linePointB是同一点时，返回的point坐标 */
        resProjPoint.lat = point.lat;
        resProjPoint.lon = point.lon;
    }
    else
    {
        gradxB_P = point.lat - linePointB.lat;
        normPow = pow(gradxB_A, 2) + pow(gradyB_A, 2);
        resProjPoint.lon = (double)(
                (gradxB_P * gradxB_A * gradyB_A + point.lon * pow(gradyB_A, 2) + linePointB.lon * pow(gradxB_A, 2)) / normPow);
        if ((fabs(gradxB_A) < DBL_EPSILON) || (fabs(gradyB_A) < DBL_EPSILON))
        {
            /* 当linePointA 和 linePointB 所在直线与 x轴或y轴平行时做规避*/
            scalarMultip = (fabs(gradxB_A) < DBL_EPSILON) ? gradyB_A : gradxB_A;
        }
        else
        {
            scalarMultip = gradxB_A * gradyB_A;
        }
        resProjPoint.lat = (double)(
                (gradxB_A * linePointB.lat * gradyB_A + gradxB_A * gradxB_A * (resProjPoint.lon - linePointB.lon)) / scalarMultip);
        if (fabs(gradxB_A) < DBL_EPSILON)
        {
            resProjPoint.lat = linePointA.lat;
        }
        if (fabs(gradyB_A) < DBL_EPSILON)
        {
            resProjPoint.lon = point.lon;
        }
    }

    return resProjPoint;
}

/**----------------------------------------------------------------------
* Function    : CalEarthParameter
* Description : 根据输入纬度返回该纬度地球相关参数
* Date        : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
EarthData_t CalEarthParameter(double lat)
{
	double temp_value = 0;
	EarthData_t earthData = { 0 };

	if (fabs(lat) >= PI / 2)lat = 0.0f;

	temp_value = sqrt(1 - WGS84_ECCENTR2 * (sin(lat)*sin(lat)));

	earthData.lat = lat;
	earthData.rmh = WGS84_RE * (1 - WGS84_ECCENTR2) / (temp_value*temp_value*temp_value);
	earthData.rnh = WGS84_RE * cos(lat) / temp_value;
	return earthData;
}

/**----------------------------------------------------------------------
* Function    : pdr_CalRadianDifferent
* Description : 计算弧度之差
* Date        : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
double CalRadianDifferent(double s_dir, double d_dir)
{
	double dirDiff = d_dir - s_dir;
	if (fabs(dirDiff) < PI)return dirDiff;
	return dirDiff > 0 ? dirDiff - 2 * PI : dirDiff + 2 * PI;
}

void ProjPointOfLatLng_cir(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;
	}

	EarthData_t earthData = CalEarthParameter(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    : pdr_WriteCsvStr
* Description : 将字符串信息写入文本，主要是对fprintf封装一层
* Date        : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
void WriteCsvStr(FILE* fp_write, char* str)
{
	if (NULL == fp_write)return;
	fprintf(fp_write, "%s\n", str);
}

int pdr_min(int a, int b){
	return (a > b ? b : a);
}

/**----------------------------------------------------------------------
* Function    : pdr_utc2hms
* Description : 将UTC时间转换为时分秒
* Date        : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
void pdr_utc2hms(double utc, double* h, double* m, double* s) {

	long lutc = (long)(utc / 1000);

	lutc = lutc % (3600 * 24);

	*h = floor((double)(lutc / (3600)));
	*m = floor((lutc - (*h) * 3600) / 60);
	*s = lutc - (*h) * 3600 - (*m) * 60;

}
