/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : pdr_detector.c
* Department         : Sensor Algorithm Team
* Current Version    : V1.2
* Author             : ZhangJingrui@vivo.com
* Date of Issued     : 2020.7.4
* Comments           : PDR检测器，主要用于区分用户的运动类型，用于PDR算法进行下一步的
*                      决策。
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <string.h>
#include <math.h>
#include <stdio.h>
#include "PDRBase.h"
#include "Detector.h"
#include "Filter.h"
#include "buffer.h"
#include "Fft.h"
#include "Munkres.h"


/* Macro Definition ----------------------------------------------------------*/
#define FLT_BUF_LEN                         256
#define FRQ_BUF_LEN                         10
#define TMP_BUF_LEN                         5
#define FRQ_RAT                             0.3f
#define ACCE                                0
#define GYRO                                1
#define X_AXIS                              0
#define Y_AXIS                              1
#define Z_AXIS                              2
#define ARR_LEN(arr)                        (sizeof(arr) / sizeof(*arr))
#define GET_MOL(x)                          (sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]))
#define INC_PTR(buf, ptr)                   (((ptr) + 1) % ((buf)->length + 1))
#define DEC_PTR(buf, ptr)                   (((ptr) + (buf)->length) % ((buf)->length + 1))
#define COST(i, j)                          cost[(i) + (j) * number]
#define CALIBRATE_ON                        1
#define CALIBRATE_OFF                       0

extern int debugCount;


/* Global Variable Definition ------------------------------------------------*/
static Detector_t   pDetector;
static BUFFER_LONG  g_mol_buf[2];
static Filter_t     g_mol_flt[2];

const double        g_mol_flt_b[] = { 4.96135120696701e-05, 0.000496135120696701, 0.00223260804313515, 0.00595362144836041, 
                                      0.0104188375346307, 0.0125026050415569, 0.0104188375346307, 0.00595362144836041, 0.00223260804313515, 
                                      0.000496135120696701, 4.96135120696701e-05 };
const double        g_mol_flt_a[] = { 1, -3.98765436730188, 8.09440659266207, -10.4762753571126, 9.42333716215302, -6.08421408355183, 
                                      2.83526165426630, -0.936403462626094, 0.208912324670401, -0.0283358586690797, 0.00176963186900233 };
const double        g_mol_flt_h0 = 1;

       BUFFER_SHORT AccFrqBuf[3];
       BUFFER_SHORT AccAmpBuf[3];
static BUFFER_SHORT AccTmpBuf[3];
       BUFFER_SHORT GyrFrqBuf[3];
       BUFFER_SHORT GyrAmpBuf[3];
static BUFFER_SHORT GyrTmpBuf[3];

static int          g_label[5] = { 0 };

/* Function Definition -------------------------------------------------------------------------- */

/**---------------------------------------------------------------------
* Function    : GetDetectorObj
* Description : 获取PDR运动分类器对象
* Date        : 2020/02/16 logzhan 
*---------------------------------------------------------------------**/
Detector_t* GetDetectorObj(void) {
    return &pDetector;
}

/**---------------------------------------------------------------------
* Function    : Detector_Init
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
* Date        : 2020/02/16 logzhan & logzhan
*---------------------------------------------------------------------**/
Detector_t* Detector_Init(void) {
    DetectorReset();
    return &pDetector;
}

