/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : pdr_main.cpp
* Department         : Sensor Algorithm Team
* Current Version    : V2.0
* Author             : logzhan 
* Date of Issued     : 2021.1.28
* Comments           : PDR PC端仿真主文件
* Modify             : 2021/02/04 logzhan : 优化kml显示，kml增加航向角图形
*                      显示，hdop、heading、accuracy信息在Google Map的显示，方便调
*                      试代码。
*                      2021/02/18 logzhan : 增加kml的时间显示，能够拖动进
*                      度条显示轨迹。可以对比查看轨迹效果。
*					
*                      一、优化方向和工作内容
*                      1) GPS速度实际需要乘以0.51444的，但是除了滤波器计算步频之外，
*                      其他地方都是用的实际的GPS速度，包括步数预测
*                      2) 惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化
*                      时间分析发现，buffer mean耗费了大量时间
*                      2021/02/10 logzhan:
*                      1) 需要跟品质谈高通对比版本，防止最后效果和高通SAP5.0持平后，
*                      高通出了新版本，品质不认可效果改进。
*                      2) 目前GPS的速度是没有乘以0.51444的，许多地方计算没有统一，需
*                      要统一。
*                      3）惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化
*                      时间分析发现，buffer mean耗费了大量时间
*                      4)*目前在不依赖磁力计的版本也能达到比以前有磁力计效果还好。但是
*                      磁力计依然是一个重要的辅助确定方向的手段。所以利用GPS航向作为
*                      观测，磁力计启动动态校准，校准完成后才启动辅助磁力计校准方向，
*                      或者作为一个观测量，用于提升预测方向的准确程度。
*                      5)*动态偏移对准，后续会开始开发PCA方向估计，那么PCA估计方向肯定
*                      和真实方向存在一定的偏差，需要动态对准偏移。
*                      6）目前的计步器，输出不够实时，对于步长的估计不够准确。目前测试
*                      的情况是，加入动态步长估计后貌似轨迹会偏大，是不是计步偏少？
*                      7）这个步频计算用到了fft，感觉没有太大作用，可以考虑在新版本中
*                      去除，然后以后开发时如果需要可以参考原理重新优化。
*                      8) 赖工提到路网信息，目前比较确定的是能拿到路的方向信息，考虑
*                      在选定的场景标定一下经纬度和最近的道路方向信息，观察如何提升
*                      定位精度(方向辅助，减少方向上的漂移)。
*                      9) PDR 目前一定概率初始对准方向会偏，所以初始对准时，检测是否
*                      是直线路径，然后考虑用轨迹计算平均方向角，辅助初始化。
*                      10) 跟张经睿讨论过，场景识别模块以前主要是针对以前的单频手机，
*                      所以这个模块考虑重构，因为以前单频手机的参数和现在双频手机不同。
*                      需要重新优化。
*                      11) 目前PDR算法在观测方向时，还是一定概率会被带偏，所以导致转弯
*                      转向不够，然而又不够相信GPS方向，考虑通过多次GPS方向角方差不大时
*                      调整GPS方向角的噪声策略。
*                      12) kml支持时间信息，可以拖动进度条比较轨迹同时运行的情况           ---- logzhan finish
*                      
*                      二、调整规划为：
                           第一期：手持稳定优化 + 摆手(平滑模块主要生效) + 多圈优化 => 不会比之前效果差，部分场景提升
						   第二期：手持稳定优化 + 摆手(PCA + 平滑) + 多圈         => 在摆手场景优化，覆盖一大半场景
						   第三期：手持稳定优化 + 摆手 + 任意动作                 => 鲁棒性优化，覆盖全部使用场景
						   第四期：手持稳定优化 + 摆手 + 任意动作 + 多圈 + 真值仪  => 保证轨迹优化和方向优化条件下，精度提升，可以方便对外
********************************************************************************/
#include <stdio.h>
#include <string>
#include <errno.h>
#include <stdlib.h>
#include "Interface.h"
#include "Main.h"
#include "Utils.h"
#include "time.h"
using namespace std;

/* Global Variable Definition ------------------------------------------------*/
ResultTracks resTracks;
FILE*        SimFile = NULL;
/* Extern Variable Definition ------------------------------------------------*/
extern "C" PDR    Pdr;
extern "C" double refGpsYaw;

