/******************** (C) COPYRIGHT 2020 VIVO************************************
* File Name          : pdr_linearFit.c
* Department         : Sensor Algorithm Team
* Current Version    : V2.0(compare QCOM SAP 5.0)
* Author             : yuanlin_rjyb@vivo.com & zhanli@vivo.com
* Date of Issued     : 2020.7.4
* Comments           : PDR 线性拟合相关函数支持, 主要是用于轨迹平滑
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <math.h>
#include <stdlib.h>
#include "pdr_linearFit.h"
#include "pdr_util.h"

/**----------------------------------------------------------------------
* Function    : pdr_linearLeastSquaresFitting
* Description : 线性最小二乘拟合 Y = c0 + c1 x 
                目标函数为sum| nDataY - (a*nDataX+b)|^2最小
* Date        : 2020/7/4 zhanli@vivo.com
*---------------------------------------------------------------------**/
int linearLeastSquaresFit(const double *nDataX, const double *nDataY, const int nLength, double *a, double *b)
{
	int fittingRes = 0;
	if (nDataX == NULL || nDataY == NULL || a == NULL || b == NULL){
		fittingRes = -1;
	} else if (nLength <= 0){
		fittingRes = 1;
	} else {
		fittingRes = gslFitLinear(nDataX, 1, nDataY, 1, nLength, b, a, NULL, NULL, NULL, NULL);
	}
	return fittingRes;
}

/**----------------------------------------------------------------------
* Function    : gsl_fit_linear
* Description : Fit the data (x_i, y_i) to the linear relationship
				Y = c0 + c1 x
				returning,
				c0, c1  --  coefficients
				cov00, cov01, cov11  --  variance-covariance matrix of
				c0 and c1,
				sumsq   --   sum of squares of residuals
				This fit can be used in the case where the errors for
				the data are uknown, but assumed equal for all points.
				The resulting variance-covariance matrix estimates the
				error in the coefficientsfrom the observed variance of
				the points around the best fit line.
* Date        :
*---------------------------------------------------------------------**/
static int gslFitLinear(const double *x, const size_t xstride,
	const double *y, const size_t ystride,
	const size_t n,
	double *c0, double *c1,
	double *cov_00, double *cov_01, double *cov_11, double *sumsq)
{
	double m_x = 0, m_y = 0, m_dx2 = 0, m_dxdy = 0;
	double dx;
	double dy;
	size_t i;
	double s2 = 0, d2 = 0;
	double b = 0;
	double a = 0;
	double d = 0;
	for (i = 0; i < n; i++)
	{
		m_x += (x[i * xstride] - m_x) / (i + 1.0);
		m_y += (y[i * ystride] - m_y) / (i + 1.0);
	}

	for (i = 0; i < n; i++)
	{
		dx = x[i * xstride] - m_x;
		dy = y[i * ystride] - m_y;

		m_dx2 += (dx * dx - m_dx2) / (i + 1.0);
		m_dxdy += (dx * dy - m_dxdy) / (i + 1.0);
	}

	/* In terms of y = a + b x */
	if (fabs(m_dx2) < 1e-20)return -1;

	b = m_dxdy / m_dx2;
	a = m_y - m_x * b;

	*c0 = a;
	*c1 = b;

	/* Compute chi^2 = \sum (y_i - (a + b * x_i))^2 */
	for (i = 0; i < n; i++)
	{
		dx = x[i * xstride] - m_x;
		dy = y[i * ystride] - m_y;
		d = dy - b * dx;
		d2 += d * d;
	}

	s2 = d2 / (n - 2.0);        /* chisq per degree of freedom */

	/* 为函数增加空指针检查以使函数可以将一下参数设置为空指针 */
	if (cov_00 != NULL)
		*cov_00 = s2 * (1.0 / n) * (1 + m_x * m_x / m_dx2);
	if (cov_11 != NULL)
		*cov_11 = s2 * 1.0 / (n * m_dx2);
	if (cov_01 != NULL)
	*cov_01 = s2 * (-m_x) / (n * m_dx2);

	if (sumsq != NULL)
		*sumsq = d2;
	
	return 0;
}


/**----------------------------------------------------------------------
* Function    : leastDistanceLinearFit
* Description : 二维坐标系点的线性拟合，ax+by+c=0, 目标函数为点到拟合直线的距离
				平方和最小， 这个函数的计算量还是相对较大的，主要注意这个函数不
				适合频繁调用
* Input       : x， y，二维坐标X和Y
				num, 二维点个数
* Output      : para， 拟合直线参数, para[0]:a, para[0]:b, para[0]:c
*---------------------------------------------------------------------**/
int leastDistanceLinearFit(double x[], double y[], int num, double para[])
{
	int i;
	double covMatrix[2][2] = { { 0.0 } };
	double p[2][2] = { { 0.0 } };

	if (num < 2)
		return -1;

	getCovMatrix(x, y, covMatrix, num);
	Jacobi(covMatrix, p, 2, 1e-10, 1000);

	if (covMatrix[0][0] > covMatrix[1][1]) {
		para[0] = p[0][1];
		para[1] = p[1][1];
	}
	else {
		para[0] = p[0][0];
		para[1] = p[1][0];
	}

	para[2] = 0;
	for (i = num - 1; i >= 0; i--) {
		para[2] -= (para[0] * x[i] + para[1] * y[i]) / num;
	}
	return 0;
}