/********************************************************************/
/* FILE NAME  : scene_recognition.c                                 */
/* AUTHOR     : ZhangJingrui, ID: 11082826                          */
/* VERSION    : V0.85_Beta (Doing)                                  */
/* DESCRIPTION: The definations of functions in scene_recognition.h */
/********************************************************************/

#include "scene_recognition.h"
#include <stdio.h>
#include <math.h>   /* fabs() */
#include <stdlib.h> /* malloc(), free() */
/* @Lai Zhilong added below code*/
#if !defined(LOCATION_PLATFORM_QCOM_MODEM)  /* The Macro is in "custom.h" */
    #include <malloc.h>
    /* @Zhang Jingrui: The <malloc.h> header is depercated (and quite Linux specific, */
    /* on which it defines non-standard functions like mallinfo(3)). Notice that the  */
    /* header <stdlib.h> is defined by C89 (and later) standards, but not <malloc.h>. */
#endif 

#include <string.h> /* memset() */
#include <float.h>  /* FLT_EPSILON */

/* Macro Defination----------------------------------------------*/
/*===============================================================*/

 /* Compatible with the value in header file "sensor.h" */
#define _CONSTELLATION_USED_CAPACITY    12      /* Size of array defined as lct_nmea_gsa::prn.             */
#define _CONSTELLATION_VISIBLE_CAPACITY 20      /* Size of array defined as lct_nmea_gsv::satellites_data. */

/* Satellite PRN index in _GnssInfo list.       */
/* These index are planed with _GNSS_INDEX_LEN. */
/* GP: index in _GnssInfo's list is 0~31, and the PRN index is 1~32    */
#define _GP_INDEX_L                     0
#define _GP_INDEX_R                     31
#define _GP_INDEX_OFFSET                ( _GP_INDEX_L )
#define _GP_SVID_OFFSET                 1
/* GA: index in _GnssInfo's list is 32~67, and the PRN index is 1~36   */
#define _GA_INDEX_L                     32
#define _GA_INDEX_R                     67
#define _GA_INDEX_OFFSET                ( _GA_INDEX_L )
#define _GA_SVID_OFFSET                 1
/* GL: index in _GnssInfo's list is 68~102, and the PRN index is 65~99 */
#define _GL_INDEX_L                     68
#define _GL_INDEX_R                     102
#define _GL_INDEX_OFFSET                ( _GL_INDEX_L )
#define _GL_SVID_OFFSET                 65


/* Global Variable-----------------------------------------------*/
/*===============================================================*/


/* g_gnss_info_buffer: the newnest value is in the 0th sub-stucture. */
static GnssInfo*               g_gnss_info_buffer               = NULL; 
static const int               g_gnss_info_buffer_size          = _TIME_BUFFER_SIZE;
/* g_gnss_info_buffer_trail: 0 ~ gnss_info_buffer_size */
static int                     g_gnss_info_buffer_trail         = 0;   
static _SCENE_MODEL_WORK_STATE g_model_state                    = MODEL_OFF;


/* static function Declaration-----------------------------------*/
/*===============================================================*/

/* Return @ _is_occupied: 0 -- non resource occupied.                                */
static int             check_resources_occupied(void);             

/* Return @ _is_failed: 0 -- process of free res is successful.                      */
static int             free_resources          (void);             

/* Return @ _is_failed: 0 -- process of allocating res is successful.                */
static int             alloc_resources         (void);              

/* Function Name: static _lctnmea_to_gnssinfo()                                      */
/* Usage: Transform from the updated lct_nmea structure to the _GnssInfo structure.  */
/*        The defination of lct_nmea is in <sensors.h>.                              */
/* Param  @ _dst            : The pointer to the target _GnssInfo structure variable.*/
/* Param  @ _src_nmea       : The pointer to the source lct_nmea structure varible.  */
/* Return @ _parse_res_state: The transformation process result.
                  0--success;   
                 -1--NULL param inputed; 
                  1--lct_nmea _src_nmea is not valiable beacause of the update flag. */
static int             lctnmea2Gnssinfo     (GnssInfo *, const lct_nmea *);

/* Function name: static _detect_good_signal()                                      */
/* Usage: judge the quality of positioning result between good and bad.             */
/* Param  @ _src: The pointer to the _GnssInfo structure in the positioning epoch.  */
/*          The value in the structure should be avaliable.                         */
/* Return @ _process_res: the quality of positioning result.                        */
/*          _SIGNAL_QUALITY::GOOD -- the signal is good.                            */
/*          _SIGNAL_QUALITY::BAD  -- the signal is not good.                        */
static SIGNAL_QUALITY detGnssSignal      (const GnssInfo *); 

