/******************** (C) COPYRIGHT 2021 Geek************************************
* File Name          : pdr_base.h
* Department         : Sensor Algorithm Team
* Current Version    : V2.0
* Author             : yuanlin@vivocom & 
* 
* 
* Date of Issued     : 2021.02.01
* Comments           : PDR 算法基本定义头文件
********************************************************************************/
#ifndef _PDR_BASE_H_
#define _PDR_BASE_H_
/* Header File Including -----------------------------------------------------*/
#include "stdint.h"
#include "PDRSensor.h"

/* PDR SYS CFG ---------------------------------------------------------------*/
#define PDR_OUTPUT_SMOOTH                   0                    // 是否开启轨迹平滑

/* Macro Definition ----------------------------------------------------------*/
#define PCA_NUM                             200
#define ACCURACY_ERR_MAX                    1000                 // GPS的accuracy最大值,一般用于初始化用
#define N                                   4                    // 矩阵维数
#define OPEN_SKY                            1
#define MAX_NO_GPS_PREDICT                  10		             // 无GPS信息状态，最大位置推算数量
#define UN_UPDATE                           0                    // 非更新状态        
#define DATA_UPDATE                         1                    // 数据更新
#define ON                                  1				     
#define OFF                                 0				     
#define PDR_TRUE                            1				     
#define PDR_FALSE                           0		

#define TYPE_FIX_NONE                       0                    // PDR修正结果为不输出
#define TYPE_FIX_GPS                        2                    // PDR修正结果为输出原始GPS
#define TYPE_FIX_PDR                        1                    // PDR修正结果为PDR融合
															     
// PDR携带方式													  
#define UNKNOWN                             0				     
#define FORWARD_UP_RIGHT                    1                    // 臂包：右臂，头朝上，屏朝外；or左臂，头朝上，屏朝内
#define BACKWARD_UP_LEFT                    2                    // 臂包：左臂，头朝上，屏朝外；or右臂，头朝上，屏朝内
#define BACKWARD_DOWN_RIGHT                 3                    // 臂包：右臂，头朝下，屏朝外；or左臂，头朝下，屏朝内
#define FORWARD_DOWN_LEFT                   4                    // 臂包：左臂，头朝下，屏朝外；or右臂，头朝下，屏朝内
#define LEFT_UP_FORWARD                     5                    // 裤兜：头朝上，屏朝前
#define RIGHT_UP_BACKWARD                   6                    // 裤兜：头朝上，屏朝后
#define RIGHT_DOWN_FORWARD                  7	                 // 裤兜：头朝下，屏朝前
#define LEFT_DOWN_BACKWARD                  8                    // 裤兜：头朝下，屏朝后


// PDR相关状态
#define PDR_NO_ERROR                            0
#define PDR_OUT_OF_MEMORY                       1
#define PDR_MOTION_TYPE_STATIC                  0
#define PDR_MOTION_TYPE_IRREGULAR               1
#define PDR_MOTION_TYPE_HANDHELD                2
#define PDR_MOTION_TYPE_SWINGING                3
#define PDR_STATUS_RESET                        0x80
#define PDR_STATUS_CACULATING_AXIS_CEOFFICIENT  0x40
#define PDR_STATUS_PCA_STABLE                   0x20
#define PDR_STATUS_BIAS_STABLE                  0x10
#define PDR_STATUS_STABLE                       (PDR_STATUS_PCA_STABLE | PDR_STATUS_BIAS_STABLE)

// 地球相关常数
#define WGS84_RE			                6378137				 // WGS-84椭球体长半轴
#define WGS84_ECCENTR2                      6.69437999014e-3     // 第一偏向率的平方，e^2(WGS-84)
#define WGS84_OMEGA_E_DOT                   7.2921151467e-5      // 地球自转角速度 rad/sec
#define WGS84_GRAVITY		                9.7803267714		 // 地球平均重力加速度 m/s^2

#define DEG_PER_RADIAN                      57.295779513082323   // degrees per radian
#define RADIAN_PER_DEG                      0.017453292519943    // radians per degree

#define GPS_SPEED_UNIT                      0.5144444            // GPS原始速度转m/s的缩放系数
#define PI                                  3.1415926 
#define DOUBLE_ZERO                         1e-10
#define TWO_PI                              (2.0*PI)
#define R2D(x)	                            (x*57.2957795130823)
#define D2R(x)                              (x*3.14159265/180.0)
#define USE_BUG_FOR_LOCAL_PARA              1
#define BUF_DMT_1                           1
#define BUF_DMT_2                           2

