/******************** (C) COPYRIGHT 2022 Geek************************************
* File Name          : Interface.c
* Current Version    : V2.0
* Author             : logzhan
* Date of Issued     : 2022.10.15
* Comments           : PDR导航算法对外部接口
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "Interface.h"
#include "PDRBase.h"
#include "Pedometer.h"
#include "Location.h"
#include "parseData.h"
#include "SceneRecognition.h"
#include "TrackSmooth.h"
#include "LapProcess.h"

/* Global Variable Definition ------------------------------------------------*/
const char* PDR_Version                     = "1.2.0";
size_t      slideWindowSize                 = 5; 
Sensor_t    SensorDataHist[IMU_LAST_COUNT]  = {{0}};
LatLng_t    llaLast                         = { 0 };
Nmea_t      Nmea;
IMU_t       Imu;

/* Extern Variable Definition ------------------------------------------------*/
extern EKFPara_t  EkfPara;
extern AHRS_t     Ahrs;
extern GNSS_t     pgnss;

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

/**---------------------------------------------------------------------
* Function    : RMC_Init
* Description : 初始化RMC协议
* Date        : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
void RMC_Init(NmeaRMC_t * 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(Nmea_t *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(Nmea_t * ln){
    NmeaFlag_Init(ln);
}

/**---------------------------------------------------------------------
* Function    : Algorithm_Init
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
*               迹平滑窗口的初始化
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void Algorithm_Init(void) {
    // 导航相关初始化
    PDR_Init();
    // 计步器初始化
    Pedometer_Init();
    // 设置轨迹平滑滑动窗口大小
    TrackSmoothSetWinSize(slideWindowSize);
	// 跑圈优化初始化
	lapInit();
    // 初始化sensor历史缓存
	memset(SensorDataHist, 0, sizeof(SensorDataHist));
	memset(&Nmea, 0, sizeof(Nmea));
	memset(&Imu, 0, sizeof(Imu));
}

/**---------------------------------------------------------------------
* Function    : ParseDataAndUpdate
* Description : 解析字符串信息并更新
* Date        : 2022/10/16 logzhan
*---------------------------------------------------------------------**/
int ParseDataAndUpdate(char* line, LctFs_t* LocFusion) 
{
	ParseGnssInsData(line, &Imu, &Nmea);
	// 给定Imu和Nmea结构体，返回融合位置
	return PDRLocationMainLoop(&Imu, &Nmea, LocFusion);
}


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

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

	// 写GPS相关LOG信息
	SaveGnssInfo(nmea, LocFusion, NULL);

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

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

			lapProcess(pgnss, LocFusion);
        } 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) {
                TrackSmoothSetWinSize(slideWindowSize);
            }
			LocFusion->latitude = R2D(LocFusion->latitude);
			LocFusion->longitudinal = R2D(LocFusion->longitudinal);
			lapProcess(pgnss, LocFusion);
			LocFusion->latitude = (LocFusion->latitude) * RADIAN_PER_DEG;
			LocFusion->longitudinal = (LocFusion->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
			LocFusion->latitude = R2D(LocFusion->latitude);
			LocFusion->longitudinal = R2D(LocFusion->longitudinal);
#endif
            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;
		lapProcess(pgnss, LocFusion);
    }
    ClearNmeaFlg(nmea);
    return type;
}

/**---------------------------------------------------------------------
* Function    : SaveGnssInfo
* Description : 保存GPS相关信息
* Date        : 2022/10/16 logzhan
*---------------------------------------------------------------------**/
void SaveGnssInfo(Nmea_t* nmea, LctFs_t* result, FILE* fp_gps)
{
	NmeaRMC_t rmc = nmea->rmc[1];
	// 选取更新时间的rmc
	if (nmea->rmc[0].time > nmea->rmc[1].time) {
		rmc = nmea->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->accuracy.time) < 500){
			accuracy = nmea->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    : GetPDRVersion
* Description : 获取pdr版本号
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
const char* GetPDRVersion(void){
    return PDR_Version;
}

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