/******************** (C) COPYRIGHT 2020 VIVO************************************
* File Name          : pdr_location.c
* Department         : Sensor Algorithm Team
* Current Version    : V1.2
* Author             : yuanlin_rjyb@vivo.com
* 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 "pdr_location.h"
#include "pdr_kalman.h"
#include "steps.h"
#include "pdr_ahrs.h"
#include "pdr_detector.h"
#include "pdr_util.h"
#include "scene_recognition.h"

/* Global Variable Definition ------------------------------------------------*/
PDR*            pdr;                                  // PDR全局信息
StepPredict     stepPredict = { 0 };
classifer       sys;
gnss            pgnss;                                // 定位相关信息
KfPara          g_kfPara;
double          GpsPos[HIST_GPS_NUM][3] = {{0}};
static uint32_t g_status;
extern AHRS     g_ahrs;

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

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

/**---------------------------------------------------------------------
* Function    : pdr_navSysInit
* Description : PDR导航系统初始化
* Date        : 2020/2/1 yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
void pdr_navSysInit(void) {

	memset(&g_kfPara,    0, sizeof(g_kfPara));
	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定位初始化
    pdr_initGnssInfo();
    // 初始化AHRS姿态解算相关
    pdr_initAhrs();
    // 检测器初始化
    pdr_initDetector();
    // PDR初始化
    pdr = pdr_initBase();
	//base文件中姿态角计算初始化
	pdr_initKalman();
    // 场景识别模块初始化
    initSceneRecognition();

	yaw_bias = -1;
}

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

/**---------------------------------------------------------------------
* Function    : pdr_resetSysStatus
* Description : PDR 重设系统状态
* Date        : 2020/2/1 yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
void pdr_resetSysStatus(KfPara *kf) 
{
    memset(kf, 0, sizeof(KfPara));
    pdr->fusionPdrFlg = PDR_FALSE;
    pdr->sysStatus    = IS_INITIAL;
    pgnss.overVelCount = 0;
}

/**---------------------------------------------------------------------
* Function    : pdr_insLocation
* Description : PDR 惯性导航定位
* Date        : 2020/2/1 yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
int pdr_insLocation(imu* ss_data, KfPara* kf) {

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

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

    // 计步器输入传感器数据，占10%的运行时间
    updatePedometer(ss_data, &pdr->steps);
    printf("step = %d\n", pdr->steps);

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

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

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

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

    pdr->heading = calPredAngle(pdr, &sys, pdr->gpsHeading, pdr->trackHeading, ss_data);
    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 = - g_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 惯性位置预测
    pdr_insPredict(pdr, &stepPredict, ss_data, &pgnss, kf);

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

/**----------------------------------------------------------------------
* Function    : pdr_gnssInsLocation
* Description : PDR GPS融合INS惯性定位
* Date        : 2021/01/29 yuanlin_rjyb@vivo.com & zhanli@vivo.com 
*---------------------------------------------------------------------**/
int pdr_gnssInsLocation(lct_nmea *nmea_data, KfPara *kf, lct_fs *result)
{
    // GPS数据更新
    pdr_gnssUpdate(&pgnss, nmea_data);
    // 检查时间差是否满足条件
    if (fabs(pgnss.timestamps - pdr->ts) >= 1000)return TYPE_FIX_NONE;
	// 根据nmea数据识别场景是否是开阔天空场景
	pdr->sceneType = isOpenArea(nmea_data);
    // 根据HDOP、速度、卫星数量等判断gps yaw是否可用
   
    pdr->gpsSpeed = pgnss.vel * GPS_SPEED_UNIT;
    pdr->lastGpsHeading = pdr->gpsHeading;
    pdr->gpsHeading = pdr_getGpsHeading(&pgnss);
    
    // 如果在win32平台下仿真调试,则添加下面代码
#ifdef _WIN32
    result->pdrHeading = kf->p_xk[3];
    result->gpsHeading = pdr->gpsHeading;
    result->hdop       = pgnss.HDOP;
    result->accuracy   = nmea_data->accuracy.err;
    result->gpsSpeed   = pdr->gpsSpeed;
    result->motionType = pdr->motionType;
   // printf("utc time = %f\n", nmea_data->gga.gga_utc);
#endif

    calPdrHeadingOffset(nmea_data, pdr);
    
    // 计算两次GPS时间之间，步数变化量
    pdr->deltaStepsPerGps = pdr->steps - pdr->lastGpsSteps;
    pdr->lastGpsSteps = pdr->steps;

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

	//根据历史GPS拟合输出预估的GPS轨迹航向角
    pdr->trackHeading = calGnssTrackHeading(GpsPos, pgnss);
	// 如果系统没有初始化，则先初始化
    if (pdr->sysStatus == IS_INITIAL) {
        return pdr_initProc(&pgnss, kf, pdr, result);
    }
    // 如果惯导没有达到输出条件，则不预测轨迹
    if (!pdr->fusionPdrFlg)return resetOutputResult(&pgnss, pdr, kf, result);
    // 根据GPS信息, 计算kf的过程噪声和观测噪声
    calEkfQRMatrix(nmea_data, 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->p_xk[3];
        }
        if (pgnss.yaw != INVAILD_GPS_YAW) {
            pdr_gnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result);
            return TYPE_FIX_PDR;
        }
    }
    
    // 不满足推算条件，直接输出原始GPS
    if (pgnss.satellites_num > 4 && nmea_data->accuracy.err > 0 &&
        nmea_data->accuracy.err < 15) {
        pdr_outputGpsPos(&pgnss, result);
        pdr->noGpsCount = 0;
        return TYPE_FIX_GPS;
    }

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


