/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : pdr_location.c
* Current Version    : V1.2
* Author             : logzhan
* Date of Issued     : 2020.7.3
* Comments           : PDR导航算法主流程
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h> 
#include <string.h>
#include <malloc.h>
#include <errno.h>
#include <stdlib.h>
#include <stdbool.h>
#include "Location.h"
#include "Kalman.h"
#include "Pedometer.h"
#include "AHRS.h"
#include "Detector.h"
#include "Utils.h"
#include "SceneRecognition.h"
#include "CoordTrans.h"

/* Global Variable Definition ------------------------------------------------*/
PDR_t*          pdr;                                  // PDR全局信息
StepPredict     stepPredict = { 0 };
Classifer_t     sys;
GNSS_t          pgnss;                                // 定位相关信息
EKFPara_t       EkfPara;
double          GpsPos[HIST_GPS_NUM][3] = {{0}};
static uint32_t g_status;
extern AHRS_t   Ahrs;

double refGpsYaw    = -1000;
double yaw_bias     = -1;

float dir[2] = { 0 };
float bias_dir[2] = { 0 };

/**---------------------------------------------------------------------
* Function    : PDRNav_Init
* Description : PDR导航系统初始化
* Date        : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void PDR_Init(void) {

	memset(&EkfPara,    0, sizeof(EkfPara));
	memset(&pgnss,       0, sizeof(pgnss));
	memset(GpsPos,       0, sizeof(GpsPos));
	memset(&stepPredict, 0, sizeof(stepPredict));
	memset(&sys,         0, sizeof(sys));
	memset(dir,          0, sizeof(dir));
	memset(bias_dir,     0, sizeof(bias_dir));

	// GNSS定位初始化
    InitGnssInfo();
    // 初始化AHRS姿态解算相关
    AHRS_Init();
    // 检测器初始化
    Detector_Init();
    // PDR初始化
    pdr = Base_Init();
	//base文件中姿态角计算初始化
	EKF_Init();
    // 场景识别模块初始化
    SceneRecog_Init();

	yaw_bias = -1;
}

/**---------------------------------------------------------------------
* Function    : InitGnssInfo
* Description : PDS GNSS定位信息初始化
* Date        : 2020/02/01 logzhan:
*               创建初始版本
*               2020/02/08: logzhan 修改initGps为initGnssInfo，更
*               加符合函数功能
*---------------------------------------------------------------------**/
void InitGnssInfo(void) {
    pgnss.lastError    = ACCURACY_ERR_MAX;
    pgnss.overVelCount = 0;
}

