/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : PDRBase.c
* Current Version    : V2.0
* Author             : logzhan
* Date of Issued     : 2022.10.15
* Comments           : PDR基础功能函数
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <string.h>
#include <math.h>
#include "Location.h"
#include "Kalman.h"
#include "Utils.h"
#include "AHRS.h"
#include "Detector.h"
#include "buffer.h"
#include "Filter.h"
#include "PDRBase.h"
#include "Fft.h"

/* Macro Definition ----------------------------------------------------------*/
#define DOWNSAMPLING_RATE                     4
#define BUF_LEN                               256
#define ARR_LEN(arr)                          (sizeof(arr) / sizeof(*arr))
#define INC_PTR(buf, ptr)                     (((ptr) + 1) % ((buf)->length + 1))
#define GET_MOL(v)                            ((float)sqrt((v)[0] * (v)[0] + (v)[1] * (v)[1]))
#define P_SET_BIT(msk)                        (Pdr.status |= PDR_STATUS_##msk)
#define P_CLR_BIT(msk)                        (Pdr.status &= ~PDR_STATUS_##msk)
#define P_TST_BIT(msk)                        (Pdr.status & PDR_STATUS_##msk)

/* Global Variable Definition ------------------------------------------------*/
PDR_t              Pdr;
static Detector_t* g_detector;
static uint64_t    g_tick_p;
static uint32_t    g_motion_type;
static uint32_t    g_axis_ceof_counter;
static BUFFER_LONG g_axis[9];
static BUFFER_LONG g_grav[2];
static BUFFER_LONG g_erro;
static BUFFER_LONG g_posi[4];


static int angle_count;
static double p_angle[3][BUF_LEN];
double angle_mean[3] = { 0 };

static Filter_t      g_grv_flt[2];

#if defined(__MATLAB_DBG)
static int         g_debug[2];
static float       axis_mean[9];
#endif

const  double      g_grv_flt_b[] = { 4.62344870721942e-10, 4.62344870721942e-09, 2.08055191824874e-08, 5.54813844866331e-08, 
                                     9.70924228516079e-08, 1.16510907421929e-07, 9.70924228516079e-08, 5.54813844866331e-08, 
                                     2.08055191824874e-08, 4.62344870721942e-09, 4.62344870721942e-10 };

const  double      g_grv_flt_a[] = {  1, -8.39368255078839, 31.8158646581919, -71.7026569821430, 106.381617694638, 
                                     -108.553825650279, 77.1444606240244, -37.6960546719428, 12.1197601392885, 
                                     -2.31493776310228, 0.199454975553812 };
const  double      g_grv_flt_h0  = 1;

PDR_t* Base_Init() {

    g_detector = GetDetectorObj();

    BufferInit((Buffer_t*)&g_axis[0], "x_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t *)&g_axis[1], "x_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t *)&g_axis[2], "x_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t *)&g_axis[3], "y_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t*)&g_axis[4], "y_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t*)&g_axis[5], "y_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t*)&g_axis[6], "z_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t*)&g_axis[7], "z_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t*)&g_axis[8], "z_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t*)&g_grav[0], "grav_0", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t*)&g_grav[1], "grav_1", BUFFER_TYPE_QUEUE, BUF_LEN);
    BufferInit((Buffer_t*)&g_erro, "erro", BUFFER_TYPE_QUEUE, 5 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE);
    BufferInit((Buffer_t*)&g_posi[0], "posi_0", BUFFER_TYPE_QUEUE, 60);
    BufferInit((Buffer_t*)&g_posi[1], "posi_1", BUFFER_TYPE_QUEUE, 60);
    BufferInit((Buffer_t*)&g_posi[2], "posi_2", BUFFER_TYPE_QUEUE, 60);
    BufferInit((Buffer_t*)&g_posi[3], "posi_3", BUFFER_TYPE_QUEUE, 60);

    FilterSetCoef(&g_grv_flt[0], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0);
    FilterSetCoef(&g_grv_flt[1], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0);

    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));

    g_tick_p = 0;
    g_motion_type = g_detector->type;
    g_axis_ceof_counter = 0;
    BufferClear((Buffer_t*)&g_axis[0]);
    BufferClear((Buffer_t*)&g_axis[1]);
    BufferClear((Buffer_t*)&g_axis[2]);
    BufferClear((Buffer_t*)&g_axis[3]);
    BufferClear((Buffer_t*)&g_axis[4]);
    BufferClear((Buffer_t *)&g_axis[5]);
    BufferClear((Buffer_t *)&g_axis[6]);
    BufferClear((Buffer_t *)&g_axis[7]);
    BufferClear((Buffer_t *)&g_axis[8]);
    BufferClear((Buffer_t *)&g_grav[0]);
    BufferClear((Buffer_t*)&g_grav[1]);
    BufferClear((Buffer_t*)&g_erro);
    BufferClear((Buffer_t*)&g_posi[0]);
    BufferClear((Buffer_t*)&g_posi[1]);
    BufferClear((Buffer_t*)&g_posi[2]);
    BufferClear((Buffer_t*)&g_posi[3]);
    FilterReset(&g_grv_flt[0]);
    FilterReset(&g_grv_flt[1]);

	angle_count = 0;
	memset(&p_angle, 0, sizeof(p_angle));
	memset(&angle_mean, 0, sizeof(angle_mean));
}