/* Function name: static _gnss_info_st_cpy()                                        */
/* Usage: Deep copy from one _GnssInfo to another one.                              */
/*        The rule of copy operation:                                               */
/*        _dst   |   _src   | Operation behavior                                    */
/*        --------------------------------------                                    */
/*        NULL   | variable | Nothing done.                                         */
/*        NULL   | NULL     | Nothing done.                                         */
/*        val A  | NULL     | Set the value of A to the initialization value.       */
/*        val A  | val B    | Deep copy B to A.                                     */
/*        Val A  | val A    | Nothing done.                                         */
/* Param  @ _src: Point to the source _GnssInfo structure.                          */
/* Param  @ _dst: Point to the target _GnssInfo structure .                         */
/* Return @ (void)                                                                  */
static void            gnss_info_st_cpy        (GnssInfo *_dst, const GnssInfo *_src);


/* external function defination ---------------------------------*/
/*===============================================================*/

SCENE_INIT_STATE initSceneRecognition(void)
{
    SCENE_INIT_STATE res          = INIT_NOT_PERFORMED;
    int              _is_occupied = -1;
    int              is_failed    =  0;
    /* Check the legality of initialization process. */
    if (g_model_state == MODEL_ON)
    {
        return INIT_ERROR_REDUNDANCY;   /* 重复初始化 */
    }

    /* Initialization of resources */
    g_model_state = MODEL_OFF;
    _is_occupied  = check_resources_occupied();
    if (_is_occupied)
    {
        free_resources();
    }
    is_failed    = alloc_resources();
    if (is_failed)
    {
        res  = INIT_ERROR_MEMORY_INIT_FAILED;
    }else{
        res  = INIT_SUCCESS;
        g_model_state = MODEL_ON;
    }
    return res;
}


SCENE_INIT_STATE scene_recognition_reset(void)
{
    SCENE_INIT_STATE _process_res = INIT_RESET_FAILED;
    int              _is_occupied = 0;
    int              _is_failed   = 0;
    if (g_model_state == MODEL_OFF)
    {
        _process_res = INIT_RESET_FAILED;
    }
    else
    {
        _is_occupied  = check_resources_occupied();
        if (_is_occupied)
        {
            free_resources();
        }
        g_model_state = MODEL_OFF;
        _is_failed    = alloc_resources();
        if (_is_failed)
        {
            _process_res  = INIT_RESET_FAILED;
            g_model_state = MODEL_OFF;
        }
        else
        {
            _process_res  = INIT_RESET_SUCCESS;
            g_model_state = MODEL_ON;
        }
    }
    return _process_res;
}

/**----------------------------------------------------------------------
* Function    : sceneRecognitionProc
* Description : GNSS场景识别处理流程：
*               1）将nmea转换为gnss
*               2) 通过accuracy、snr、卫星数量分析GNSS信号质量
*               3) 返回结果
* Input       : PDR的nmea结构体
* Return      : 场景识别结果
* Author      : Zhang Jingrui, VIVO ID: 11082826
* Date        : 2020/02/18  yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
SCENE_RECOGNITION_RESULT sceneRecognitionProc(const lct_nmea *nmea)
{
    SCENE_RECOGNITION_RESULT res          = RECOG_UNKNOWN;
    int                      transRes     = 0;
    SIGNAL_QUALITY           detRes       = SIG_UNKNOWN;
    GnssInfo                 gnssInfo;
    // 将nmea转换为gnss
    transRes = lctnmea2Gnssinfo(&gnssInfo, nmea);
    switch (transRes)
    {
        case -1:
            res = RECOG_ERROR_PARAM_INCRECT; 
            break;
        case 1:
            res = RECOG_ERROR_NMEA_INFO_MISS; 
            break;
        case 0:
            // GNSS信号检测结果
            detRes  = detGnssSignal(&gnssInfo);
            if      (detRes == GOOD)  
            { res = RECOG_OPEN_AREA; }
            else if (detRes == BAD)   
            { res = RECOG_MULTIPATH; }
            else /*UNKOWN but is not exist*/
            { res = RECOG_UNKNOWN;   }
            break;
        default:
            res = RECOG_UNKNOWN;
    }
    return res;
}

/**----------------------------------------------------------------------
* Function    : isOpenArea
* Description : GNSS开阔地检测，通过输入GNSS信号，分析当前信号质量，从而确定
*               目标是否处于开阔场地。
* Return      : 0 : 非开阔地  1：开阔地(信号质量较好)
* Author      : Zhang Jingrui, VIVO ID: 11082826
* Date        : 2020/02/18  yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
int isOpenArea(const lct_nmea *nmea)
{
    SCENE_RECOGNITION_RESULT type = sceneRecognitionProc(nmea);
    // 如果是开阔地则返回1
    return (type == RECOG_OPEN_AREA);
}


SCENE_DESTROY_STATE scene_recognition_destroy(void)
{
    SCENE_DESTROY_STATE res = DESTROY_INVALID;
    if (g_model_state == MODEL_OFF){
        res   = DESTROY_INVALID;
    }else{
        // 释放资源
        free_resources();
        g_model_state  = MODEL_OFF;
        res   = DESTROY_SUCCESS;
    }
    return res;
    
}


/* static function defination -----------------------------------*/
/*===============================================================*/

