/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : pdr_api.c
* Department         : Sensor Algorithm Team
* Current Version    : V2.0
* Author             : logzhan
* 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 "Pedometer.h"
#include "Location.h"
#include "parseData.h"
#include "scene_recognition.h"

/* Global Variable Definition ------------------------------------------------*/
const char* PDR_Version                     = "2.0";
sensor_agm  SensorDataHist[IMU_LAST_COUNT]  = {{0}};
LatLng_t      llaLast                       = { 0 };
lct_nmea    NmeaData;
IMU_t         ImuData;

/* Extern Variable Definition ------------------------------------------------*/
extern KfPara_t   g_kfPara;
extern AHRS_t     g_ahrs;
extern GNSS_t     pgnss;

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

/**---------------------------------------------------------------------
* Function    : RMC_Init
* Description : 初始化RMC协议
* Date        : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
void RMC_Init(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    : NmeaFlag_Init
* Description : 初始化GPS的NMEA协议的相关标志位
* Date        : 2020/7/3 logzhan
*---------------------------------------------------------------------**/
void NmeaFlag_Init(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;

	RMC_Init(&ln->rmc[0]);
	RMC_Init(&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 logzhan
*---------------------------------------------------------------------**/
void ClearNmeaFlg(lct_nmea * ln){
    NmeaFlag_Init(ln);
}

/**---------------------------------------------------------------------
* Function    : Algorithm_Init
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
*               迹平滑窗口的初始化
* Date        : 2020/7/3 logzhan
*---------------------------------------------------------------------**/
void Algorithm_Init(void) {
    // 导航相关初始化
    PDRNav_Init();
    // 计步器初始化
    Pedometer_Init();
    // 初始化sensor历史缓存
	memset(SensorDataHist, 0, sizeof(SensorDataHist));
	memset(&NmeaData, 0, sizeof(NmeaData));
	memset(&ImuData, 0, sizeof(ImuData));
}

/**---------------------------------------------------------------------
* Function    : ParseLineAndUpdate
* Description : 解析字符串信息并更新
* Date        : 2020/07/03 logzhan
*               2020/02/02 logzhan : 增加宏控制是否开启输出平滑
*---------------------------------------------------------------------**/
int ParseLineAndUpdate(char* line, lct_fs* locFusion) {

	ParseLineStr(line, &ImuData, &NmeaData);
	return LocationMainLoop(&ImuData, &NmeaData, locFusion, NULL);
}


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

    if (ImuData->gyr.update) {
        // 如果陀螺仪更新则采用惯性传感器计算
        InsLocation(ImuData,&g_kfPara);
        ImuData->gyr.update = UN_UPDATE;
    }
    // 如果GPS不更新，返回
    if (!NmeaData->update)return TYPE_FIX_NONE;

	// 写GPS相关LOG信息
	SaveGnssInfo(NmeaData, LocFusion, fp_gps);

    // 有GPS则采用GPS融合定位
    int flag = GnssInsFusionLocation(NmeaData, &g_kfPara, LocFusion);

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

        } else {
			LatLng_t lla = { 0 };
            lla.lat = R2D(LocFusion->latitude);
            lla.lon = R2D(LocFusion->longitudinal);

            if (LocFusion->last_lat != 0.0 && LocFusion->last_lon != 0.0 &&
                CalDistance(lla, llaLast) > 10) {
            }
			LocFusion->latitude = R2D(LocFusion->latitude);
			LocFusion->longitudinal = R2D(LocFusion->longitudinal);

			LocFusion->latitude = (LocFusion->latitude) * RADIAN_PER_DEG;
			LocFusion->longitudinal = (LocFusion->longitudinal) * RADIAN_PER_DEG;

			LocFusion->latitude = R2D(LocFusion->latitude);
			LocFusion->longitudinal = R2D(LocFusion->longitudinal);

            LocFusion->last_lat = LocFusion->latitude;
			LocFusion->last_lon = LocFusion->longitudinal;

            llaLast.lat = lla.lat;
            llaLast.lon = lla.lon;
        }
        type = 1;
    }else if (LocFusion->last_lat != 0.0 && LocFusion->last_lon != 0.0) {
        LocFusion->latitude = LocFusion->last_lat;
        LocFusion->longitudinal = LocFusion->last_lon;
        type = 1;
    }
    ClearNmeaFlg(NmeaData);
    return type;
}

/**---------------------------------------------------------------------
* Function    : pdr_saveGnssInfo
* Description : 保存GPS相关信息
* Date        : 2020/7/3 logzhan
*---------------------------------------------------------------------**/
void 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);

		WriteCsvStr(fp_gps, str);

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

/**----------------------------------------------------------------------
* Function    : GetLIBVersion
* Description : 获取pdr库版本号
* Date        : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
const char* GetLIBVersion(void){
    return PDR_Version;
}

/**----------------------------------------------------------------------
* Function    : pdr_closeAlgorithm
* Description : 关闭PDR算法,释放资源
* Date        : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void Algorithm_DeInit() {
	scene_recognition_destroy();
}