static float get_main_frequency(BUFFER_SHORT *frequency, BUFFER_SHORT *amplitude, int number) {
    float amp[3] = { {0.0f} };
    float frq;
    int count;
    int index;
    int i;

    if (number > 3) {
        return 0;
    }

    for (i = 0; i < number; ++i) {
        amp[i] = 0;
        count = 0;
        for (index = amplitude[i]._bottom; index != amplitude[i]._top; index = INC_PTR(&amplitude[i], index)) {
            if (0 != amplitude[i].data[index]) {
                amp[i] += amplitude[i].data[index];
                ++count;
            }
        }
        if (0 != count) {
            amp[i] /= count;
        }
    }
    i = amp[0] > amp[1] ? 0 : 1;
    i = amp[i] > amp[2] ? i : 2;
    frq = 0;
    count = 0;
    for (index = frequency[i]._bottom; index != frequency[i]._top; index = INC_PTR(&frequency[i], index)) {
        if (0 != frequency[i].data[index]) {
            frq += frequency[i].data[index];
            ++count;
        }
    }
    if (0 != count) {
        frq /= count;
    }

    return frq;
}

static int get_optimal_length_by_frequency(BUFFER_LONG *buffer, float frequency) {
    int temp;

    if (0 != frequency) {
        temp = (int)((int)(buffer->length / (IMU_SAMPLING_FREQUENCY / frequency)) * (IMU_SAMPLING_FREQUENCY / frequency));
        return 0 == temp ? buffer->length : temp;
    }
    else {
        return buffer->length;
    }
}

static void get_buffer_mean(float *mean, BUFFER_LONG *buffer, int length) {
    int count;
    int index;
    int i;

    count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
    length = count < length ? count : length;
    count = 0;
    memset(mean, 0, 3 * sizeof(*mean));
    for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
        for (i = 0; i < 3; ++i) {
            mean[i] += buffer[i].data[index];
        }
        ++count;
    }
    mean[0] /=  count;
    mean[1] /= -count;
    mean[2] /= -count;
}

static void get_pca_direction(float *direction, BUFFER_LONG *buffer, int length) {
    float buff_mean[2];
    float varX;
    float varY;
    float varXY;
    float lambda;
    int count;
    int index;
    int i;

    count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
    length = count < length ? count : length;
    memset(buff_mean, 0, sizeof(buff_mean));
    for (index = (buffer[0]._top + buffer[0].length + 1 - length) % (buffer[0].length + 1); index != buffer[0]._top; index = INC_PTR(&buffer[0], index)) {
        for (i = 0; i < 2; ++i) {
            buff_mean[i] += buffer[i].data[index];
        }
    }
    buff_mean[0] /= length;
    buff_mean[1] /= length;
    varX = 0;
    varY = 0;
    varXY = 0;
    for (index = (buffer[0]._top + buffer[0].length + 1 - length) % (buffer[0].length + 1); index != buffer[0]._top; index = INC_PTR(&buffer[0], index)) {
        float deltaX = buffer[0].data[index] - buff_mean[0];
        float deltaY = buffer[1].data[index] - buff_mean[1];
        varX += deltaX * deltaX;
        varY += deltaY * deltaY;
        varXY += deltaX * deltaY;
    }
    lambda = ((varX + varY) + (float)sqrt((varX - varY) * (varX - varY) + 4 * varXY * varXY)) * 0.5f;
    if (0 != varXY) {
        direction[0] = (lambda - varY) / varXY;
        direction[1] = InvSqrt(direction[0] * direction[0] + 1);
        direction[0] *= direction[1];
        direction[1] = -direction[1];
    }
    else {
        direction[0] = 0;
        direction[1] = 0;
    }
}