/**---------------------------------------------------------------------
* Function    : DetectorReset
* Description : 重置PDR运动分类器
* Date        : 2022/09/23 logzhan 
*---------------------------------------------------------------------**/
void DetectorReset(void) {

    pDetector.tick = 0;
    pDetector.lastType = DETECTOR_TYPE_STATIC;

    BufferInit((Buffer_t *)&g_mol_buf[ACCE], "acce_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN);
    BufferInit((Buffer_t *)&g_mol_buf[GYRO], "gyro_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN);

    BufferInit((Buffer_t *)&AccFrqBuf[0], "acc_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
    BufferInit((Buffer_t *)&AccFrqBuf[1], "acc_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
    BufferInit((Buffer_t *)&AccFrqBuf[2], "acc_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);

    BufferInit((Buffer_t *)&AccAmpBuf[0], "acc_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
    BufferInit((Buffer_t *)&AccAmpBuf[1], "acc_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
    BufferInit((Buffer_t *)&AccAmpBuf[2], "acc_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);

    BufferInit((Buffer_t *)&AccTmpBuf[0], "acc_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
    BufferInit((Buffer_t *)&AccTmpBuf[1], "acc_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
    BufferInit((Buffer_t *)&AccTmpBuf[2], "acc_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);

    BufferInit((Buffer_t *)&GyrFrqBuf[0], "gyr_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
    BufferInit((Buffer_t *)&GyrFrqBuf[1], "gyr_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
    BufferInit((Buffer_t *)&GyrFrqBuf[2], "gyr_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);

    BufferInit((Buffer_t *)&GyrAmpBuf[0], "gyr_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
    BufferInit((Buffer_t *)&GyrAmpBuf[1], "gyr_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
    BufferInit((Buffer_t *)&GyrAmpBuf[2], "gyr_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);

    BufferInit((Buffer_t *)&GyrTmpBuf[0], "gyr_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
    BufferInit((Buffer_t *)&GyrTmpBuf[1], "gyr_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
    BufferInit((Buffer_t *)&GyrTmpBuf[2], "gyr_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
    FilterSetCoef(&g_mol_flt[ACCE], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0);
    FilterSetCoef(&g_mol_flt[GYRO], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0);


    memset((void *)g_label, 0, sizeof(g_label));
}


/**---------------------------------------------------------------------
* Function    : DetectorUpdateIMU
* Description : 更新运动类型检测器的imu信息，如果到达一定的时间间隔，则会检测
*               用户的运动类型
* Date        : 2022/09/23 
*---------------------------------------------------------------------**/
void DetectorUpdateIMU(IMU_t* imu) {

    BufferPush((Buffer_t *)&g_mol_buf[ACCE], (float)FilterFilter(&g_mol_flt[ACCE], GET_MOL(imu->acc.s)));
    BufferPush((Buffer_t *)&g_mol_buf[GYRO], (float)FilterFilter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s)));

    // g_tick_d 实际上是1.28s检测一次，不然检测的频率过高
    if ((pDetector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) {
        // 检测用户的运动类型
		int detectResult = DetectMotionType();

		if (detectResult == DETECTOR_NO_ERROR && pDetector.lastType != pDetector.type) {
            DetectorUpdate(&pDetector);
            pDetector.lastType = pDetector.type;
        }
    }
    ++pDetector.tick;
}


/**---------------------------------------------------------------------
* Function    : fft_buffer
* Description : 对acc数据做傅里叶变换，频率信息保存在fft中
* Date        : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static int fft_buffer(Complex_t *fft, Buffer_t *buffer) {
    static float s_signal[1024] = { 0 };

    float mean;
    int   index;
    int   i;

    BufferMean(buffer, &mean);
    for (i = 0, index = buffer->_bottom; index != buffer->_top; ++i, index = INC_PTR(buffer, index)) {
        s_signal[i] = buffer->data[index] - mean;  /*时序上255比0新*/
    }

	//序号i之后的长度里元素全部置为0
    memset(s_signal + i, 0, (ARR_LEN(s_signal) - i) * sizeof(*s_signal));

    if (0 == FFT_Fft(fft, s_signal, ARR_LEN(s_signal))) {
        return DETECTOR_OUT_OF_MEMORY;
    }
    return DETECTOR_NO_ERROR;
}

/**----------------------------------------------------------------------
* Function    : find_peaks
* Description : 获取前number个较大频点的频率frequency和幅度amplitude
* Date        : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static int find_peaks(float *frequency, float *amplitude, int number, Complex_t *fft, 
                      int length, float cut_off_frequency, float min_threshold) {
    float amp_2[2];
    int min_ind;
    int dir;
    int n;
    int i;
    int j;

    min_ind = 0;
    memset(frequency, 0, number * sizeof(*frequency));
    memset(amplitude, 0, number * sizeof(*amplitude));

    dir = 1;
    n = (int)(cut_off_frequency * length / IMU_SAMPLING_FREQUENCY);
    amp_2[1] = fft[0].real * fft[0].real + fft[0].image * fft[0].image;
    for (i = 1; i < n; ++i) {
        amp_2[0] = amp_2[1];
        amp_2[1] = fft[i].real * fft[i].real + fft[i].image * fft[i].image;
        if (1 == dir) {
            if (amp_2[1] < amp_2[0]) {
                if (amp_2[0] > amplitude[min_ind]) {
                    amplitude[min_ind] = amp_2[0];
                    frequency[min_ind] = (float)i;
                    min_ind = 0;
                    for (j = 1; j < number; ++j) {
                        if (amplitude[min_ind] > amplitude[j]) {
                            min_ind = j;
                        }
                    }
                }
                dir = -1;
            }
        }
        else {
            if (amp_2[1] > amp_2[0]) {
                dir = 1;
            }
        }
    }

    for (i = 0; i < number; ++i) {
        amplitude[i] = (float)sqrt(amplitude[i]);
        if (amplitude[i] < min_threshold) {
            frequency[i] = 0;
            amplitude[i] = 0;
        }
        else {
            frequency[i] *= (float)IMU_SAMPLING_FREQUENCY / length;
        }
    }

    return DETECTOR_NO_ERROR;
}


static void get_nonzero_weighted_mean(float *mean, BUFFER_SHORT *buffer, int length) {
    int weight_sum;
    int weight;
    int index;

    if (buffer->_bottom == buffer->_top) {
        *mean = 0;
        return;
    }

    *mean = 0;
    weight_sum = 0;
    index = buffer->_top;
    for (weight = length; weight > 0 && index != buffer->_bottom; --weight) {
        index = DEC_PTR(buffer, index);
        if (0 != buffer->data[index]) {
            *mean += weight * buffer->data[index];
            weight_sum += weight;
        }
    }
    if (0 != weight_sum) {
        *mean /= weight_sum;
    }
}

static void count_zeros(int *count, BUFFER_SHORT *buffer, int length) {
    int index;

    *count = 0;
    for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
        if (0 == buffer->data[index]) {
            ++*count;
        }
    }
}

static void copy_buffer(BUFFER_SHORT *dst, BUFFER_SHORT *src, int length) {
    int dst_cnt;
    int src_cnt;
    int dst_idx;
    int src_idx;

    BufferCount((Buffer_t *)dst, &dst_cnt);
    BufferCount((Buffer_t *)src, &src_cnt);
    length = length < dst_cnt ? length : dst_cnt;
    length = length < src_cnt ? length : src_cnt;

    dst_idx = (dst->_top + dst->length + 1 - length) % (dst->length + 1);
    src_idx = (src->_top + src->length + 1 - length) % (src->length + 1);
    while (dst_idx != dst->_top) {
        dst->data[dst_idx] = src->data[src_idx];
        dst_idx = INC_PTR(dst, dst_idx);
        src_idx = INC_PTR(src, src_idx);
    }
}

static void clear_buffer(BUFFER_SHORT *buffer) {
    int index;

    index = buffer->_bottom;
    while (index != buffer->_top) {
        buffer->data[index] = 0;
        index = INC_PTR(buffer, index);
    }
}

static int track_frequency(BUFFER_SHORT *frequency_buffer, BUFFER_SHORT *amplitude_buffer, BUFFER_SHORT *temp_buffer, 
    float *frequency, float *amplitude, int number) {
    static float mean[3];
    static float cost[3 * 3];
    static int   assignment[3];
	float distance;
	float prev;
    int i;
    int j;
    int transform_flag;
	int count;

    /* 0.detect argument */
    if (number > ARR_LEN(mean)) {
        return DETECTOR_OUT_OF_MEMORY;
    }

    /* 1.caculate main mean */
    for (i = 0; i < number; ++i) {
        get_nonzero_weighted_mean(&mean[i], &frequency_buffer[i], TMP_BUF_LEN);
    }

    /* 2.caculate main cost */
    for (i = 0; i < number; ++i) {
        for (j = 0; j < number; ++j) {
            distance = (float)fabs(mean[i] - frequency[j]);

            if (0 == mean[i] || 0 == frequency[j] || distance > FRQ_RAT) {
                COST(i, j) = 1e3;
            }
            else {
                COST(i, j) = distance;
            }
        }
    }

    /* 3.get main assignment */
    MUNKRES_get_assignment(assignment, cost, number, number);

    /* 4.push main frequency and main amplitude buffer */
    for (i = 0; i < number; ++i) {
        if (1e3 == COST(i, assignment[i])) {
            BufferPush((Buffer_t *)&frequency_buffer[i], 0);
            BufferPush((Buffer_t *)&amplitude_buffer[i], 0);
        }
        else {
            BufferPush((Buffer_t *)&frequency_buffer[i], frequency[assignment[i]]);
            BufferPush((Buffer_t *)&amplitude_buffer[i], amplitude[assignment[i]]);
            frequency[assignment[i]] = 0;
            amplitude[assignment[i]] = 0;
        }
    }

    /* 5.caculate temp mean */
    for (i = 0; i < number; ++i) {
        get_nonzero_weighted_mean(&mean[i], &temp_buffer[i], TMP_BUF_LEN);
    }

    /* 6.caculate temp cost */
    for (i = 0; i < number; ++i) {
        for (j = 0; j < number; ++j) {
            if (0 == frequency[j]) {
                COST(i, j) = 1e3;
            }
            else if (0 == mean[i]) {
                COST(i, j) = FRQ_RAT;
            }
            else {
                COST(i, j) = (float)fabs(mean[i] - frequency[j]);
            }
        }
    }

    /* 7.get main assignment */
    MUNKRES_get_assignment(assignment, cost, number, number);

    /* 8.push temp buffer */
    for (i = 0; i < number; ++i) {
        BufferPush((Buffer_t *)&temp_buffer[i], frequency[assignment[i]]);
    }

    /* 9.caculate temp mean */
    for (i = 0; i < number; ++i) {
        get_nonzero_weighted_mean(&mean[i], &temp_buffer[i], TMP_BUF_LEN);
    }

    /* 10.check temp buffer */
    transform_flag = 0;
    for (i = 0; i < number; ++i) {

        prev = temp_buffer[i].data[temp_buffer[i]._bottom];
        for (j = temp_buffer[i]._bottom; j != temp_buffer[i]._top; j = INC_PTR(&temp_buffer[i], j)) {
            if (0 == temp_buffer[i].data[j] || fabs(temp_buffer[i].data[j] - prev) > FRQ_RAT) {
                break;
            }
            prev = temp_buffer[i].data[j];
        }
        if (j == temp_buffer[i]._top) {
            frequency[i] = mean[i];
            transform_flag = 1;
        }
        else {
            frequency[i] = 0;
        }
    }

    if (transform_flag) {
        /* 11.caculate main mean */
        for (i = 0; i < number; ++i) {
            get_nonzero_weighted_mean(&mean[i], &frequency_buffer[i], TMP_BUF_LEN);
        }

        /* 12.caculate main cost */
        for (i = 0; i < number; ++i) {

            count_zeros(&count, &frequency_buffer[i], TMP_BUF_LEN);
            for (j = 0; j < number; ++j) {
                if (0 == frequency[j]) {
                    COST(i, j) = 1e3;
                }
                else if (0 == mean[i]) {
                    COST(i, j) = -5;
                }
                else if (count >= 3) {
                    // COST(i, j) = 3 - count;
                    COST(i, j) = (float)(-5 + fabs(mean[i] - frequency[j]));
                }
                else {
                    COST(i, j) = (float)fabs(mean[i] - frequency[j]);
                }
            }
        }

        /* 13.get main assignment */
        MUNKRES_get_assignment(assignment, cost, number, number);

        /* 14.push main frequency and main amplitude buffer */
        for (i = 0; i < number; ++i) {
            if (0 != frequency[assignment[i]]) {
                // Buffer_push(&frequency_buffer[i], frequency[assignment[i]]);
                // Buffer_push(&amplitude_buffer[i], 0);
                copy_buffer(&frequency_buffer[i], &temp_buffer[assignment[i]], TMP_BUF_LEN);
                clear_buffer(&temp_buffer[assignment[i]]);
            }
        }
    }

    return DETECTOR_NO_ERROR;
}

static void get_nonzero_mean(float *mean, BUFFER_SHORT *buffer, int length) {
    int count;
    int index;

    count  = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
    length = count < length ? count : length;
    *mean  = 0;
    count  = 0;
    for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
        if (0 != buffer->data[index]) {
            *mean += buffer->data[index];
            ++count;
        }
    }
    if (count >= 0.5 * length) {
        *mean /= count;
    }
}

static void get_nonzero_std(float *std, BUFFER_SHORT *buffer, int length) {
    float mean;
	float delta;
    int   count;
    int   index;

    count  = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
    length = count < length ? count : length;
    mean  = 0;
    count  = 0;
    for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
        if (0 != buffer->data[index]) {
            mean += buffer->data[index];
            ++count;
        }
    }
    if (0 != count) {
        mean /= count;
    }

    *std = 0;
    for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
        if (0 != buffer->data[index]) {
            delta = buffer->data[index] - mean;
            *std += delta * delta;
        }
    }
    if (count >= 0.5 * length) {
        *std = (float)sqrt(*std / count);
    }
    else {
        *std = -1;      // default;
    }
}

/**---------------------------------------------------------------------
* Function    : get_feature
* Description : 获取用于判断手机携带方式（静止、手持、摆手等）的特征
* Date        : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static void get_feature(float *feature, int length) {
    int i;
    int j;
	float temp;

    BufferMean((Buffer_t *)&g_mol_buf[ACCE], &feature[0]);
    BufferStd( (Buffer_t *)&g_mol_buf[ACCE], &feature[1]);
    BufferStd( (Buffer_t *)&g_mol_buf[GYRO], &feature[2]);
    get_nonzero_std( &feature[3] , &AccFrqBuf[0], length); if (feature[3]  < 0) feature[3] = 10;
    get_nonzero_std( &feature[4] , &AccFrqBuf[1], length); if (feature[4]  < 0) feature[4] = 10;
    get_nonzero_std( &feature[5] , &AccFrqBuf[2], length); if (feature[5]  < 0) feature[5] = 10;
    get_nonzero_mean(&feature[6] , &AccAmpBuf[0], length);
    get_nonzero_mean(&feature[7] , &AccAmpBuf[1], length);
    get_nonzero_mean(&feature[8] , &AccAmpBuf[2], length);
    get_nonzero_std( &feature[9] , &GyrFrqBuf[0], length); if (feature[9]  < 0) feature[9]  = 10;
    get_nonzero_std( &feature[10], &GyrFrqBuf[1], length); if (feature[10] < 0) feature[10] = 10;
    get_nonzero_std( &feature[11], &GyrFrqBuf[2], length); if (feature[11] < 0) feature[11] = 10;
    get_nonzero_mean(&feature[12], &GyrAmpBuf[0], length);
    get_nonzero_mean(&feature[13], &GyrAmpBuf[1], length);
    get_nonzero_mean(&feature[14], &GyrAmpBuf[2], length);

    /* sort by std - bubble sort */
    for (i = 0; i < 2; ++i) {
        for (j = 0; j < 2 - i; ++j) {
            if (feature[3 + j] > feature[3 + j + 1]) {

                temp               = feature[3 + j];
                feature[3 + j]     = feature[3 + j + 1];
                feature[3 + j + 1] = temp;
                temp               = feature[6 + j];
                feature[6 + j]     = feature[6 + j + 1];
                feature[6 + j + 1] = temp;
            }
        }
    }
    for (i = 0; i < 2; ++i) {
        for (j = 0; j < 2 - i; ++j) {
            if (feature[9 + j] > feature[9 + j + 1]) {

                temp                = feature[9 + j];
                feature[9 + j]      = feature[9 + j + 1];
                feature[9 + j + 1]  = temp;
                temp                = feature[12 + j];
                feature[12 + j]     = feature[12 + j + 1];
                feature[12 + j + 1] = temp;
            }
        }
    }
}

/**---------------------------------------------------------------------
* Function    : predict
* Description : 判断手机携带方式（静止、手持、摆手等）
* Date        : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
int DetectorPredict(float *feature) 
{
    if (feature[1] < 0.932921) {
        return DETECTOR_TYPE_STATIC;
    }else if ((feature[3] < 0.10) + (feature[4] < 0.10) + (feature[5] < 0.10) > 0) {
        if (feature[12] < 49.9795) {
            if (feature[0] < 10.7220) {
                return DETECTOR_TYPE_HANDHELD;
            }
            else {
                return DETECTOR_TYPE_SWINGING;
            }
        }
        else {
            if (feature[2] < 0.312002) {
                return DETECTOR_TYPE_HANDHELD;
            }
            else {
                return DETECTOR_TYPE_SWINGING;
            }
        }
    }
    return DETECTOR_TYPE_IRREGULAR;
}

/**---------------------------------------------------------------------
* Function    : debounce
* Description : 去抖动，判断之前length个label，根据较多的携带方式最终确定手
                机携带方式
* Date        : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static int debounce(int *label, int length) {
    int cnt[4];
    int def;
    int i;

    memset(cnt, 0, sizeof(cnt));
    for (i = 0; i < length; ++i) {
        ++cnt[label[i]];
    }

    def = 0;
    for (i = 0; i < ARR_LEN(cnt); ++i) {
        if (cnt[i] > length * 3 / 4) {
            return i;
        }
        if (cnt[i] > 0) {
            def = i;
        }
    }
    return def;
}

/**---------------------------------------------------------------------
* Function    : DetectMotionType
* Description : pdr运动类型检测器
* Date        : 2022/09/23 logzhan
*---------------------------------------------------------------------**/
int DetectMotionType() {
    static Complex_t s_fft[1024]   = { {0.0} };
    static float     s_feature[15] = { 0 };
    float frq[3];
    float amp[3];
    int ret;
    int i;

    // 分析加速度计
    if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (Buffer_t *)&g_mol_buf[ACCE]))) {
        return ret;
    }
    if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, 0))) {
        return ret;
    }
    if (DETECTOR_NO_ERROR != (ret = track_frequency(AccFrqBuf, AccAmpBuf, AccTmpBuf, frq, amp, 3))) {
        return ret;
    }

    // 分析陀螺仪
    if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (Buffer_t *)&g_mol_buf[GYRO]))) {
        return ret;
    }
    if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, (DETECTOR_TYPE_SWINGING == pDetector.type || 
        DETECTOR_TYPE_HANDHELD == pDetector.type) ? 20.0f : 100.0f))) {
        return ret;
    }
    if (DETECTOR_NO_ERROR != (ret = track_frequency(GyrFrqBuf, GyrAmpBuf, GyrTmpBuf, frq, amp, 3))) {
        return ret;
    }

    get_feature(s_feature, 5);

    pDetector.type = DetectorPredict(s_feature);

    /* debounce */
    for (i = 0; i < ARR_LEN(g_label) - 1; ++i) {
        g_label[i] = g_label[i + 1];
    }
    g_label[i] = pDetector.type;
    pDetector.type = debounce(g_label, ARR_LEN(g_label));

    return DETECTOR_NO_ERROR;
}