#define PRINT_ALGO_RUN_TIME                 0
#define PRINT_CLCT                          1
#define PATTERN_RECOGNITION_LEN             256                  // 缓存256 / SAMPLE_RATE的数量

#define OPEN_AREA                           1                    // 开阔区域(GPS信号较好)
#define UN_OPEN_AREA                        0                    // 非开阔区域(GPS信号较弱)


#define DETECTOR_RUN_FREQ                   1280                 // 用户运动类型检测器检测周期ms
#define DETECTOR_TYPE_STATIC                0                    // 用户静止 
#define DETECTOR_TYPE_IRREGULAR             1                    // 无规律运动
#define DETECTOR_TYPE_HANDHELD              2                    // 手持运动
#define DETECTOR_TYPE_SWINGING              3                    // 摆手运动

#define ulong                               unsigned long
#define uchar                               unsigned char

typedef enum {
	IS_INITIAL = 0,
	IS_NORMAL = 1,
}pdrStatus;


typedef struct {
	double Xk[N];                              // 状态变量Xk              xk[0]: 北向x  xk[1]：东向y  xk[2]：步长  xk[3] ：航向角
	double pXk[N];                             // 最佳预测变量Xk_predict  xk[0]: 北向x  xk[1]：东向y  xk[2]：步长  xk[3] ：航向角
	double Zk[N];				                  
	double pPk[N][N];			                  
	double Pk[N][N];			                  
	double Phi[N][N];			                
	double hk[N][N];			                
	double Q[N][N];                             // 卡尔曼滤波的Q矩阵(过程噪声)
	double R[N][N];                             // 卡尔曼滤波R矩阵(观测噪声)
	double Kk[N][N];
	double Lambda;
	double pLat;
	double pLon;
	double initHeading;
}EKFPara_t;

typedef struct{
	int fnum;
	int deltaStep;
	float fsum;
	float meanTime;
	double lastTime;
}StepPredict;

typedef struct PDR {
	uint32_t        status;                     // PDR当前状态
	uint32_t        MotionType;                 // 用户运动类型
	uint32_t        NoGnssCount;                // 没有GNSS信息次数
	pdrStatus       sysStatus;                  // PDR系统状态
	int             sceneType;                  // 场景类型：1：开阔区域，0：非开阔区域(信号较弱)
	int             fusionPdrFlg;               // 融合PDR位置标志位, 当flg为0时纯输出GPS
	double          PllaInit[3];                // 初始plla坐标
	double          ts;                         // 时间戳
	double          x0;                         // 初始北向坐标
	double          y0;                         // 初始东向坐标
	// 航向角相关
	double          heading;                    // PDR方向角
	double          lastHeading;                // 上一次PDR方向角
	double          deltaHeading;               // 偏航角变化量
	double          insHeadingOffset;           // 惯导方向角偏移 
	double          imuDeltaHeading;            // 没间隔一次GPS信号，PDR变化的角度，用于区分转弯            
	double          GpsHeading;                 // GPS航向角
	double          lastGpsHeading;             // 上一次GPS航向角
	double          trackHeading;               // GPS轨迹航向角
	// 速度相关
	double          gpsSpeed;                   // GPS速度
	// 步数相关
	ulong           steps;                      // 当前步数信息
	ulong           lastSteps;                  // 上一次的步数
	ulong           lastGpsSteps;               // 上一次GPS步数
	ulong           deltaStepsPerGps;           // 两次GPS更新之间，步数的变化量
	float           motionFreq;                 // 运动频率
	double          gyroTime;                   // 陀螺仪时间
	float           axis_ceofficient[3];
	float           axis_direction[2];
	float           pca_direction[2];
	float           pca_accuracy;
	float           bias_direction[2];
	float           bias_accuracy;
	float           cal_direction[2];
} PDR_t;

typedef struct {
	int type;
	float accBuffer[PATTERN_RECOGNITION_LEN];
}Classifer_t;

// 用户运动类型分类器
typedef struct DETECTOR {
	uint32_t type;                              // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动
	uint32_t lastType;
	uint64_t tick;                              // 次数统计，用于调整检测器工作频率
}Detector_t;


Detector_t *GetDetectorObj(void);

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

PDR_t* Base_Init(void);

/**----------------------------------------------------------------------
* Function    : pdr_resetData
* Description : 重置PDR变量
* Date        : 2020/7/21 
* 
*---------------------------------------------------------------------**/
void ResetPDRData(void);

