// 秒寻科技
// 选择定位坐标
// zt
// 2023-03-30

#include "../../includes/includes.h"
#include "./lct_public.h"

// 定义全局变量
static uint32 gLct_dwUsePredictionCoordTimes = 0; // 连续使用预测坐标次数

// 导入全局变量
extern LCT_COORDSEL_PARAM_T gg_stLctCoordselParam;
extern PUB_SYS_STATUS_T gg_stSysStatus;                           // 全局系统状态
extern LCT_COORDSEL_ORIGINAL_COORD_T gLct_stOriginalLctCoordList; // 原始定位坐标队列
extern LCT_COORDSEL_MEAN_COORD_T gLct_stMeanLctCoordList;         // 历史定位点

// 模块初始化
void lct_coordsel_init(LCT_COORDSEL_PARAM_T stInitParam)
{
    // 模块初始化参数
    gg_stLctCoordselParam.dwValidDistanceThres = stInitParam.dwValidDistanceThres;

    return;
}

// 选择最优定位坐标
// pstMinimumValueGroup:极小值数据组
// byPredictionFlag:预测标识符，0：预测失败，1：预测成功
// stPredictionCoord:预测坐标
// pstOptimumCoord:最优坐标（返回参数）
void lct_coordsel_main(LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueGroup, uint8 byPredictionFlag, COORD_T stPredictionCoord, LCT_COORDSEL_OPTIMUM_COORD_T *pstOptimumCoord)
{
    // 1、预测坐标成功
    if (byPredictionFlag)
    {
        // 基于预测点坐标选择最优定位点坐标
        lct_coordsel_sel_optimum_coord_by_prediction_coord(pstMinimumValueGroup, stPredictionCoord, pstOptimumCoord);
    }
    // 2、系统处于初始化阶段，预测坐标失败
    else
    {
        lct_coordsel_sel_optimum_coord_by_minimum_value(pstMinimumValueGroup, pstOptimumCoord);
    }

    return;
}

// 过滤跳变坐标（检查极小值点于上一个定位坐标点之间的距离，距离超过合法门限的点判定为跳变点）
// pstMinimumValueGroup:极小值队列
void lct_coordsel_filter_jump_coord(LCT_COORDSEL_OPTIMUM_COORD_T *pstMinimumValueList, uint8 *pbyMinimumValueListLen, uint8 byPredictionFlag)
{
    uint8 byListLen = gLct_stMeanLctCoordList.byListLen; // 平滑队列长度
    COORD_T stAheadMeanCoord = {0};                      // 前面一点坐标
    COORD_T stTmpLctCoord = {0};

    int32 xx = 0;
    int32 yy = 0;
    flt32 fDistance = 0;
    flt32 fValidDistanceThres = (flt32)gg_stLctCoordselParam.dwValidDistanceThres; // 有效距离门限

    uint8 byCounter = 0;

    // 初始化阶段扩大有效距离门限值
    if (!byPredictionFlag)
    {
        fValidDistanceThres *= 2.0;
    }

    if (byListLen > 0 && !gg_stSysStatus.byReLocationFlag)
    {
        // 过滤
        stAheadMeanCoord = gLct_stMeanLctCoordList.astCoordList[byListLen - 1]; // 前一点定位坐标
        for (uint8 i = 0; i < *pbyMinimumValueListLen; i++)
        {
            stTmpLctCoord = pstMinimumValueList[i].stCoord;
            xx = (stTmpLctCoord.dwX - stAheadMeanCoord.dwX) * (stTmpLctCoord.dwX - stAheadMeanCoord.dwX);
            yy = (stTmpLctCoord.dwY - stAheadMeanCoord.dwY) * (stTmpLctCoord.dwY - stAheadMeanCoord.dwY);
            fDistance = sqrtf(xx + yy);

            // 间距未超过门限
            if (fDistance < fValidDistanceThres)
            {
                pstMinimumValueList[byCounter] = pstMinimumValueList[i];
                byCounter++;
            }
        }

        *pbyMinimumValueListLen = byCounter;
    }

    return;
}

