/******************** (C) COPYRIGHT 2020 VIVO************************************
* File Name          : pdr_api.c
* Department         : Sensor Algorithm Team
* Current Version    : V2.0
* Author             : yuanlin_rjyb@vivo.com
* Date of Issued     : 2020.7.3
* Comments           : PDR导航算法对外部接口
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "pdr_api.h"
#include "pdr_base.h"
#include "steps.h"
#include "pdr_location.h"
#include "parseData.h"
#include "scene_recognition.h"
#include "pdr_trackSmooth.h"
#include "LapProcess.h"

/* Global Variable Definition ------------------------------------------------*/
const char* pdrVersion                      = "2.0.0.20210130_qcom_sdm845_r";
size_t      slideWindowSize                 = 5; 
sensor_agm  Hist_SensorData[IMU_LAST_COUNT] = {{0}};
LatLng      lla_last                        = { 0 };
lct_nmea    ln;
imu         imu_data;

/* Extern Variable Definition ------------------------------------------------*/
extern KfPara g_kfPara;
extern AHRS   g_ahrs;
extern gnss   pgnss;

/* Function Declaration ------------------------------------------------------*/

/**---------------------------------------------------------------------
* Function    : pdr_initRmc
* Description : 初始化RMC协议
* Date        : 2020/7/3 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_initRmc(lct_nmea_rmc * rmc)
{
	rmc->type            = SENSOR_MIN;
	rmc->rmc_utc         = ITEM_INVALID;
	rmc->status          = POSITIONING_N;
	rmc->latitude        = ITEM_INVALID;
	rmc->rmc_latitude_ns = LATITUDE_N;
	rmc->longitudinal    = ITEM_INVALID;
	rmc->longitudinal_ew = LATITUDE_N;
	rmc->speed           = ITEM_INVALID;
	rmc->cog             = ITEM_INVALID;
	rmc->utc_ddmmyy      = ITEM_INVALID;
	rmc->magnetic        = ITEM_INVALID;
	rmc->mag_h           = MAG_E;
	rmc->mode            = MODE_A;
	rmc->time            = ITEM_INVALID;
	rmc->update          = UN_UPDATE;

	memset(rmc->rmc_check_sum, 0, sizeof(rmc->rmc_check_sum));
}

/**---------------------------------------------------------------------
* Function    : pdr_initNmeaFlg
* Description : 初始化GPS的NMEA协议的相关标志位
* Date        : 2020/7/3 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_initNmeaFlg(lct_nmea * ln)
{
	ln->gga.update = 0;
	for (char i = 0; i < 3; i++) {
		ln->gsa[i].update = UN_UPDATE;
		ln->gsv[i].update = UN_UPDATE;
	}

	ln->minTime = 0;
	ln->maxTime = 0;
	ln->update  = UN_UPDATE;

	pdr_initRmc(&ln->rmc[0]);
	pdr_initRmc(&ln->rmc[1]);

	ln->accuracy.time   = ITEM_INVALID;
	ln->accuracy.update = UN_UPDATE;
	ln->accuracy.status = ITEM_INVALID;
	ln->accuracy.err    = ITEM_INVALID;
}


/**---------------------------------------------------------------------
* Function    : clearNmeaFlg
* Description : 清空Nmea标志位
* Date        : 2020/7/4 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void clearNmeaFlg(lct_nmea * ln){
    pdr_initNmeaFlg(ln);
}

/**---------------------------------------------------------------------
* Function    : pdr_algorithmInit
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
*               迹平滑窗口的初始化
* Date        : 2020/7/3 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_initAlgorithm(void) {
    // 导航相关初始化
    pdr_navSysInit();
    // 计步器初始化
    initPedometer();
    // 设置轨迹平滑滑动窗口大小
    setTrackSmoothWindow(slideWindowSize);
	// 跑圈优化初始化
	lapInit();
    // 初始化sensor历史缓存
	memset(Hist_SensorData, 0, sizeof(Hist_SensorData));

	memset(&ln, 0, sizeof(ln));
	memset(&imu_data, 0, sizeof(imu_data));
}

/**---------------------------------------------------------------------
* Function    : pdr_locationMainLoopStr
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
*               迹平滑窗口的初始化
* Date        : 2020/07/03 yuanlin_rjyb@vivo.com
*               2020/02/02 zhanli@vivo.com : 增加宏控制是否开启输出平滑
*---------------------------------------------------------------------**/
int pdr_locationMainLoopStr(char* line, lct_fs* result) {

	pdr_parseLineStr(line, &imu_data, &ln);
	return pdr_locationMainLoop(&imu_data, &ln, result, NULL);
}