/**----------------------------------------------------------------------
* Function    : pdr_computePCA
* Description : 进入RESET_PHASE_1\STABLE阶段之后进行PCA计算
* Date        : 2020/7/21 logzhan
*---------------------------------------------------------------------**/
void ComputePCA(AHRS_t* ahrs);

/**---------------------------------------------------------------------
* Function    : DetectorUpdate
* Description : 根据新确定的手机携带方式更新状态等
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void DetectorUpdate(Detector_t* detector);


void gpsUpdateCb(GNSS_t* gps);


/**************************************************************************
* Description : 计算每步所耗时间
* Input       : stepPredict， 步数预测结构体
				timestamp，时间戳
				steps_last，上一次步数
				steps，本次步数
**************************************************************************/
void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last, unsigned long steps);

/**************************************************************************
* Description : 预测步数
* Input       : stepPredict， 步数预测结构体
				timestamp，时间戳
				steps_last，上一次步数
				gnssVel，gps速度
* Output      : int， 步数
**************************************************************************/
int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel);

/**----------------------------------------------------------------------
* Function    : stateRecognition
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳  0: 平稳
* Date        : 2020/7/22 logzhan
*---------------------------------------------------------------------**/
void StateRecognition(float *acc, Classifer_t *sys);

/**************************************************************************
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
* Input       : g_pdr，PDR结构体
				sys，运动幅度数据结构体
				gps_yaw，GPS轨迹角
				G_yaw，GPS方向角
				ss_data， 传感器数据结构体
* Output      : double，用户行走方向
**************************************************************************/
double calPredAngle(PDR_t *g_pdr, Classifer_t *sys, double gps_yaw, double G_yaw, IMU_t *ss_data);

/**************************************************************************
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
* Input       : g_pdr，PDR结构体
				steps，本次步数
				steps_last，上一次步数
				stepPredict，步数预测结构体
				pdr_angle，用户行走方向
				scene_type，GPS用户所处场景（开阔和非开阔）
				ss_data， 传感器数据结构体
				pgnss，GPS数据结构体
				kf，EKF结构体
				pdr, PDR预测位置成功标志位
**************************************************************************/
void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data, 
	GNSS_t *pgnss, EKFPara_t *kf);

/**----------------------------------------------------------------------
* Function    : DetUserStatic
* Description : 检测用户是否处于静止状态
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
int DetUserStatic(PDR_t *g_pdr, GNSS_t *pgnss, unsigned long delSteps);

/**----------------------------------------------------------------------
* Function    : detIsCarMode
* Description : 识别是否是车载模式
* Input       : pgnss，GPS数据结构体
				g_pdr，PDR结构体
				delSteps， 与上次相比步数的变化量
				time，计数器
* Output      : int，车载模式标志位
* Date        : 2021/01/28 logzhan
*---------------------------------------------------------------------**/
int DetectCarMode(GNSS_t *pgnss, PDR_t *g_pdr, unsigned long delSteps, int *time);

/**----------------------------------------------------------------------
* Function    : detPdrToReset
* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况
*               重置PDR到GPS的位置。
* Date        : 2021/01/28 logzhan
*---------------------------------------------------------------------**/
int DetectPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR_t *g_pdr);

/**************************************************************************
* Description : 初始化EKF滤波器
* Input       : pgnss，GPS数据结构体
				plla_init，NED坐标系原点经纬度
				kf， EKF数据结构体
				x_init，y_init，NED坐标系原点的n和e坐标
				sys_status，定位系统状态（未初始化和已初始化）
* Output      : int，初始化标志位
				result，定位结果结构体
**************************************************************************/
int GnssInsInitialAlignment(GNSS_t *pgnss,  EKFPara_t *kf, PDR_t* g_pdr, LctFs_t *result);

/**************************************************************************
* Description : 设置EKF滤波器的Q和R
* Input       : scane_type, 用户所处场景（开阔和非开阔）
				nmea_data，NMEA数据结构体
				g_pdr，PDR结构体
				sys，运动幅度数据结构体
				kf，EKF数据结构体
**************************************************************************/


/**************************************************************************
* Description : 计算GPS轨迹角
* Input       : gpsPos，GPS历史定位点在ECEF下的三维坐标
				pgnss，GPS数据结构体
* Output      : double，轨迹角
**************************************************************************/
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss);


#endif

#pragma once