// 选择最优坐标
// pstMinimumValueGroup:极小值数据组
// stPredictionCoord:预测坐标
// pstOptimumCoord:最优坐标（返回参数）
void lct_coordsel_sel_optimum_coord_by_prediction_coord(LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueGroup, COORD_T stPredictionCoord, LCT_COORDSEL_OPTIMUM_COORD_T *pstOptimumCoord)
{
    uint8 byCounter = 0; // 极小值个数
    uint8 byOldCounter = 0;
    LCT_COORDSEL_OPTIMUM_COORD_T astMinimumValueList[LCT_COORDSEL_MINIMUM_VALUE_NUM] = {0}; // 极小值队列
    LCT_COORDSEL_OPTIMUM_COORD_T stOptimumCoord1 = {0};                                     // 1维最优坐标
    LCT_COORDSEL_OPTIMUM_COORD_T stOptimumCoord2 = {0};                                     // 2维最优坐标
    uint8 byResult1 = 0;
    uint8 byResult2 = 0;
    LCT_COORDSEL_OPTIMUM_COORD_T stOptimumCoord = {0}; // 最优坐标

    //// 选1维坐标
    //  组织1维极小值队列
    lct_coordsel_organization_minimum_value_list(1, pstMinimumValueGroup, astMinimumValueList, &byCounter);
    byOldCounter = byCounter;

    // 选择1维最终坐标
    lct_coordsel_select_final_coord_by_variance(astMinimumValueList, byCounter, stPredictionCoord, &stOptimumCoord1, &byResult1);
    stOptimumCoord1.byInsideFlag = pstMinimumValueGroup->byOneDimensionInsideFlag;

    // 选2维坐标
    //  组织2维极小值队列
    byCounter = 0;
    lct_coordsel_organization_minimum_value_list(2, pstMinimumValueGroup, astMinimumValueList, &byCounter);
    byOldCounter = byCounter;

    // 选择2维最终坐标
    lct_coordsel_select_final_coord_by_variance(astMinimumValueList, byCounter, stPredictionCoord, &stOptimumCoord2, &byResult2);
    stOptimumCoord2.byInsideFlag = pstMinimumValueGroup->byTwoDimensionInsideFlag;

    // 存在一维或二维或预测坐标
    stOptimumCoord.byResultFlag = 1;

    //// 选择最终定位结果
    // 当前坐标既在一维空间内也在二维空间内
    if (stOptimumCoord1.byInsideFlag && stOptimumCoord2.byInsideFlag)
    {
        if (byResult2)
        {
            stOptimumCoord.stCoord.dwX = stOptimumCoord2.stCoord.dwX;
            stOptimumCoord.stCoord.dwY = stOptimumCoord2.stCoord.dwY;
            stOptimumCoord.stCoord.dwZ = 120;
            stOptimumCoord.bySourceFlag = stOptimumCoord2.bySourceFlag;
        }
        else if (byResult1)
        {
            stOptimumCoord.stCoord.dwX = stOptimumCoord1.stCoord.dwX;
            stOptimumCoord.stCoord.dwY = stOptimumCoord1.stCoord.dwY;
            stOptimumCoord.stCoord.dwZ = 120;
            stOptimumCoord.bySourceFlag = stOptimumCoord1.bySourceFlag;
        }
        else
        {
            stOptimumCoord.stCoord.dwX = stPredictionCoord.dwX;
            stOptimumCoord.stCoord.dwY = stPredictionCoord.dwY;
            stOptimumCoord.stCoord.dwZ = 120;
            stOptimumCoord.bySourceFlag = 3;
            gLct_dwUsePredictionCoordTimes++; // 适用预测坐标次数加1
        }
    }
    // 当前坐标在二维空间内
    else if (stOptimumCoord2.byInsideFlag)
    {
        if (byResult2)
        {
            stOptimumCoord.stCoord.dwX = stOptimumCoord2.stCoord.dwX;
            stOptimumCoord.stCoord.dwY = stOptimumCoord2.stCoord.dwY;
            stOptimumCoord.stCoord.dwZ = 120;
            stOptimumCoord.bySourceFlag = stOptimumCoord2.bySourceFlag;
        }
        else
        {
            stOptimumCoord.stCoord.dwX = stPredictionCoord.dwX;
            stOptimumCoord.stCoord.dwY = stPredictionCoord.dwY;
            stOptimumCoord.stCoord.dwZ = 120;
            stOptimumCoord.bySourceFlag = 3;
            gLct_dwUsePredictionCoordTimes++; // 适用预测坐标次数加1
        }
    }
    // 当前坐标在一维空间内
    else if (stOptimumCoord1.byInsideFlag)
    {
        if (byResult1)
        {
            stOptimumCoord.stCoord.dwX = stOptimumCoord1.stCoord.dwX;
            stOptimumCoord.stCoord.dwY = stOptimumCoord1.stCoord.dwY;
            stOptimumCoord.stCoord.dwZ = 120;
            stOptimumCoord.bySourceFlag = stOptimumCoord1.bySourceFlag;
        }
        else
        {
            stOptimumCoord.stCoord.dwX = stPredictionCoord.dwX;
            stOptimumCoord.stCoord.dwY = stPredictionCoord.dwY;
            stOptimumCoord.stCoord.dwZ = 120;
            stOptimumCoord.bySourceFlag = 3;
            gLct_dwUsePredictionCoordTimes++; // 适用预测坐标次数加1
        }
    }
    // 当前坐标既不在一维空间内也不在二维空间内
    else
    {
        stOptimumCoord.stCoord.dwX = stPredictionCoord.dwX;
        stOptimumCoord.stCoord.dwY = stPredictionCoord.dwY;
        stOptimumCoord.stCoord.dwZ = 120;
        stOptimumCoord.bySourceFlag = 3;
        gLct_dwUsePredictionCoordTimes++; // 适用预测坐标次数加1
    }

    *pstOptimumCoord = stOptimumCoord;

    return;
}