/**---------------------------------------------------------------------
* Function    : pdr_locationMainLoop
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
*               迹平滑窗口的初始化
* Date        : 2020/07/03 yuanlin_rjyb@vivo.com
*               2020/02/02 zhanli@vivo.com : 增加宏控制是否开启输出平滑
*---------------------------------------------------------------------**/
int pdr_locationMainLoop(imu *ss_data, lct_nmea *nmea_data, lct_fs *result, FILE *fp_gps) {
    int type = 0;

    if (ss_data->gyr.update) {
        // 如果陀螺仪更新则采用惯性传感器计算
        pdr_insLocation(ss_data,&g_kfPara);
        ss_data->gyr.update = UN_UPDATE;
    }
    // 如果GPS不更新，返回
    if (!nmea_data->update)return TYPE_FIX_NONE;
	// 保存GPS相关信息
	pdr_saveGnssInfo(nmea_data, result, fp_gps);

    // 有GPS则采用GPS融合定位
    int flag = pdr_gnssInsLocation(nmea_data, &g_kfPara, result);

    if (flag != TYPE_FIX_NONE) {
		// 如果是开车这种，直接输出GPS，不进行平滑处理
        if (flag == TYPE_FIX_GPS) {
            result->latitude = R2D(result->latitude);
            result->longitudinal = R2D(result->longitudinal);

			lapProcess(pgnss, result);
        } else {
			LatLng lla = { 0 };
            lla.lat = R2D(result->latitude);
            lla.lon = R2D(result->longitudinal);

            if (result->last_lat != 0.0 && result->last_lon != 0.0 &&
                calDistance(lla, lla_last) > 10) {
                setTrackSmoothWindow(slideWindowSize);
            }
			result->latitude = R2D(result->latitude);
			result->longitudinal = R2D(result->longitudinal);
			lapProcess(pgnss, result);
			result->latitude = (result->latitude) * RADIAN_PER_DEG;
			result->longitudinal = (result->longitudinal) * RADIAN_PER_DEG;

#if			PDR_OUTPUT_SMOOTH
			double outLat = 0, outLon = 0;
            // 输出轨迹平滑
            pdr_trackSmooth(R2D(result->latitude), R2D(result->longitudinal) ,
                           &outLat, &outLon);

			result->latitude     = outLat;
			result->longitudinal = outLon;
#else
			result->latitude = R2D(result->latitude);
			result->longitudinal = R2D(result->longitudinal);
#endif
            result->last_lat = result->latitude;
			result->last_lon = result->longitudinal;

            lla_last.lat = lla.lat;
            lla_last.lon = lla.lon;
        }
        type = 1;
    }else if (result->last_lat != 0.0 && result->last_lon != 0.0) {
        result->latitude = result->last_lat;
        result->longitudinal = result->last_lon;
        type = 1;
		lapProcess(pgnss, result);
    }
    clearNmeaFlg(nmea_data);
    return type;
}

/**---------------------------------------------------------------------
* Function    : pdr_saveGnssInfo
* Description : 保存GPS相关信息
* Date        : 2020/7/3 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_saveGnssInfo(lct_nmea* nmea_data, lct_fs* result, FILE* fp_gps)
{
	lct_nmea_rmc rmc = nmea_data->rmc[1];
	// 选取更新时间的rmc
	if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
		rmc = nmea_data->rmc[0];
	}
	if (rmc.update && (rmc.status == POSITIONING_Y)) {
		char str[256];
		memset(str, 0, sizeof(str));
		float accuracy = -1.0f;
		if (fabs(rmc.time - nmea_data->accuracy.time) < 500){
			accuracy = nmea_data->accuracy.err;
		}
		sprintf(str, "%f,%.10f,%d,%.10f,%d,%.10f,%f,%f", rmc.time, rmc.latitude, 
			rmc.rmc_latitude_ns,rmc.longitudinal, rmc.longitudinal_ew, rmc.cog, accuracy, 
			rmc.speed);

		pdr_writeCsvStr(fp_gps, str);

		result->gpsLat = rmc.latitude;
		result->gpsLon = rmc.longitudinal;
	}
}

/**----------------------------------------------------------------------
* Function    : pdr_libVersion
* Description : pdr库版本号
* Date        : 2020/8/3 zhanli@vivo.com
*---------------------------------------------------------------------**/
const char* pdr_libVersion(void){
    return pdrVersion;
}

/**----------------------------------------------------------------------
* Function    : pdr_closeAlgorithm
* Description : 关闭PDR算法,释放资源
* Date        : 2020/8/3 zhanli@vivo.com
*---------------------------------------------------------------------**/
void pdr_closeAlgorithm() {
	freeTrackSmooth();
	scene_recognition_destroy();
}