/**---------------------------------------------------------------------
* Function    : ResetSystemStatus
* Description : PDR 重设系统状态
* Date        : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
void ResetSystemStatus(EKFPara_t *kf) 
{
    memset(kf, 0, sizeof(EKFPara_t));
    pdr->fusionPdrFlg = PDR_FALSE;
    pdr->sysStatus    = IS_INITIAL;
    pgnss.overVelCount = 0;
}

/**---------------------------------------------------------------------
* Function    : InsLocationUpdate
* Description : PDR 惯性导航定位
* Date        : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
int InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf) {

    // AHRS姿态更新占2%的运行时间
    if (UpdateAHRS(ImuData)) {
        // 完成姿态估计后计算PCA， 占26%的运行时间
        ComputePCA(&Ahrs);
    }
    // 更新用户运动类型检测器IMU信息以及用户运动类型检测, 占21%的运行时间
    DetectorUpdateIMU(ImuData);

    pdr->gyroTime = ImuData->gyr.time;
    // 为什么手持类型是0.1的运动频率？
    if (pdr->MotionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f;

    // 计步器输入传感器数据，占10%的运行时间
    PedometerUpdate(ImuData, &pdr->steps);

    // ----- Filter Initialization ------- //
    if (pdr->sysStatus == IS_INITIAL) {
        pdr->ts = ImuData->gyr.time;
        pdr->lastSteps = pdr->steps;
        stepPredict.lastTime = 0;
        stepPredict.fnum = 0;
        stepPredict.fsum = 0;
        return 0;
    }

    pdr->ts = ImuData->gyr.time;

    if ((0 == ImuData->acc.s[0] && 0 == ImuData->acc.s[1] && 0 == ImuData->acc.s[2]) ||
        (0 == ImuData->mag.s[0] && 0 == ImuData->mag.s[1] && 0 == ImuData->mag.s[2])) {
        return 0;
    }

    // 识别当前状态是否平稳，这个函数计算量到达整个系统10%的计算量, 以后根据情况进行优化
    // stateRecognition(ss_data->acc.s, &sys);

    pdr->heading = calPredAngle(pdr, &sys, pdr->GpsHeading, pdr->trackHeading, ImuData);
    if (yaw_bias != -1) {
        if (pdr->pca_accuracy > 0 && pdr->pca_accuracy < 0.1 && pdr->MotionType == 3 &&
            (pdr->bias_accuracy > 1.5 || pdr->bias_accuracy == -1 || pgnss.error > 30)
            && (fabs(yaw_bias - 2 * PI) < YAW_THRES * PI / 180 || fabs(yaw_bias - PI) < YAW_THRES * PI / 180 ||
                fabs(yaw_bias) < YAW_THRES * PI / 180)) {
            dir[0] = pdr->pca_direction[0] * bias_dir[0] + pdr->pca_direction[1] * bias_dir[1];
            dir[1] = pdr->pca_direction[1] * bias_dir[0] - pdr->pca_direction[0] * bias_dir[1];
            pdr->heading = atan2(dir[1], dir[0]);
            if (pdr->heading < 0) {
                pdr->heading += TWO_PI;
            }
        }
    }

    double imuYaw = - Ahrs.Yaw;
    if (imuYaw < 0) {
        imuYaw += 360;
    }

    pdr->heading = imuYaw / (180 / 3.14159265);
    if (fabs(pdr->insHeadingOffset) > 0.000001) {
        imuYaw = imuYaw - pdr->insHeadingOffset * (180 / 3.14159265);
        pdr->heading = imuYaw / (180 / 3.14159265);
    }

    // PDR 惯性位置预测
    InsLocationPredict(pdr, &stepPredict, ImuData, &pgnss, Ekf);

    pdr->lastSteps   = pdr->steps;
    pdr->lastHeading = pdr->heading;
    return 0;
}

/**----------------------------------------------------------------------
* Function    : GnssInsFusionLocation
* Description : PDR GPS融合INS惯性定位
* Date        : 2021/01/29 logzhan 
*---------------------------------------------------------------------**/
int GnssInsFusionLocation(Nmea_t *nmea, EKFPara_t *kf, LctFs_t *result)
{
    Nmea2Gnss(nmea, &pgnss);
    // 检查时间差是否满足条件
    if (fabs(pgnss.timestamps - pdr->ts) >= 1000)return TYPE_FIX_NONE;

	// 根据nmea数据识别场景是否是开阔天空场景
	pdr->sceneType = isOpenArea(nmea);
    // 根据HDOP、速度、卫星数量等判断gps yaw是否可用
   
    pdr->gpsSpeed = pgnss.vel * GPS_SPEED_UNIT;
    pdr->lastGpsHeading = pdr->GpsHeading;
    pdr->GpsHeading = GnssCalHeading(&pgnss);
    
    // 如果在win32平台下仿真调试,则添加下面代码
#ifdef _WIN32
    result->pdrHeading = kf->pXk[3];
    result->gpsHeading = pdr->GpsHeading;
    result->hdop       = pgnss.HDOP;
    result->accuracy   = nmea->accuracy.err;
    result->gpsSpeed   = pdr->gpsSpeed;
    result->motionType = pdr->MotionType;
#endif

    // 利用Nmea信息修正Ins航向角
    CalPdrHeadingOffset(nmea, pdr);
    
    // 计算两次GPS时间之间，步数变化量
    pdr->deltaStepsPerGps = pdr->steps - pdr->lastGpsSteps;
    pdr->lastGpsSteps = pdr->steps;

    int mode = DetectFixMode(&pgnss, kf, pdr, result);
    if (mode != TYPE_FIX_PDR)return mode;

	//根据历史GPS拟合输出预估的GPS轨迹航向角
    pdr->trackHeading = CalGnssTrackHeading(GpsPos, pgnss);
	// 如果系统没有初始化，则先初始化
    if (pdr->sysStatus == IS_INITIAL) {
        // Gnss系统初始对准
        return GnssInsInitialAlignment(&pgnss, kf, pdr, result);
    }
    // 如果惯导没有达到输出条件，则不预测轨迹
    if (!pdr->fusionPdrFlg) 
    {
        return ResetOutputLoction(&pgnss, pdr, kf, result);
    }

    // 根据GPS信息, 计算kf的过程噪声和观测噪声
    EKFCalQRMatrix(nmea, pdr, &sys, kf);

    // 运行卡尔曼位置融合
    if (pgnss.satellites_num > 4){
        // 如果GPS航向角处于无效状态且速度也是无效状态
        if (pgnss.yaw == INVAILD_GPS_YAW || pgnss.vel <= 0.0) {
            //if(pdr->heading != INVAILD_GPS_YAW){
            //    pgnss.yaw = (float)(R2D(pdr->heading));
            //}else if (pdr->trackHeading != INVAILD_GPS_YAW) {
            //    pgnss.yaw = (float)(R2D(pdr->trackHeading));
            //}
            pgnss.yaw = (float)kf->pXk[3];
        }
        if (pgnss.yaw != INVAILD_GPS_YAW) {
            EkfGnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result);
            return TYPE_FIX_PDR;
        }
    }
    
    // 不满足推算条件，直接输出原始GPS
    if (pgnss.satellites_num > 4 && nmea->accuracy.err > 0 &&
        nmea->accuracy.err < 15) {
        OutputRawGnssPos(&pgnss, result);
        pdr->NoGnssCount = 0;
        return TYPE_FIX_GPS;
    }

    // 如果没有GPS信号，那么PDR也会接着推算位置, 但是最多推算10个点
    if (pdr->NoGnssCount < MAX_NO_GPS_PREDICT) {
        // 无GPS状态位置推算
        NoGnssInfoPredict(kf, result, pdr);
        return TYPE_FIX_PDR;
    }
    pdr->NoGnssCount++;
	return 0;
}

