/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : pdr_base.c
* Department         : Sensor Algorithm Team
* Current Version    : V1.2
* Author             : logzhan
* Date of Issued     : 2021.02.02
* Comments           : PDR基础功能函数
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <string.h>
#include <math.h>
#include "Location.h"
#include "Kalman.h"
#include "Utils.h"
#include "AHRS.h"
#include "pdr_detector.h"
#include "buffer.h"
#include "Filter.h"
#include "pdr_base.h"


/* Global Variable Definition ------------------------------------------------*/
PDR_t              Pdr;


PDR_t* Base_Init() 
{
    ResetPDRData();
    return &Pdr;
}

/**----------------------------------------------------------------------
* Function    : ResetPDRData
* Description : 重置PDR变量
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void ResetPDRData(void) {
    memset(&Pdr, 0, sizeof(Pdr));
    Pdr.status              = PDR_STATUS_RESET;
    Pdr.axis_ceofficient[0] = 0;
    Pdr.axis_ceofficient[1] = 1;
    Pdr.axis_ceofficient[2] = 0;
    Pdr.bias_direction[0]   = 1;
    Pdr.bias_direction[1]   = 0;
    Pdr.pca_accuracy        = -1;
    Pdr.bias_accuracy       = -1;
    // 初始坐标初始化为0
    Pdr.x0                  = 0;
    Pdr.y0                  = 0;
    // 初始步数为0             
    Pdr.steps               = 0;
    Pdr.lastSteps           = 0;
    Pdr.lastGpsSteps        = 0;
    Pdr.deltaStepsPerGps    = 0;
    Pdr.heading             = 0.0;
    Pdr.lastHeading         = 0.0;
    Pdr.deltaHeading        = 0.0;
    Pdr.ts                  = 0.0;
    Pdr.sysStatus           = IS_INITIAL;
    Pdr.NoGnssCount          = 0;
    Pdr.sceneType           = UN_OPEN_AREA;
    Pdr.fusionPdrFlg        = PDR_FALSE;
    Pdr.gpsHeading          = 0;
    Pdr.trackHeading        = ITEM_INVALID;
    Pdr.motionFreq          = 0.1f;
    Pdr.insHeadingOffset    = 0.0f;
    Pdr.gyroTime            = 0.0;

    memset(Pdr.pllaInit, 0, sizeof(Pdr.pllaInit));
}

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

/**----------------------------------------------------------------------
* Function    : DetUserStatic
* Description : 检测用户是否处于静止状态, 目前主要采用计步器判断和运动频率分析
*               单运动频率计算量太大，可以考虑用加速度判断用户状态。
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
int DetUserStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps) {

    if (pgnss->vel != -1 && pgnss->vel < 1 && (delSteps == 0 &&
        g_pdr->motionFreq == 0.0)) {
        return PDR_TRUE;
    }
    return PDR_FALSE;
}

/**----------------------------------------------------------------------
* Function    : DetectCarMode
* Description : 识别是否是车载模式
* Input       : pgnss，GPS数据结构体
                g_pdr，PDR结构体
                delSteps， 与上次相比步数的变化量
                time，计数器
* Output      : int，车载模式标志位
* Date        : 2021/01/28 logzhan
*---------------------------------------------------------------------**/
int DetectCarMode(GNSS_t* gnss, PDR_t* pdr, unsigned long delSteps, int* time)
{
    if ((gnss->vel > 10 && delSteps == 0) ||
        (delSteps == 0 && pdr->motionFreq == 0.0 && gnss->vel > 3 && gnss->error < 30)) 
    {
        (*time)++;
    }else {
        *time = 0;
    }
    if (*time > 5) {
        return PDR_TRUE;
    }
    return PDR_FALSE;
}

/**----------------------------------------------------------------------
* Function    : DetectPdrToReset
* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况
*               重置PDR到GPS的位置。
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
int DetectPdrToReset(double pdr_angle, int* gpsCnt, ulong deltsteps, PDR_t* pdr) {

    if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (pdr->motionFreq == 0))) {
        (*gpsCnt)++;
    }else {
        *gpsCnt = 0;
    }
    return (*gpsCnt > 6);
}

/**---------------------------------------------------------------------
* Function    : DetectorUpdate
* Description : 根据新确定的手机携带方式更新状态等
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void DetectorUpdate(Detector_t *Detector) {
    // 设置PDR的运动类型
    Pdr.MotionType = Detector->type;
}