/**----------------------------------------------------------------------
* Function    : pdr_noGpsPredict
* Description : 在没有gps信息时预测GPS位置，最多预测10个点
* Date        : 2020/07/08 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_noGpsPredict(KfPara* kf, lct_fs* result, PDR* p_pdr)
{
    double ned[3] = {0}, lla[3] = {0};
    ned[0] = kf->p_xk[0] - p_pdr->x0;
    ned[1] = kf->p_xk[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->noGpsCount++;
}

/**----------------------------------------------------------------------
* Function    : calPdrHeadingOffset
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
* Date        : 2020/07/08 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void calPdrHeadingOffset(lct_nmea* nmea_data, PDR* 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)
        && (pdr_detStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->motionType == 2))
    {
        /*这里放姿态角与GPS对准part1 start*/
        double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]), 
            1 - 2 * (g_ahrs.qnb[2] * g_ahrs.qnb[2] + g_ahrs.qnb[3] * g_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    : pdr_detectFixMode
* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出
*               原始GPS或者不输出
* Date        : 2020/07/08 yuanlin_rjyb@vivo.com
*               2020/02/08 zhanli@vivo.com : 修改-1为INVAILD_GPS_YAW，提高
*               代码的可读性
*---------------------------------------------------------------------**/
int pdr_detectFixMode(gnss* pgnss, KfPara* kf, PDR* g_pdr, lct_fs* result) {
    // 检测用户是否处于静止状态
    if (pdr_detStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) {
        if (pgnss->error > 0 && pgnss->error < 15 &&
            pgnss->lastError >= pgnss->error) {
            pdr_outputGpsPos(pgnss, result);
            pgnss->lastError = pgnss->error;
            return TYPE_FIX_PDR;
        }
        // 否则不输出位置
        return TYPE_FIX_NONE;
    }
    // 如果是检测到是开车状态则输出GPS位置
    if (detIsCarMode(pgnss, pdr, pdr->deltaStepsPerGps, &pgnss->overVelCount)) {
        // 如果检测到车载模式，则重置PDR状态，需要重新初始化
        pdr_resetSysStatus(kf);
        if (pgnss->error >= 0 || (pgnss->satellites_num > 4
            && pgnss->HDOP < 2.0)) {
            pdr_outputGpsPos(pgnss, result);
            return TYPE_FIX_GPS;
        }
        // 精度过差，不输出位置
        return TYPE_FIX_NONE;
    }
    return TYPE_FIX_PDR;
}


/**----------------------------------------------------------------------
* Function    : pdr_getGpsHeading
* Description : 获取GPS偏航角(0 - 360°)
* Date        : 2020/07/08 yuanlin_rjyb@vivo.com
*               2020/02/08 zhanli@vivo.com : 修改-1为INVAILD_GPS_YAW，提高
*               代码的可读性
*---------------------------------------------------------------------**/
double pdr_getGpsHeading(gnss* pgnss) {
    double gpsYaw = INVAILD_GPS_YAW;
    if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.0
        && pgnss->yaw != INVAILD_GPS_YAW && pgnss->error < 50) {
        gpsYaw = pgnss->yaw * RADIAN_PER_DEG;
    }
    return gpsYaw;
}

/**----------------------------------------------------------------------
* Function    : pdr_nmea2Gnss
* Description : nmea数据结构转gnss数据
* Date        : 2020/07/08 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_nmea2Gnss(lct_nmea* nmea_data, gnss* pgnss) 
{
    lct_nmea_rmc 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    : pdr_gnssUpdate
* Description : nmea数据结构转gnss数据
* Date        : 2020/07/08 yuanlin_rjyb@vivo.com
*               2020/02/09 zhanli@vivo.com : 函数对应原pdr_gpsUpdate函数，
*               经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没
*               什么用，还增加运算量
*---------------------------------------------------------------------**/
void pdr_gnssUpdate(gnss* gps, lct_nmea* nmea) {
    // nmea数据转gnss数据结构
    pdr_nmea2Gnss(nmea, &pgnss);
    // GPS回调更新，暂时不清楚函数功能
    gpsUpdateCb(gps);
}