/**----------------------------------------------------------------------
* Function    : NoGnssInfoPredict
* Description : 在没有gps信息时预测GPS位置
* Date        : 2022/09/16 
*---------------------------------------------------------------------**/
void NoGnssInfoPredict(EKFPara_t* kf, LctFs_t* result, PDR_t* p_pdr)
{
    double ned[3] = {0}, lla[3] = {0};
    ned[0] = kf->pXk[0] - p_pdr->x0;
    ned[1] = kf->pXk[1] - p_pdr->y0;
    ned[2] = 0;

    NED2WGS84(p_pdr->PllaInit, ned, lla);

    result->latitude        = lla[0];
    result->latitude_ns     = pgnss.lat_ns;
    result->longitudinal    = lla[1];
    result->longitudinal_ew = pgnss.lon_ew;
    result->time            = pgnss.timestamps;
    p_pdr->NoGnssCount++;
}

/**----------------------------------------------------------------------
* Function    : calPdrHeadingOffset
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
* Date        : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr) {
    //惯导姿态角与GPS行进角度的对准（判断GPS信号是否正常以及惯导是否平稳）
    if (refGpsYaw != -1000)return;

    if ((pgnss.satellites_num > 4 && pgnss.HDOP < 2.0 && pgnss.yaw != -1 && pgnss.vel >= 1.0)
        && (nmea_data->accuracy.err > 0 && nmea_data->accuracy.err < 4)
        && (DetUserStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->MotionType == 2))
    {
        /*这里放姿态角与GPS对准part1 start*/
        double imuHeading = atan2(2 * (Ahrs.qnb[0] * Ahrs.qnb[3] + Ahrs.qnb[1] * Ahrs.qnb[2]), 
            1 - 2 * (Ahrs.qnb[2] * Ahrs.qnb[2] + Ahrs.qnb[3] * Ahrs.qnb[3]));
        if (imuHeading < 0) {
            imuHeading += TWO_PI;
        }

        if (fabs(imuHeading - p_pdr->GpsHeading) < PI / 18)
        {
            //printf("offset 标定\n");
            //p_pdr->insHeadingOffset = imuHeading - p_pdr->GpsHeading;
        }
    }
}