// 当前暂不具备预测条件，从极小值中选择一个最优坐标
// pstMinimumValueGroup:极小值数据组
// pstLocationCoord:定位坐标（返回参数）
void lct_coordsel_sel_optimum_coord_by_minimum_value(LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueGroup, LCT_COORDSEL_OPTIMUM_COORD_T *pstOptimumCoord)
{
    LCT_COORDSEL_OPTIMUM_COORD_T astMinimumValueList[LCT_COORDSEL_MINIMUM_VALUE_NUM]; // 极小值点队列
    uint8 byCounter = 0;                                                              // 极小值个数

    LCT_COORDSEL_OPTIMUM_COORD_T stOptimumCoord1 = {0};
    LCT_COORDSEL_OPTIMUM_COORD_T stOptimumCoord2 = {0};
    LCT_COORDSEL_OPTIMUM_COORD_T stOptimumCoord = {0};
    uint8 byResult1 = 0;
    uint8 byResult2 = 0;

    // 结构体初始化
    memset(astMinimumValueList, 0, sizeof(LCT_COORDSEL_OPTIMUM_COORD_T) * LCT_COORDSEL_MINIMUM_VALUE_NUM);

    ////选1维坐标
    // 组织1维极小值队列，按照时延误差排序
    lct_coordsel_organization_minimum_value_list(1, pstMinimumValueGroup, astMinimumValueList, &byCounter);
    lct_coordsel_select_final_coord_by_distance(astMinimumValueList, byCounter, &stOptimumCoord1, &byResult1);

    ////选2维坐标
    // 组织2维极小值队列，按照时延误差排序
    byCounter = 0;
    lct_coordsel_organization_minimum_value_list(2, pstMinimumValueGroup, astMinimumValueList, &byCounter);
    lct_coordsel_select_final_coord_by_distance(astMinimumValueList, byCounter, &stOptimumCoord2, &byResult2);

    // 一维和二维计算结果都存在
    stOptimumCoord.byResultFlag = 1;

    // 二维空间内有定位结果，优化二维空间内的结果
    if (byResult2)
    {
        stOptimumCoord.stCoord.dwX = stOptimumCoord2.stCoord.dwX;
        stOptimumCoord.stCoord.dwY = stOptimumCoord2.stCoord.dwY;
        stOptimumCoord.stCoord.dwZ = 120;
        stOptimumCoord.bySourceFlag = stOptimumCoord2.bySourceFlag;
    }
    // 一维空间内有定位结果
    else if (byResult1)
    {
        stOptimumCoord.stCoord.dwX = stOptimumCoord1.stCoord.dwX;
        stOptimumCoord.stCoord.dwY = stOptimumCoord1.stCoord.dwY;
        stOptimumCoord.stCoord.dwZ = 120;
        stOptimumCoord.bySourceFlag = stOptimumCoord1.bySourceFlag;
    }
    // 定位失败
    else
    {
        stOptimumCoord.byResultFlag = 0;
    }

    *pstOptimumCoord = stOptimumCoord;

    return;
}