/**----------------------------------------------------------------------
* Function    : main
* Description : pdr算法仿真用的main函数
* Date        : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
int main(int argc, char* argv[])
{

	string logPath  = "E:\\SoftwareProject\\GnssIns\\PDR_Data\\Datasets\\";
	string logDate  = "shenzhen";
	string logLabel = "data\\";
	string fileHead;
	char line[256] = { 0 };

	// 解析命令行, 用于解析参数
	for (int i = 0; i < argc; i++) {
		string s = argv[i];
		if(i == 1)logPath = s;
		if(i == 2)logDate = s;
	}

	clock_t t = clock();
	// 获取kml输出路径
	string kmlPath     = logPath  + logDate + "\\output\\";
	// 仿真文件夹路径和Catalog.txt路径
	string filePath    = logPath  + logDate + "\\" + logLabel + "\\";
	string catalogPath = filePath + "catalog.txt";

	FILE* catalogFp = fopen(catalogPath.c_str(), "r");
	if (catalogFp == NULL){
		printf("open catalog.txt file %s error %s\n", catalogPath.c_str(), 
			strerror(errno));
		system("pause");
		return -1;
	}

	while (!feof(catalogFp))
	{
		FILE* fileFp = GetSimulateFile(catalogFp, filePath, fileHead);
		// PDR 仿真流程
		Algorithm_Init();

		LctFs_t locFusion;
		memset(&locFusion,0, sizeof(locFusion));
		memset(&resTracks, 0, sizeof(resTracks));

		// 解析文本仿真
		while (fgets(line, 256, fileFp)){
			// 解析文本数据仿真
			if (ParseDataAndUpdate(line, &locFusion) != TYPE_FIX_NONE){
				// 更新结果轨迹，用于最后的kml生成
				UpdateResTrack(resTracks, locFusion);
			}
		}
		// 关闭PDR算法，释放资源
		Algorithm_DeInit();
		// 写结果kml
		KmlWrite(kmlPath, fileHead, "_PDR_" + string(GetPDRVersion()));
		// 关闭文件
		fclose(fileFp);
	}
	// 打印仿真时间信息
	printf("PDR simulate spends %f s\n", ((double)(clock() - t) / CLOCKS_PER_SEC));
	system("pause");
	return 0;
}

/**----------------------------------------------------------------------
* Function    : GetSimulateFile
* Description : 给定文件名，以及文件路径，获取仿真文件的文件指针
*               catalogFp : 目录文件的文件指针
*               path_file ：仿真文件的文件目录
*               fileHead  : 通过C++引用返回的去除了扩展名的文件名称
* Date        : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
FILE* GetSimulateFile(FILE* catalogFp, string path_file, string& fileHead) {

	char file_name[256] = { 0 };
	FILE* fp = NULL;

	if (fscanf(catalogFp, "%s\n", file_name) == -1){
		printf("get data file name failed\n");
		system("pause");
		exit(-1);
	}
	string fileNamePath = path_file + string(file_name);
	fp = fopen(fileNamePath.c_str(), "rb");
	if (fp == NULL){
		printf("open file %s failed error %s\n", fileNamePath.c_str(), strerror(errno));
		system("pause");
		exit(-1);
	}
	// 去除文件扩展名.csv
	fileHead = file_name;
	fileHead = fileHead.substr(0, fileHead.length() - 4);

	printf("%s\n", fileNamePath.c_str());
	return fp;
}

/**----------------------------------------------------------------------
* Function    : gpsYaw2GoogleYaw
* Description : 为了让kml显示的角度方位正常，需要把0-360顺时针旋转的Yaw转换
*               为谷歌支持的Yaw规则
* Date        : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
double gpsYaw2GoogleYaw(double heading) {
	double gh = R2D(heading) - 180;
	while (gh > 360)gh -= 360;
	while (gh < 0)gh += 360;
	return gh;
}

/**----------------------------------------------------------------------
* Function    : UpdateResTrack
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
* Date        : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs)
{
	resTrack.pdrTrack[resTrack.pdrLen].lat = lctfs.latitude;
	resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal;
	resTrack.pdrTrack[resTrack.pdrLen].heading = 
		gpsYaw2GoogleYaw(lctfs.pdrHeading);
	resTrack.pdrTrack[resTrack.pdrLen].hdop = lctfs.hdop;
	resTrack.pdrTrack[resTrack.pdrLen].accuracy = lctfs.accuracy;
	resTrack.pdrTrack[resTrack.pdrLen].time = lctfs.time;
	resTrack.pdrTrack[resTrack.pdrLen].motionType = lctfs.motionType;
	resTrack.pdrLen++;

	if (lctfs.gpsLat > 0 && lctfs.gpsLon > 0) {
		resTrack.gpsTrack[resTrack.gpsLen].lat = lctfs.gpsLat;
		resTrack.gpsTrack[resTrack.gpsLen].lon = lctfs.gpsLon;
		resTrack.gpsTrack[resTrack.gpsLen].heading = 
			gpsYaw2GoogleYaw(lctfs.gpsHeading);

		resTrack.gpsTrack[resTrack.gpsLen].vel = lctfs.gpsSpeed;
		resTrack.gpsTrack[resTrack.gpsLen].hdop = lctfs.hdop;
		resTrack.gpsTrack[resTrack.gpsLen].accuracy = lctfs.accuracy;
		resTrack.gpsTrack[resTrack.gpsLen].time = lctfs.time;
		resTrack.gpsLen++;
	}
}

string time2str(double t) {
	long time = (long)t;
	if (t < 10) {
		string str = "0" + std::to_string(time);
		return str;
	}else {
		string str = std::to_string(time);
		return str;
	}
}

/**----------------------------------------------------------------------
* Function    : KmlWrite
* Description : 将pdr算法输出的gps和pdr轨迹写为kml形式
*               path    : kml文件的输出文件路径
*               name    : kml文件主体名称
*               postfix ：在主体名称后面添加的后缀，用于区分类型或者版本
* Date        : 2020/11/1 logzhan
*---------------------------------------------------------------------**/
void KmlWrite(string path, string name, string postfix)
{
	string pdrColor = "ff0000ff";
	string gpsColor = "ff00ffff";

	if (path == "") {
		return;
	}

	string kmlPath = path + name + postfix + ".kml";
	string kmlName = name + postfix;
	FILE* fid = fopen(kmlPath.c_str(), "w");

	fprintf(fid, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
	fprintf(fid, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
	fprintf(fid, "\t<Document>\n");
	fprintf(fid, "\t\t<name>%s</name>\n", kmlName.c_str());

	fprintf(fid, "\t\t<Folder id=\"ID1\">\n");
	fprintf(fid, "\t\t<name>GPS Fixs</name>\n");

	// 写gps结果
	for (int i = 0; i < resTracks.gpsLen; i++) {
		fprintf(fid, "\t\t<Placemark>\n");
		fprintf(fid, "\t\t\t<Style>\n");
		fprintf(fid, "\t\t\t\t<IconStyle>\n");
		fprintf(fid, "\t\t\t\t\t<Icon>\n");
		fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
			"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
		fprintf(fid, "\t\t\t\t\t</Icon>\n");
		fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);

		// 描述轨迹方向信息
		fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n", resTracks.gpsTrack[i].heading);
		fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", gpsColor.c_str());
		fprintf(fid, "\t\t\t\t</IconStyle>\n");
		fprintf(fid, "\t\t\t</Style>\n");

		// 写入时间信息
		double h, m, s;
		pdr_utc2hms(resTracks.gpsTrack[i].time, &h, &m, &s);
		fprintf(fid, "\t\t\t<TimeStamp>\n");
		fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
			time2str(m).c_str(), time2str(s).c_str());
		fprintf(fid, "\t\t\t</TimeStamp>\n");

		// 写入描述信息
		fprintf(fid, "\t\t\t<description>\n");
		fprintf(fid, "\t\t\t\t<![CDATA[\n");
		fprintf(fid, "\t\t\t\t<dl>\n");
		fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
		fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
			resTracks.gpsTrack[i].hdop);
		fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m)</dd>\n",
			resTracks.gpsTrack[i].accuracy);
		fprintf(fid, "\t\t\t\t<dd>speed : %.2f (m/s)</dd>\n",
			resTracks.gpsTrack[i].vel);
		fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n", 
			resTracks.gpsTrack[i].heading);
		fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
		fprintf(fid, "\t\t\t</description>\n");
		// 写入坐标信息
		fprintf(fid, "\t\t\t<Point>\n");
		fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
			resTracks.gpsTrack[i].lon,
			resTracks.gpsTrack[i].lat);
		fprintf(fid, "\t\t\t</Point>\n");
		fprintf(fid, "\t\t</Placemark>\n");
	}
	fprintf(fid, "\t\t</Folder>\n");

	fprintf(fid, "\t\t<Folder id=\"ID2\">\n");
	fprintf(fid, "\t\t<name>PDR Fixs</name>\n");
	// 写pdr结果
	for (int i = 0; i < resTracks.pdrLen; i++) {
		fprintf(fid, "\t\t<Placemark>\n");
		fprintf(fid, "\t\t\t<Style>\n");
		fprintf(fid, "\t\t\t\t<IconStyle>\n");
		fprintf(fid, "\t\t\t\t\t<Icon>\n");
		fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
			"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
		fprintf(fid, "\t\t\t\t\t</Icon>\n");
		fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
		fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n", 
			resTracks.pdrTrack[i].heading);

		fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", pdrColor.c_str());
		fprintf(fid, "\t\t\t\t</IconStyle>\n");
		fprintf(fid, "\t\t\t</Style>\n");

		// 写入时间信息
		double h, m, s;
		pdr_utc2hms(resTracks.pdrTrack[i].time, &h, &m, &s);
		fprintf(fid, "\t\t\t<TimeStamp>\n");
		fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(), 
			time2str(m).c_str(), time2str(s).c_str());
		fprintf(fid, "\t\t\t</TimeStamp>\n");

		// description
		fprintf(fid, "\t\t\t<description>\n");
		fprintf(fid, "\t\t\t\t<![CDATA[\n");
		fprintf(fid, "\t\t\t\t<dl>\n");
		fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
		fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n", 
			resTracks.pdrTrack[i].hdop);

		fprintf(fid, "\t\t\t\t<dd>MOTION TYPE : %s (m/s)</dd>\n",
			Motion2TypeStr(resTracks.pdrTrack[i].motionType));

		fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m/s)</dd>\n",
			resTracks.pdrTrack[i].accuracy);
		fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
			resTracks.pdrTrack[i].heading);
		fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
		fprintf(fid, "\t\t\t</description>\n");

		fprintf(fid, "\t\t\t<Point>\n");
		fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
			resTracks.pdrTrack[i].lon,
			resTracks.pdrTrack[i].lat);
		fprintf(fid, "\t\t\t</Point>\n");
		fprintf(fid, "\t\t</Placemark>\n");
	}
	fprintf(fid, "\t\t</Folder>\n");

	fprintf(fid, "\t</Document>\n");
	fprintf(fid, "</kml>\n");

	fclose(fid);
}