static void caculate_determinant(float *determinant, float A[][3], int row, int col) {
    int k = 0;
    float temp[4];
    for (uchar i = 0; i < 3; ++i) {
        if (row == i) continue;           
        for (uchar j = 0; j < 3; ++j) {
            if (col == j) continue;
            temp[k++] = A[i][j];
        }
    }
    *determinant = temp[0] * temp[3] - temp[1] * temp[2];
}

static void caculate_adjoint_matrix(float A_star[][3], float A[][3]) {
    int i;
    int j;

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            caculate_determinant(&A_star[i][j], A, j, i);
            if ((i + j) % 2) {
                A_star[i][j] = -A_star[i][j];
            }
        }
    }
}

static void get_ceof_by_pca(float *ceof, float *axis_mean, float *pca_direction) {
    float A[3][3];
    float A_star[3][3];
    float x[3];
    float axs_dir[2];
    float mold;
    int   dir;
    int   i;
    int   j;

    /* caculate adjoint matrix */
    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            A[i][j] = axis_mean[j * 3 + i];
        }
    }
    caculate_adjoint_matrix(A_star, A);

    /* caculate x vector (Ax = b) */
    for (i = 0; i < 3; ++i) {
        x[i] = A_star[i][0] * pca_direction[0] + A_star[i][1] * pca_direction[1];
    }
    mold = (float)sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
    if (0 == mold) {
        goto GET_CEOF_BY_PCA_ARITHMETIC_ERROR;
    }
    for (i = 0; i < 3; ++i) {
        x[i] /= mold;
    }

    for (i = 0; i < 2; ++i) {
        axs_dir[i] = A[i][0] * ceof[0] + A[i][1] * ceof[1] + A[i][2] * ceof[2];
    }
    dir = (axs_dir[0] * pca_direction[0] + axs_dir[1] * pca_direction[1] < 0) ? -1 : 1;

    for (i = 0; i < 3; ++i) {
        ceof[i] = dir * x[i];
    }

    return;

GET_CEOF_BY_PCA_ARITHMETIC_ERROR:
    ceof[0] = 0;
    ceof[1] = 1;
    ceof[2] = 0;
}

static void correct_axis_ceofficient(float ratio) {
    float axis_mean[9];
    float pca_direction[2];
    float axis_ceofficient[3];
    float mold;
    int length;
    int i;

    length = get_optimal_length_by_frequency(g_axis, Pdr.motionFreq);
    get_buffer_mean(&axis_mean[0], &g_axis[0], length);
    get_buffer_mean(&axis_mean[3], &g_axis[3], length);
    get_buffer_mean(&axis_mean[6], &g_axis[6], length);
    get_pca_direction(pca_direction, g_grav, length);
    memcpy((void *)axis_ceofficient, (void *)Pdr.axis_ceofficient, sizeof(Pdr.axis_ceofficient));
    get_ceof_by_pca(axis_ceofficient, axis_mean, pca_direction);
    for (i = 0; i < 3; ++i) {
        Pdr.axis_ceofficient[i] = (1 - ratio) * Pdr.axis_ceofficient[i] + ratio * axis_ceofficient[i];
    }
    mold = (float)sqrt(Pdr.axis_ceofficient[0] * Pdr.axis_ceofficient[0] + Pdr.axis_ceofficient[1] * Pdr.axis_ceofficient[1] + Pdr.axis_ceofficient[2] * Pdr.axis_ceofficient[2]);
    for (i = 0; i < 3; ++i) {
        Pdr.axis_ceofficient[i] /= mold;
    }
}