// 组织极小值
// byLctType:定位类型，1：一维定位，2：二维定位
void lct_coordsel_organization_minimum_value_list(uint8 byLctType, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueGroup, LCT_COORDSEL_OPTIMUM_COORD_T *pstMinimumValueList, uint8 *pbyMinimumValueListLen)
{
    uint8 byCounter = 0;
    LCT_MINIMUM_VALUE_T *pstMinimumValueData = NULL;
    uint8 byMinimumValueDataLen = 0;
    LCT_COORDSEL_OPTIMUM_COORD_T stTmp = {0};

    // 一维
    if (byLctType == 1)
    {
        // 一维计算径
        pstMinimumValueData = pstMinimumValueGroup->astLinearCalMinimumList;
        byMinimumValueDataLen = pstMinimumValueGroup->byLinearCalMinimumListLen;
        for (uint8 i = 0; i < byMinimumValueDataLen; i++)
        {
            pstMinimumValueList[byCounter].fDelayDeviationSum = pstMinimumValueData[i].fDelayDeviationSum;
            pstMinimumValueList[byCounter].stCoord = pstMinimumValueData[i].stCoord;
            pstMinimumValueList[byCounter].bySourceFlag = 1; // 跟踪
            byCounter++;
        }

        // 一维跟踪坐标
        pstMinimumValueData = pstMinimumValueGroup->astLinearTrackMinimumList;
        byMinimumValueDataLen = pstMinimumValueGroup->byLinearTrackMinimumListLen;
        for (uint8 j = 0; j < byMinimumValueDataLen; j++)
        {
            pstMinimumValueList[byCounter].fDelayDeviationSum = pstMinimumValueData[j].fDelayDeviationSum;
            pstMinimumValueList[byCounter].stCoord = pstMinimumValueData[j].stCoord;
            pstMinimumValueList[byCounter].bySourceFlag = 2; // 跟踪
            byCounter++;
        }
    }
    // 二维
    else
    {
        // 二维计算径
        pstMinimumValueData = pstMinimumValueGroup->astCalMinimumList;
        byMinimumValueDataLen = pstMinimumValueGroup->byCalMinimumListLen;
        for (uint8 i = 0; i < byMinimumValueDataLen; i++)
        {
            pstMinimumValueList[byCounter].fDelayDeviationSum = pstMinimumValueData[i].fDelayDeviationSum;
            pstMinimumValueList[byCounter].stCoord = pstMinimumValueData[i].stCoord;
            pstMinimumValueList[byCounter].bySourceFlag = 1; // 跟踪
            byCounter++;
        }

        // 二维跟踪坐标
        pstMinimumValueData = pstMinimumValueGroup->astTrackMinimumList;
        byMinimumValueDataLen = pstMinimumValueGroup->byTrackMinimumListLen;
        for (uint8 j = 0; j < byMinimumValueDataLen; j++)
        {
            pstMinimumValueList[byCounter].fDelayDeviationSum = pstMinimumValueData[j].fDelayDeviationSum;
            pstMinimumValueList[byCounter].stCoord = pstMinimumValueData[j].stCoord;
            pstMinimumValueList[byCounter].bySourceFlag = 2; // 跟踪
            byCounter++;
        }
    }

    // 预测坐标
    if (pstMinimumValueGroup->byPredicMinimumListLen)
    {
        pstMinimumValueList[byCounter].fDelayDeviationSum = pstMinimumValueGroup->stPredicMinimumList.fDelayDeviationSum;
        pstMinimumValueList[byCounter].stCoord = pstMinimumValueGroup->stPredicMinimumList.stCoord;
        pstMinimumValueList[byCounter].bySourceFlag = 3; // 预测
        byCounter++;
    }

    // 根据时延误差排序
    for (uint8 k = 0; k < byCounter; k++)
    {
        for (uint8 l = k + 1; l < byCounter; l++)
        {
            if (pstMinimumValueList[l].fDelayDeviationSum < pstMinimumValueList[k].fDelayDeviationSum)
            {
                stTmp = pstMinimumValueList[k];
                pstMinimumValueList[k] = pstMinimumValueList[l];
                pstMinimumValueList[l] = stTmp;
            }
        }
    }

    *pbyMinimumValueListLen = byCounter;

    return;
}