#ifdef _WIN32
/**----------------------------------------------------------------------
* Function    : PDR_Init
* Description : 初始化PDR算法(DLL调用)
* Date        : 2022/09/15 logzhan
*---------------------------------------------------------------------**/
void LibPDR_Init() {
	Algorithm_Init();
}

/**----------------------------------------------------------------------
* Function    : PDR_StepUpdateGPS
* Description : 仿真器按照GPS单步执行(DLL调用)
* Date        : 2022/9/16 logzhan
*---------------------------------------------------------------------**/
PosFusion LibPDR_StepUpdateGPS(int useGpsFlg)
{
	PosFusion fusion;
	fusion.vaild = 0;

	Nmea_t location_nmea;
	Nmea_t* ln = &location_nmea;
	imu imu_data;
	LctFs_t lct_fusion;
	memset(&lct_fusion, 0, sizeof(lct_fusion));
	//memset(&location_nmea, 0, sizeof(location_nmea));
	memset(&imu_data, 0, sizeof(imu_data));
	/*memset(&kf, 0, sizeof(kf));*/
	char line[256] = { 0 };
	while (fgets(line, 256, SimFile)) {

		int flag = ParseDataAndUpdate(line, &lct_fusion);

		if (flag) {

			fusion.gpsLat = lct_fusion.gpsLat;
			fusion.gpsLon = lct_fusion.gpsLon;
		}

		// 写PDR融合结果
		if (flag) {
			fusion.lat = lct_fusion.latitude;
			fusion.lon = lct_fusion.longitudinal;

			fusion.t = lct_fusion.time;
			fusion.vaild = 1;

			break;
		}
	}
	return fusion;
}

/**----------------------------------------------------------------------
* Function    : setSimulatorFileCsvPath
* Description : 设置仿真文件的路径(DLL调用)
* Date        : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void setSimulatorFileCsvPath(char* path) {
	if (SimFile == NULL) {
		SimFile = fopen(path, "rb");
	}
	else {
		fclose(SimFile);
		SimFile = NULL;
	}
}

/**----------------------------------------------------------------------
* Function    : setRefGpsYaw
* Description : 设置GPS参考角度
* Date        : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void setRefGpsYaw() {
	refGpsYaw = Pdr.GpsHeading;

}

/**----------------------------------------------------------------------
* Function    : closePdrAlgo
* Description : 关闭pdr算法，释放相关内存，用于dll调用(DLL调用)
* Date        : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void LibPDR_DeInit() {
	Algorithm_DeInit();

	if (SimFile != NULL) {
		fclose(SimFile);
		SimFile = NULL;
	}
}
#endif