/**----------------------------------------------------------------------
* Function    : DetectFixMode
* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出
*               原始GPS或者不输出
* Date        : 2020/07/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
*               代码的可读性
*---------------------------------------------------------------------**/
int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* pdr, LctFs_t* result) {
    // 检测用户是否处于静止状态
    if (DetUserStatic(pdr, pgnss, pdr->deltaStepsPerGps)) {
        if (pgnss->error > 0 && pgnss->error < 15 &&
            pgnss->lastError >= pgnss->error) {
            OutputRawGnssPos(pgnss, result);
            pgnss->lastError = pgnss->error;
            return TYPE_FIX_PDR;
        }
        // 否则不输出位置
        return TYPE_FIX_NONE;
    }
    // 如果是检测到是开车状态则输出GPS位置
    if (DetectCarMode(pgnss, pdr, pdr->deltaStepsPerGps, &pgnss->overVelCount)) {
        // 如果检测到车载模式，则重置PDR状态，需要重新初始化
        ResetSystemStatus(kf);
        if (pgnss->error >= 0 || (pgnss->satellites_num > 4
            && pgnss->HDOP < 2.0)) {
            OutputRawGnssPos(pgnss, result);
            return TYPE_FIX_GPS;
        }
        // 精度过差，不输出位置
        return TYPE_FIX_NONE;
    }
    return TYPE_FIX_PDR;
}


/**----------------------------------------------------------------------
* Function    : GnssCalHeading
* Description : 获取GPS偏航角(0 - 360°)
* Date        : 2022/09/16 logzhan
*               代码的可读性
*---------------------------------------------------------------------**/
double GnssCalHeading(GNSS_t* pgnss) {
    double gnssHeading = INVAILD_GPS_YAW;
    if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.0
        && pgnss->yaw != INVAILD_GPS_YAW && pgnss->error < 50) {
        gnssHeading = pgnss->yaw * RADIAN_PER_DEG;
    }
    return gnssHeading;
}