// 选择一维最优坐标点
// pstMinimumValueList:极小值队列
// byMinimumValueListLen：极小值队列长度
// stPredictionCoord：预测坐标
// pstOptimumCoord：最优坐标，返回参数
// pbyResult：选择结果，返回参数。0：没有最优坐标，1：存在最优坐标
void lct_coordsel_select_final_coord_by_variance(LCT_COORDSEL_OPTIMUM_COORD_T *pstMinimumValueList, uint8 byMinimumValueListLen, COORD_T stPredictionCoord, LCT_COORDSEL_OPTIMUM_COORD_T *pstOptimumCoord, uint8 *pbyResult)
{
    uint8 byCounter = byMinimumValueListLen; // 极小值数
    uint8 byOldCounter = byMinimumValueListLen;

    // 1、过滤极小值跳变点
    if (byMinimumValueListLen > 0)
    {
        lct_coordsel_filter_jump_coord(pstMinimumValueList, &byCounter, 1);
    }
    if (byCounter <= 0)
    {
        byCounter = byOldCounter;
    }

    // 2、计算各极小值点与参考点之间的距离方差或者距离
    lct_coordsel_cal_p2p_distance_variance(stPredictionCoord, pstMinimumValueList, byCounter);

    // 选择距离方差最小或者距离最小的点为最优点
    if (byCounter > 0)
    {
        // 选择最优坐标
        pstOptimumCoord->stCoord.dwX = pstMinimumValueList[0].stCoord.dwX;
        pstOptimumCoord->stCoord.dwY = pstMinimumValueList[0].stCoord.dwY;
        pstOptimumCoord->stCoord.dwZ = 120;
        pstOptimumCoord->bySourceFlag = pstMinimumValueList[0].bySourceFlag;

        *pbyResult = 1;
    }
    else
    {
        *pbyResult = 0;
    }

    return;
}

// pstMinimumValueList:极小值队列
// byMinimumValueListLen:极小值队列长度
// pstOptimumCoord:最优坐标，返回参数
// pbyResult:选择结果，返回参数。0：没有最优坐标，1：存在最优坐标
void lct_coordsel_select_final_coord_by_distance(LCT_COORDSEL_OPTIMUM_COORD_T *pstMinimumValueList, uint8 byMinimumValueListLen, LCT_COORDSEL_OPTIMUM_COORD_T *pstOptimumCoord, uint8 *pbyResult)
{
    LCT_COORDSEL_MEAN_COORD_T *pstData = (LCT_COORDSEL_MEAN_COORD_T *)&gLct_stMeanLctCoordList; // 平滑定位坐标队列
    uint8 byListLen = pstData->byListLen;                                                       // 平滑定位坐标队列长度

    uint8 byCounter = byMinimumValueListLen;
    COORD_T stAheadPointCoord = {0}; // 前一个定位点坐标
    COORD_T stLocationCoord = {0};   // 定位坐标

    // 过滤极小值跳变点
    if (byCounter > 0)
    {
        lct_coordsel_filter_jump_coord(pstMinimumValueList, &byCounter, 0);
    }
    else
    {
        *pbyResult = 0;
        return;
    }

    // 取与上一点之间距离最小的点为定位点
    if (byListLen > 0)
    {
        // 计算极小值点与前一个点之间的距离方差
        stAheadPointCoord = pstData->astCoordList[byListLen - 1]; // 前一个定位点坐标
        lct_coordsel_cal_p2p_distance(stAheadPointCoord, pstMinimumValueList, byCounter);

        stLocationCoord = pstMinimumValueList[0].stCoord;
    }
    // 取时延误差最小的点
    else
    {
        stLocationCoord = pstMinimumValueList[0].stCoord;
    }

    // 保存参数
    pstOptimumCoord->stCoord = stLocationCoord;
    pstOptimumCoord->bySourceFlag = pstMinimumValueList[0].bySourceFlag;

    *pbyResult = 1;
    return;
}

