/******************** (C) COPYRIGHT 2020 VIVO************************************
* File Name          : simulator_location_pdr.c
* Department         : Sensor Algorithm Team
* Current Version    : V1.2
* Author             : ZhangJingrui@vivo.com
* Date of Issued     : 2020.7.4
* Comments           : PDR 输出轨迹平滑的功能函数
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <stddef.h>
#include <string.h>
#include "pdr_trackSmooth.h"
#include "pdr_linearFit.h"
#include "pdr_util.h"

/* Global Variable Definition ------------------------------------------------*/

/* 轨迹平滑历史输入点存储缓冲区 */
static LatLng *SmoothBuffer = NULL; //最新的点在[0]索引处
static size_t SmoothBufferSize = 0;
static size_t elemTrail = 0;
/* 输出点缓存                */
// #define OUTPUT_BUFFER_SIZE 5
static LatLng *OutputBuffer = NULL;
static size_t outputTrail = 0;
static size_t outputBufferSize = 0;

/* 拐点存储                  */
static LatLng InflectionPoint = {0,0};
/* 直线拟合缓存区 */

static LatLng fittingDock[FITTING_DOCK_SIZE] = {{0,0}};
static size_t dockTrail = 0;
/* 条件阈值                  */
const double thresholdDis = 5;                   // 拐点判断阈值
const double thresholdFittingDockSparsity = 1;	 // 直线拟合样本输入点间的稀疏度(即间隙)
const double thresholdFittingDis = 1;            // 拟合直线更新阈值
const double half = 0.5;