/**----------------------------------------------------------------------
* Function    : pdr_computePCA
* Description : 进入RESET_PHASE_1\STABLE阶段之后进行PCA计算
* Date        : 2020/7/21 logzhan
*---------------------------------------------------------------------**/
void ComputePCA(AHRS_t *ahrs) {
    float axis_mean[9];
    float freq;
    float mold;
    int length;
    int i;
	double attitude[3];

	double angle_sum[3] = { 0 };
	int angle_len;

	//RESET_1时只加ticker，之后直接返回
    /* filter bad data */
    if (AHRS_STATUS_RESET & ahrs->status) {
        ++g_tick_p;
        return;
    }
    /* caculate axis ceofficient */
	/*PDR_STATUS_CACULATING_AXIS_CEOFFICIENT 0x40*/
	if (P_TST_BIT(CACULATING_AXIS_CEOFFICIENT) && \
       (g_grav[0]._top - g_grav[0]._bottom + g_grav[0].length + 1) % (g_grav[0].length + 1) > (BUF_LEN >> 1)) {

        correct_axis_ceofficient(0.50f);
        P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT);
        P_SET_BIT(PCA_STABLE);

        g_erro._bottom = 0;
        for (g_erro._top = 0; g_erro._top < g_erro.length && g_erro._top < 2 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE; ++g_erro._top) {
            g_erro.data[g_erro._top] = (g_erro._top % 2) ? +0.4f : -0.4f;
        }
        BufferClear((Buffer_t*)&g_posi[0]);
        BufferClear((Buffer_t*)&g_posi[1]);
        BufferClear((Buffer_t*)&g_posi[2]);
        BufferClear((Buffer_t*)&g_posi[3]);
        Pdr.bias_direction[0] = 1;
        Pdr.bias_direction[1] = 0;
        Pdr.bias_accuracy = -1;
        //++flag;
        //}
    }

    /* correct axis ceofficient */
	if ((Pdr.status & PDR_STATUS_PCA_STABLE) && 0 == (g_tick_p % (2 * IMU_SAMPLING_FREQUENCY))) {
        correct_axis_ceofficient(0.10f);
    }

    /* downsample */
    if (0 != (g_tick_p % DOWNSAMPLING_RATE)) {
        ++g_tick_p;
        return;
    }

    /* push data */
    for (i = 0; i < 3; ++i) {
        BufferPush((Buffer_t*)&g_axis[0 + i], ahrs->x_axis[i]);
        BufferPush((Buffer_t*)&g_axis[3 + i], ahrs->y_axis[i]);
        BufferPush((Buffer_t*)&g_axis[6 + i], ahrs->z_axis[i]);
    }
    for (i = 0; i < 2; ++i) {
        //Buffer_push((BUFFER *)&g_grav[i], (float)FILTER_filter(&g_grv_flt[i], ahrs->gravity[i]));
        BufferPush((Buffer_t*)&g_grav[i], ahrs->gravity[i]);
    }

    /* motion frequency */
    freq = get_main_frequency(AccFrqBuf, AccAmpBuf, 3);
    if (3.5 >= freq && freq > 1.5) {
        Pdr.motionFreq = freq;
    }
    else if (1.5 >= freq && freq > 0.75){
        Pdr.motionFreq = 2 * freq;
    }

    /* axis direction */
    length = get_optimal_length_by_frequency(g_axis, Pdr.motionFreq);
    get_buffer_mean(&axis_mean[0], &g_axis[0], length);
    get_buffer_mean(&axis_mean[3], &g_axis[3], length);
    get_buffer_mean(&axis_mean[6], &g_axis[6], length);
    memset(Pdr.axis_direction, 0, sizeof(Pdr.axis_direction));
    for (i = 0; i < 2; ++i) {
        Pdr.axis_direction[i] = Pdr.axis_ceofficient[0] * axis_mean[0 + i] + Pdr.axis_ceofficient[1] * axis_mean[3 + i] + Pdr.axis_ceofficient[2] * axis_mean[6 + i];
    }
    mold = (float)GET_MOL(Pdr.axis_direction);
    if (0 != mold) {
        Pdr.axis_direction[0] /= mold;
        Pdr.axis_direction[1] /= mold;
    }

    /* pca direction */
    get_pca_direction(Pdr.pca_direction, g_grav, length);
    if (Pdr.pca_direction[0] * Pdr.axis_direction[0] + Pdr.pca_direction[1] * Pdr.axis_direction[1] < 0) {
        Pdr.pca_direction[0] = -Pdr.pca_direction[0];
        Pdr.pca_direction[1] = -Pdr.pca_direction[1];
    }

    /* direction accuracy */
	/*代码疑问1：g_erro为什么由下式确定，什么含义*/
    buffer_push((Buffer_t *)&g_erro, (float)fabs(Pdr.axis_direction[0] * Pdr.pca_direction[1] - Pdr.axis_direction[1] * Pdr.pca_direction[0]));
	if (!P_TST_BIT(CACULATING_AXIS_CEOFFICIENT)) {
        float accuracy;

        Buffer_std((Buffer_t*)&g_erro, &accuracy);
        if (DETECTOR_TYPE_HANDHELD == Pdr.MotionType) {
			if (!P_TST_BIT(STABLE) && !P_TST_BIT(CACULATING_AXIS_CEOFFICIENT) && 0.15 >= accuracy) {
                P_SET_BIT(CACULATING_AXIS_CEOFFICIENT);
            }
            else if (Pdr.pca_accuracy < 0.30 && 0.30 <= accuracy) {
                P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT);
                P_CLR_BIT(STABLE);
            }
        }
        Pdr.pca_accuracy = accuracy;
    }
    else {
        Pdr.pca_accuracy = -1;
    }

    /* calibrated direction */
    Pdr.cal_direction[0] = Pdr.pca_direction[0] * Pdr.bias_direction[0] + Pdr.pca_direction[1] * Pdr.bias_direction[1];
    Pdr.cal_direction[1] = Pdr.pca_direction[1] * Pdr.bias_direction[0] - Pdr.pca_direction[0] * Pdr.bias_direction[1];

    //PDR_LOGD("%f %f\n", Pdr.cal_direction[0],Pdr.cal_direction[1]);

    if (DETECTOR_TYPE_STATIC == g_detector->type || DETECTOR_TYPE_IRREGULAR == g_detector->type) {
        Pdr.motionFreq = 0;
    }

	Qnb2Att(ahrs->qnb,attitude);

	if (angle_count < BUF_LEN - 1)
	{
		p_angle[0][angle_count] = attitude[0];
		p_angle[1][angle_count] = attitude[1];
		p_angle[2][angle_count] = attitude[2];
		angle_count++;
	}
	else{
		for (int i = 0; i < BUF_LEN - 1; i++)
		{
			p_angle[0][i] = p_angle[0][i + 1];
			p_angle[1][i] = p_angle[1][i + 1];
			p_angle[2][i] = p_angle[2][i + 1];
		}
		p_angle[0][BUF_LEN - 1] = attitude[0];
		p_angle[1][BUF_LEN - 1] = attitude[1];
		p_angle[2][BUF_LEN - 1] = attitude[2];

		angle_len = (length <= BUF_LEN) ? length : BUF_LEN;
		for (int j = BUF_LEN - 1; j >(BUF_LEN - 1 - angle_len); j--)
		{
			angle_sum[0] = angle_sum[0] + p_angle[0][j];
			angle_sum[1] = angle_sum[1] + p_angle[1][j];
			angle_sum[2] = angle_sum[2] + p_angle[2][j];
		}
		angle_mean[0] = angle_sum[0] / angle_len;
		angle_mean[1] = angle_sum[1] / angle_len;
		angle_mean[2] = angle_sum[2] / angle_len;
	}

    ++g_tick_p;
}

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

    if (pgnss->vel != -1 && pgnss->vel < 1 && (delSteps == 0 &&
        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;

    if (DETECTOR_TYPE_SWINGING == Pdr.MotionType && DETECTOR_TYPE_SWINGING != g_motion_type) {
        if (!P_TST_BIT(STABLE)) {
            P_SET_BIT(CACULATING_AXIS_CEOFFICIENT);
            P_CLR_BIT(STABLE);
        }
    }
    else if ((DETECTOR_TYPE_STATIC == Pdr.MotionType || DETECTOR_TYPE_IRREGULAR == Pdr.MotionType) &&
        (DETECTOR_TYPE_HANDHELD == g_motion_type || DETECTOR_TYPE_SWINGING == g_motion_type)) {
        P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT);
        P_CLR_BIT(STABLE);
    }

    g_motion_type = Pdr.MotionType;
}