static int check_resources_occupied(void)
{
    int res = 0;
    if (g_gnss_info_buffer != NULL){
        ++res;
    }
    return res;
}


static int free_resources(void)
{
    int res = 0;
    if (g_gnss_info_buffer != NULL)
    {
        free(g_gnss_info_buffer);
    }
    g_gnss_info_buffer       = NULL;    /* The pointer should be set NULL after free() for memory safety. */
    g_gnss_info_buffer_trail = 0;       /* 0 because of the memory of g_gnss_info_buffer is NULL now.     */
    return res;
}    


static int alloc_resources(void)
{
    int _process_res = 0;
    if (g_gnss_info_buffer != NULL)
    { 
        /* Check the buffer's occupied. This branch is generally because of the */
        /* functions flow call errors.                                          */
        _process_res = -1;
        return _process_res;
    }
    GnssInfo *ptr = (GnssInfo *)malloc(sizeof(GnssInfo) * g_gnss_info_buffer_size);
    if (ptr != NULL)
    {
        g_gnss_info_buffer       = ptr;
        g_gnss_info_buffer_trail = 0;
        _process_res             = 0;
    }
    else
    {
        /* Failed to apply for memory.*/
        _process_res = 1;
    }
	return _process_res;
}


static int lctnmea2Gnssinfo(GnssInfo *_dst, const lct_nmea * _src_nmea)
{
    int _parse_res_state = 0;
    /* Temporarily deprecated. */
    /* int _gsa_used_prn[3][_CONSTELLATION_USED_CAPACITY] = {0}; */ /* GPGSA, GLGSA, GAGSA */
    
    int _i_first        = 0;
    int _i_second       = 0;
    int _offset[6]      = { _GP_SVID_OFFSET, _GP_INDEX_OFFSET,
                            _GL_SVID_OFFSET, _GL_INDEX_OFFSET, 
                            _GA_SVID_OFFSET, _GA_INDEX_OFFSET };
    int _index          = -1;
    /* Detecting the NULL pamrameters inputed.       */
    if (_src_nmea == NULL || _dst == NULL)
    {
        _parse_res_state = -1;
    }
    /* Detecting the _src_nmea if it wasn't updated. */
    else if (_src_nmea->update == 0)
    {
        _parse_res_state = 1;
    }
    /*****************************************************************/
    /* DELETE: abandon this branch for the stability of the program. */
    /* Detecting the incorrect accuracy value.       */
    /*else if ((_src_nmea->accuracy.update == 0) || 
            (_src_nmea->accuracy.update ==  0) )
    {
        _parse_res_state = 2;
    }*/
    /****************************************************************/
    /* update the the _GnssInfo.                    */
    else
    {
        /* initialize the memory space pointed by _dst to all 0. */
        gnss_info_st_cpy(_dst, NULL);  

        /* pick the raw accuracy from lct_nmea::loc_accuracy. Therefore _dst->accuracy
         could be equal with ITEM_INVALID. */
        _dst->accuracy = _src_nmea->accuracy.err;
        
        /* pick the aviable satellites PRN from GxGSA and match the SNR value in 
        GxGSV where Gx is the presented to GP, GA or GL.                          */
        for (_i_first = 0 ; _i_first < 3 ; ++ _i_first) /* 0 - GP, 1 - GL, 2 - GA */
        {
            /* make sure the GxGSA and GxGSV has been update in the epoch */
            if (_src_nmea->gsa[_i_first].update == 0 || _src_nmea->gsv[_i_first].update == 0)
            {
                continue;
            }
            /* copy all the PRN number in GSA to the _dst pointed _GnssInfo */
            for (_i_second = 0 ; 
                _i_second < _CONSTELLATION_USED_CAPACITY     && 
                _src_nmea->gsa[_i_first].prn[_i_second] != 0 && 
                _src_nmea->gsa[_i_first].prn[_i_second] != ITEM_INVALID ; 
                ++ _i_second)
            {
                _index = _src_nmea->gsa[_i_first].prn[_i_second]
                         - _offset[_i_first*2] + _offset[_i_first*2 + 1];
                if (_index > 0)
                {
                    _dst->sat_used_list[_index] = 1;
                }
            }
            /* copy all the SNR values in GSV to the _dst pointed _GnssInfo */
            for (_i_second = 0 ;
                 (_i_second < _CONSTELLATION_VISIBLE_CAPACITY) &&
                 _src_nmea->gsv[_i_first].satellites_data[_i_second].snr != ITEM_INVALID ;
                ++ _i_second)
            {
                _index = _src_nmea->gsv[_i_first].satellites_data[_i_second].prn
                         - _offset[_i_first*2] + _offset[_i_first*2 + 1];
                if (_index > 0)
                {
                    _dst->snr_list[_index] = (float)(_src_nmea->gsv[_i_first].satellites_data[_i_second].snr);
                }
            }
        }
        _dst->update     = 1;
        _parse_res_state = 0;
    }
    return _parse_res_state;
}