// 计算极小值点到一个点的距离方差
void lct_coordsel_cal_p2p_distance_variance(COORD_T stMainPoint, LCT_COORDSEL_OPTIMUM_COORD_T *pstMinimumValueList, uint8 byMinimumValueListLen)
{
    COORD_T stMinimumCoord; // 极小值坐标

    int32 xx = 0;
    int32 yy = 0;

    flt32 fDistance = 0;
    flt32 fAverageDistance = 0; // 平均距离
    flt32 fVariance = 0;

    LCT_COORDSEL_OPTIMUM_COORD_T stTmpOptimumCoord = {0};

    // 有3个及以上极小值点
    if (byMinimumValueListLen > 2)
    {
        // 计算极小值与前一个定位点之间的距离
        for (uint8 i = 0; i < byMinimumValueListLen; i++)
        {
            stMinimumCoord = pstMinimumValueList[i].stCoord;
            xx = stMinimumCoord.dwX - stMainPoint.dwX;
            xx = xx * xx;
            yy = stMinimumCoord.dwY - stMainPoint.dwY;
            yy = yy * yy;

            fDistance = sqrt(xx + yy);
            fAverageDistance += fDistance / byMinimumValueListLen; // 平均距离

            pstMinimumValueList[i].fDistance = fDistance; // 极小值点与一个定位点间距离
        }

        // 计算各极小值的距离方差
        for (uint8 j = 0; j < byMinimumValueListLen; j++)
        {
            fDistance = pstMinimumValueList[j].fDistance;
            fVariance = (fDistance - fAverageDistance) * (fDistance - fAverageDistance);
            pstMinimumValueList[j].fDistanceVariance = fVariance;
        }

        // 对距离方差进行排序
        for (uint8 m = 0; m < byMinimumValueListLen; m++)
        {
            for (uint8 n = m + 1; n < byMinimumValueListLen; n++)
            {
                if (pstMinimumValueList[n].fDistanceVariance < pstMinimumValueList[m].fDistanceVariance)
                {
                    stTmpOptimumCoord = pstMinimumValueList[m];
                    pstMinimumValueList[m] = pstMinimumValueList[n];
                    pstMinimumValueList[n] = stTmpOptimumCoord;
                }
            }
        }
    }
    // 只有1个或者2两个极小值点
    else
    {
        // 计算极小值与前一个定位点之间的距离
        for (uint8 i = 0; i < byMinimumValueListLen; i++)
        {
            stMinimumCoord = pstMinimumValueList[i].stCoord;

            xx = stMinimumCoord.dwX - stMainPoint.dwX;
            xx = xx * xx;
            yy = stMinimumCoord.dwY - stMainPoint.dwY;
            yy = yy * yy;

            fDistance = sqrt(xx + yy);
            pstMinimumValueList[i].fDistance = fDistance; // 极小值点与一个定位点间的距离
        }

        // 对距离进行排序
        for (uint8 m = 0; m < byMinimumValueListLen; m++)
        {
            for (uint8 n = m + 1; n < byMinimumValueListLen; n++)
            {
                if (pstMinimumValueList[n].fDistance < pstMinimumValueList[m].fDistance)
                {
                    stTmpOptimumCoord = pstMinimumValueList[m];
                    pstMinimumValueList[m] = pstMinimumValueList[n];
                    pstMinimumValueList[n] = stTmpOptimumCoord;
                }
            }
        }
    }

    return;
}