/* Function Declaration ------------------------------------------------------*/
/**----------------------------------------------------------------------
* Function    : pushBuffer
* Description : 将输入的位置坐标推入缓存buffer
* Date        : 2020/7/4 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
static void pushBuffer(LatLng inLoc){
	size_t i;
    for (i = outputBufferSize - 1; i > 0; --i){
        OutputBuffer[i] = OutputBuffer[i-1];
    }
    OutputBuffer[0] = inLoc;
    if(outputTrail < outputBufferSize)
        ++outputTrail;
}

/**----------------------------------------------------------------------
* Function    : trackSmoothingSlideWindowSet
* Description : 使用平滑模块时首先调用的函数，设定轨迹平滑窗口大小。也可以通过此函
*               数重新设定滑动窗口大小。
* Input       : slideWindowSize: 滑动窗口的大小
* Output      : size : 当前滑动窗口的大小，一般大于0,当返回大小为0时说明设置滑动
 *              窗口为0或是内存申请失败。
* Date        : 2020/7/4 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
int setTrackSmoothWindow(size_t slideWindowSize)
{
    LatLng *ptr = NULL;
	size_t smoothBufferMallocRes = 0;
	size_t OutputBufferMallocRes = 0;

    ptr = (LatLng *)malloc( sizeof(LatLng) * slideWindowSize);
    if (ptr != NULL)
    {
        smoothBufferMallocRes = slideWindowSize;
        if (SmoothBuffer != NULL)
        {
            free(SmoothBuffer);
            SmoothBuffer = NULL;
            SmoothBufferSize = 0; 
        } 
        SmoothBuffer = ptr;
        SmoothBufferSize = slideWindowSize;
        memset(SmoothBuffer, 0, sizeof(LatLng)*SmoothBufferSize);
        elemTrail = 0;
    }
    else
    {
        smoothBufferMallocRes = 0;
    }
    ptr = NULL;
    ptr = (LatLng *)malloc( sizeof(LatLng) * slideWindowSize);
    if( ptr != NULL)
    {
        OutputBufferMallocRes = slideWindowSize;
        if(OutputBuffer != NULL)
        {
            free(OutputBuffer);
            OutputBuffer = NULL;
            outputBufferSize = 0;
        }
        OutputBuffer = ptr;
        outputBufferSize = slideWindowSize;
        memset(OutputBuffer, 0, sizeof(LatLng)*outputBufferSize);
        outputTrail = 0;
    }
    else
    {
        OutputBufferMallocRes = 0;
    }
    /* 不论是否重新调整SmoothBuffer和SmoothBufferSize，都会重置fittingDock和dockTrail*/
    memset(fittingDock, 0, sizeof(LatLng)*FITTING_DOCK_SIZE);
    dockTrail = 0;

    return (int)((smoothBufferMallocRes && OutputBufferMallocRes)? slideWindowSize: 0);
}
/**----------------------------------------------------------------------
* Function    : trackSmoothing
* Description : 轨迹平滑模块关闭后需要使用此函数释放资源
* Input       : lat: 输入纬度
*               lon: 输入经度
* Output      : outLat: 输出纬度存储变量指针
*               outLon: 输出经度存储变量指针
*               return: 返回处理状态
* Date        : 2020/7/4 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
int pdr_trackSmooth(double lat, double lon, double *outLat, double *outLon)
{
	size_t i;
	double distance;
	LatLng outLatLng;
	LatLng projPointLatLng;
	double fittingDockLat[FITTING_DOCK_SIZE] = { 0 };
	double fittingDockLon[FITTING_DOCK_SIZE] = { 0 };
	LatLng tmp_lla = { 0 };
	double a = 0; double b = 0;    /* y = a*x + b */
	// double sumLat = 0; double sumLon = 0;
	int ret;
	LatLng fittingProjPoint;
	double distanceFitting;
    if( SmoothBuffer == NULL || SmoothBufferSize <= 1)
    {
        *outLat = lat;
        *outLon = lon;
        return 1;
    }
    /* 配置各个缓冲区、列表的相应内容和参数*/
    /* 将SmoothBuffer右移以插入新数据 */
    for (i = SmoothBufferSize -1 ; i > 0 ; --i){
        SmoothBuffer[i] = SmoothBuffer[i-1];
    }
    SmoothBuffer[0].lat = lat;
    SmoothBuffer[0].lon = lon;
    if (elemTrail == 0) { /* 当第一个点输入时，将其设置为第一个拐点 */
        InflectionPoint.lat = lat;
        InflectionPoint.lon = lon;
    }
    /* 及时更新elemTrail */
    if (elemTrail < SmoothBufferSize ) {
        ++elemTrail;
    }
    /* 初始化用于集合的存储点数组fittingDock[FITTING_DOCK_SIZE]*/
    if (dockTrail < FITTING_DOCK_SIZE) {
		if (dockTrail > 0) 
		{	/*当不是fittingDock的首个元素时，要注意fittingDock输入样本的稀疏度*/
			tmp_lla.lat = lat;
			tmp_lla.lon = lon;
			distance = calDistance(tmp_lla, fittingDock[dockTrail - 1]);
			if (distance >= thresholdFittingDockSparsity) {
				fittingDock[dockTrail].lat = lat;
				fittingDock[dockTrail].lon = lon;
				if (dockTrail < outputBufferSize)
					++dockTrail;
			}
		}
		else
		{	/*当本次输入的坐标点是fittingDock的首个元素，直接将此坐标点作为拟合直线的起点*/
			fittingDock[dockTrail].lat = lat;
			fittingDock[dockTrail].lon = lon;
			if (dockTrail < outputBufferSize)
				++dockTrail;
		}
    }
    /* 平滑处理逻辑开始 */
    if(elemTrail < 3 ) //计算拐点的前提是输入点至少有3个
    {
        *outLat = lat;
        *outLon = lon;
		outLatLng.lat = lat;
		outLatLng.lon = lon;
        pushBuffer(outLatLng);

    }
    else
    {
        projPointLatLng = ProjPointOfLatLng(SmoothBuffer[1], InflectionPoint, SmoothBuffer[0]);
		distance = calDistance(SmoothBuffer[1], projPointLatLng);
        /*判断SmoothBuffer[1]到预测直线的距离distance，根据距离的大小执行不同的逻辑*/
        if( distance > thresholdDis)
        {
			/*输入坐标点被判断为拐点后，要更新拐点，清空拟合点集fittingDock*/
            /* 输出点(lat, lon) */
			outLatLng.lat = SmoothBuffer[1].lat;
			outLatLng.lon = SmoothBuffer[1].lon;
            pushBuffer(outLatLng);            
            /* 将(lat, lon)作为新的拐点更新 InflectionPoint */
            InflectionPoint.lat = lat;
            InflectionPoint.lon = lon;
            /* 拐点更新后，重新设置fittingDock中的数值内容 */
            memset(fittingDock, 0, sizeof(LatLng) * FITTING_DOCK_SIZE);
            fittingDock[0].lat = lat;
            fittingDock[0].lon = lon;
            dockTrail = 1;
        }
        else
        {
			/*当输入点被判断为非拐点后，根据拟合点集fittingDock的大小执行不同的逻辑*/
            if( dockTrail >= FITTING_DOCK_SIZE)
            {
                
                for ( i = 0 ; i < FITTING_DOCK_SIZE ; ++i)
                {
                    fittingDockLat[i] = fittingDock[i].lat;
                    fittingDockLon[i] = fittingDock[i].lon;
                }
                ret = linearLeastSquaresFit(fittingDockLat, fittingDockLon, FITTING_DOCK_SIZE, &a, &b);
                fittingProjPoint = SmoothBuffer[1];
                if (ret == 0) /* ret==0表示直线拟合成功 */
                {
                    fittingProjPoint.lat = (a*(SmoothBuffer[1].lon - b) + SmoothBuffer[1].lat) / (vivopow(a, 2) + 1);
                    fittingProjPoint.lon = a*fittingProjPoint.lat + b;
                }
				distanceFitting = calDistance(SmoothBuffer[1], fittingProjPoint);
                if( distanceFitting > thresholdFittingDis)
                {
					// //FIXME: 射线拟合可能有逻辑漏洞，修复。
                    // dockTrail = 2;
                    // fittingDock[1] = fittingProjPoint;
					// //fittingDock[1] = SmoothBuffer[1]; //TEST
                    // LatLng outLatLng = {lat, lon}; //有问题，漏更一个点
                    // pushBuffer(outLatLng);
                    //TODO: 测试新的添点策略
                    dockTrail           = 2;     /* remove the elements whose the index >= 3 */
                    fittingDock[1] = SmoothBuffer[1];
					outLatLng.lat = SmoothBuffer[1].lat;
					outLatLng.lon = SmoothBuffer[1].lon;
                    pushBuffer(outLatLng);
                }
                else
                {
					outLatLng.lat = fittingProjPoint.lat;
					outLatLng.lon = fittingProjPoint.lon;
                    pushBuffer(outLatLng);
                }
			}else{
				/*当fittingDock未被装满，则说明当前输入点不符合fittingDock的输入条件，直接将其输入以保证实时性*/
				outLatLng.lat = lat;
				outLatLng.lon = lon;
				pushBuffer(outLatLng);
			}
        }

		/* 2019-05-22 平滑改动：由均值滤波变成加权求和*/
		double factor = 0;
		if (outputTrail > 1)
		{
			factor = half / (outputTrail - 1);
		}
		double sumLat = 0; double sumLon = 0;
		for (i = 0; i < outputTrail; ++i)
		{
			sumLat += OutputBuffer[i].lat * factor;
			sumLon += OutputBuffer[i].lon * factor;
		}
        *outLat = sumLat + OutputBuffer[0].lat * (half - factor);
        *outLon = sumLon + OutputBuffer[0].lon * (half - factor);
    }
	return 0;
}

/**----------------------------------------------------------------------
* Function    : trackSmoothSlideWindowFree
* Description : 轨迹平滑模块关闭后需要使用此函数释放资源
* Date        : 2020/7/4 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
int freeTrackSmooth(void)
{
    if ( SmoothBuffer != NULL)
    {
        free(SmoothBuffer);
        SmoothBuffer = NULL;
        SmoothBufferSize = 0;
        elemTrail = 0;
    }
    if ( OutputBuffer != NULL)
    {
        free(OutputBuffer);
        OutputBuffer = NULL;
        outputBufferSize = 0;
        outputTrail = 0;
    }
    return 0;
}