/**----------------------------------------------------------------------
* Function    : pdr_nmea2Gnss
* Description : nmea数据结构转gnss数据
* Date        : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss) 
{
    NmeaRMC_t rmc = { 0 };
    if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
        rmc = nmea_data->rmc[0];
    }else {
        rmc = nmea_data->rmc[1];
    }
    // 更新上一次的Accuracy Error
    pgnss->lastError      = pgnss->error;
    pgnss->lat            = rmc.latitude * PI / 180;
    pgnss->lat_ns         = rmc.rmc_latitude_ns;
    pgnss->lon            = rmc.longitudinal * PI / 180;
    pgnss->lon_ew         = rmc.longitudinal_ew;
    //pgnss->vel            = rmc.speed * GPS_SPEED_UNIT;
    pgnss->vel            = rmc.speed;
    pgnss->HDOP           = nmea_data->gsa[0].hdop;
    pgnss->yaw            = rmc.cog;
    pgnss->quality        = 0;
    pgnss->error          = nmea_data->accuracy.err;
    pgnss->timestamps     = rmc.time;
    pgnss->satellites_num = nmea_data->gga.satellites_nb;

}
/**----------------------------------------------------------------------
* Function    : OutputRawGnssPos
* Description : 输出GPS位置
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result) {
    result->latitude = pgnss->lat;
    result->latitude_ns = pgnss->lat_ns;
    result->longitudinal = pgnss->lon;
    result->longitudinal_ew = pgnss->lon_ew;
    result->time = pgnss->timestamps;
}


/**----------------------------------------------------------------------
* Function    : ResetOutputLoction
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
*               2、GPS值赋值给result
* Date        : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
int ResetOutputLoction(GNSS_t* pgnss, PDR_t* g_pdr, EKFPara_t* kf, LctFs_t* result) {

    double stepLength = 0.7;
    double ned[3] = { 0 };

    g_pdr->PllaInit[0] = pgnss->lat;
    g_pdr->PllaInit[1] = pgnss->lon;
    g_pdr->PllaInit[2] = 0;
    WGS842NED(g_pdr->PllaInit, g_pdr->PllaInit, ned);

    g_pdr->x0 = ned[0];
    g_pdr->y0 = ned[1];

    kf->pXk[0] = ned[0];
    kf->pXk[1] = ned[1];
    kf->pXk[2] = stepLength;
    // 重置输出GPS时，GPS的航向角不一定准
    kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG;

    for (uchar i = 0; i < N; i++) {
        kf->Xk[i] = kf->pXk[i];
    }
    // 清除卡尔曼Q矩阵和R矩阵
    for (int i = 0; i < N; i++) {
        for (int l = 0; l < N; l++) {
            kf->Q[i][l] = 0.0;
            kf->R[i][l] = 0.0;
        }
    }
    // 输出GPS位置结果
    OutputRawGnssPos(pgnss, result);
    g_pdr->NoGnssCount = 0;
    return PDR_TRUE;
}

/**----------------------------------------------------------------------
* Function    : EkfGnssInsLocFusion
* Description : 采用EKF对GNSS和INS融合定位
* Date        : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void EkfGnssInsLocFusion(GNSS_t* pgnss, PDR_t* pdr, Classifer_t* sys, double yaw_bias,
    EKFPara_t* kf, LctFs_t* res) {
    double plla[3] = { 0 };
    double Ned[3] = { 0 };
    double yaw_thr = 6;

    int scene_type = pdr->sceneType;

    plla[0] = pgnss->lat;
    plla[1] = pgnss->lon;
    plla[2] = 0;

    WGS842NED(plla, pdr->PllaInit, Ned);

    pgnss->xNed = Ned[0];
    pgnss->yNed = Ned[1];

    //调整融合策略
    EKFStateUpdate(kf, pgnss, sys, pdr, scene_type);

    for (uchar i = 0; i < N; i++) {
        for (uchar l = 0; l < N; l++) {
            kf->pPk[i][l] = kf->Pk[i][l];
        }
        kf->pXk[i] = kf->Xk[i];
    }

    Ned[0] = kf->pXk[0] - pdr->x0;
    Ned[1] = kf->pXk[1] - pdr->y0;
    Ned[2] = 0;

    NED2WGS84(pdr->PllaInit, Ned, plla);

    res->latitude = plla[0];
    res->latitude_ns = pgnss->lat_ns;
    res->longitudinal = plla[1];
    res->longitudinal_ew = pgnss->lon_ew;
    res->time = pgnss->timestamps;

    pdr->NoGnssCount = 0;
}

/**----------------------------------------------------------------------
* Function    : PDRInitialAlignment
* Description : PDR初始对准
* Date        : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
int GnssInsInitialAlignment(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result)
{
    double stepLen = 0.7;      // 初始运动步长为0.7m
    if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.5 && pgnss->yaw != -1) {
        // plla 类似位置坐标的原点(单位是经纬度)

        g_pdr->PllaInit[0] = pgnss->lat;
        g_pdr->PllaInit[1] = pgnss->lon;
        g_pdr->PllaInit[2] = 0;

        // plla 转 ned
        double ned[3] = { 0 };
        WGS842NED(g_pdr->PllaInit, g_pdr->PllaInit, ned);

        // 在NED坐标系的计算, 单位是米
        g_pdr->x0 = ned[0];
        g_pdr->y0 = ned[1];

        // 初始化卡尔曼滤波器的观测量
        kf->pXk[0] = ned[0];                        // 状态量1: 北向x
        kf->pXk[1] = ned[1];                        // 状态量2：东向y
        kf->pXk[2] = stepLen;                       // 状态量3：步长
        kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG;   // 状态量4：航向角

        for (uchar i = 0; i < N; i++) {
            kf->Xk[i] = kf->pXk[i];
        }

        kf->initHeading = kf->pXk[3];
        for (int i = 0; i < N; i++) {
            for (int l = 0; l < N; l++) {
                kf->Q[i][l] = 0.0;
                kf->R[i][l] = 0.0;
            }
        }
        OutputRawGnssPos(pgnss, result);
        g_pdr->sysStatus = IS_NORMAL;

        return PDR_TRUE;
    }

    if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) {
        OutputRawGnssPos(pgnss, result);
        return PDR_TRUE;
    }
    return PDR_FALSE;
}