/**---------------------------------------------------------------------
* Function    : detGnssSignal
* Description : 检测GPS信号是否良好，原理：
*               1）当Accuracy在0到5之间，那么是较好的信号
*               2）如果Accuracy在5到10之间，那么利用卫星总数和使用数量判断
*               GPS质量好坏
*               3）如果Accuracy在10以外，那么说明GPS信号不好
* Date        : 2020/02/18 yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
static SIGNAL_QUALITY detGnssSignal(const GnssInfo* _src)
{
    SIGNAL_QUALITY procRes          = SIG_UNKNOWN;
    int             i               = 0;
    float           accuracy        = 0;
    float           snr_large_rate  = 0;
    int             sat_used_num    = 0;           // 卫星使用数量
    /* Simulating a Queue data structure which can be optimazated in the future. */
    /* move all the elements to the right one memory and abandon the rightmost one.*/
    for (i = 0 ; i < g_gnss_info_buffer_size - 1 ; ++ i)
    {
        gnss_info_st_cpy((g_gnss_info_buffer + i + 1) , (g_gnss_info_buffer + i) );
    }
    /* Enqueue where the new element struct is put in the leftmost gpsFusionLocation of the array. */
    gnss_info_st_cpy((g_gnss_info_buffer), _src);
    if (g_gnss_info_buffer_trail < g_gnss_info_buffer_size) 
    { 
        ++ g_gnss_info_buffer_trail; 
    }

    /* make sure _src->accuracy is available otherwise the quality of signal is not correct. */
    if (fabs(_src->accuracy - ITEM_INVALID) < FLT_EPSILON )
    {
        procRes = SIG_UNKNOWN;
        return procRes;
    }

    /* features extraction */
    for (i = 0 ; i < _GNSS_INDEX_LEN ; ++ i)
    {
        if (_src->sat_used_list[i] == 1 && _src->snr_list[i] > 20)
        {
            ++ snr_large_rate;
        }
    }
    sat_used_num   = _src->sat_visible_number;
    accuracy       = _src->accuracy;
    snr_large_rate = sat_used_num > 0 ? (snr_large_rate / sat_used_num) : 0 ;
    /* classification process BEGIN */
    if (accuracy > 0 && accuracy < (5 + FLT_EPSILON) )
    { procRes = GOOD; }
    else if (accuracy > (10 + FLT_EPSILON) )
    { procRes = BAD; }
    else
    {
        /* 5 <= _accuracy <= 10 */
        if (snr_large_rate < 0.878676)
        {
            if (snr_large_rate < 0.781746)
            { procRes = BAD; }
            else
            {
                if (sat_used_num < 18.5)
                { procRes = BAD; }
                else
                { procRes = GOOD; }
            }
        }else{
            if (accuracy > 0 && accuracy < 9.2)
            { procRes = GOOD; }
            else
            { procRes = BAD; }
        }
    }
    /* classification process END */
    return procRes;
}


static void gnss_info_st_cpy(GnssInfo *_dst, const GnssInfo *_src)
{
    if (_dst != NULL && _src == NULL)
    {
        /* initialize _dst if _src is a empty pointer. */
        _dst->update              = 0;
        _dst->local_timestamp     = 0;
        _dst->accuracy            = 0;
        _dst->sat_visible_number  = 0;
        memset(_dst->snr_list,      0, sizeof(_dst->snr_list)      );
        memset(_dst->sat_used_list, 0, sizeof(_dst->sat_used_list) );
    }
    else if (_dst != NULL && _src != NULL && _dst != _src)
    {
        /* deep copy from _src to _dst if both of them isn't empty and not equal. */
        _dst->update              = _src->update;
        _dst->local_timestamp     = _src->local_timestamp;
        _dst->accuracy            = _src->accuracy;
        _dst->sat_visible_number  = _src->sat_visible_number;
        memcpy(_dst->snr_list,      _src->snr_list,      sizeof(_src->snr_list)      );
        memcpy(_dst->sat_used_list, _src->sat_used_list, sizeof(_src->sat_used_list) );
    }
    else
    {
        /* _dst and _src are both NULL pointer, then PASS.               */
        /* _dst is NULL pointer, then PASS.                              */
        /* _dst and _src are both pointer to the same memory, then PASS. */
    }
}