// 选择最优坐标
// stAheadCoord:上一个定位坐标
// stPredicCoord:预测坐标
// pstMinimumValueList:极小值队列
// byMinimumValueListLen:极小值队列长度
// pstOptimumCoord:最优坐标
void lct_coordsel_sel_optimum_coord(COORD_T stPredicCoord, LCT_COORDSEL_OPTIMUM_COORD_T *pstMinimumValueList, uint8 byMinimumValueListLen, LCT_COORDSEL_OPTIMUM_COORD_T *pstOptimumCoord)
{
    flt32 dwPredictionRadius = (flt32)gg_stLctCoordselParam.dwPredictionRadius;
    LCT_COORDSEL_OPTIMUM_COORD_T *pstTmpOpimumCoord = NULL;
    flt32 fDistance = 0.0;
    flt32 xx = 0;
    flt32 yy = 0;
    uint8 byFlag = 0;

    for (uint8 i = 0; i < byMinimumValueListLen; i++)
    {
        pstTmpOpimumCoord = &pstMinimumValueList[i];
        xx = (pstTmpOpimumCoord->stCoord.dwX - stPredicCoord.dwX) * (pstTmpOpimumCoord->stCoord.dwX - stPredicCoord.dwX);
        yy = (pstTmpOpimumCoord->stCoord.dwY - stPredicCoord.dwY) * (pstTmpOpimumCoord->stCoord.dwY - stPredicCoord.dwY);
        fDistance = sqrtf(xx + yy);

        // 定位坐标与上一个坐标点的距离小于门限值
        if (fDistance < dwPredictionRadius)
        {
            pstOptimumCoord->stCoord = pstTmpOpimumCoord->stCoord;
            pstOptimumCoord->bySourceFlag = pstTmpOpimumCoord->bySourceFlag;
            byFlag = 1;
            break;
        }
    }

    // 没有合适的定位坐标，则使用预测坐标
    if (!byFlag)
    {
        pstOptimumCoord->stCoord = stPredicCoord;
        pstOptimumCoord->bySourceFlag = 3;
    }

    return;
}

// 计算极小值点到一个点的距离
void lct_coordsel_cal_p2p_distance(COORD_T stMainPoint, LCT_COORDSEL_OPTIMUM_COORD_T *pstMinimumValueList, uint8 byMinimumValueListLen)
{
    COORD_T stMinimumCoord; // 极小值坐标

    int32 xx = 0;
    int32 yy = 0;

    flt32 fDistance = 0;

    LCT_COORDSEL_OPTIMUM_COORD_T stTmpOptimumCoord = {0};

    if (byMinimumValueListLen > 2)
    {
        // 计算极小值与前一个定位点之间的距离
        for (uint8 i = 0; i < byMinimumValueListLen; i++)
        {
            stMinimumCoord = pstMinimumValueList[i].stCoord;

            xx = stMinimumCoord.dwX - stMainPoint.dwX;
            xx = xx * xx;

            yy = stMinimumCoord.dwY - stMainPoint.dwY;
            yy = yy * yy;

            fDistance = sqrt(xx + yy);
            pstMinimumValueList[i].fDistance = fDistance; // 极小值点与一个定位点间距离
        }

        // 对距离进行排序
        for (uint8 m = 0; m < byMinimumValueListLen; m++)
        {
            for (uint8 n = m + 1; n < byMinimumValueListLen; n++)
            {
                if (pstMinimumValueList[n].fDistance < pstMinimumValueList[m].fDistance)
                {
                    stTmpOptimumCoord = pstMinimumValueList[m];
                    pstMinimumValueList[m] = pstMinimumValueList[n];
                    pstMinimumValueList[n] = stTmpOptimumCoord;
                }
            }
        }
    }

    return;
}
