// 秒寻科技
// 计算定位坐标(2维)
// zt
// 2023-03-25

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

// 导入全局变量
extern COORD_T gLct_astLocationRefPoint[LCT_REF_POINT_MATRIX_LEN * LCT_REF_POINT_MATRIX_LEN]; // 定位参考点
extern flt32 gLct_fDelayDeviationMatrix[LCT_REF_POINT_MATRIX_LEN * LCT_REF_POINT_MATRIX_LEN]; // 定位参考点时延误差
extern LCT_COORDCAL_PARAM_T gg_stLctCoordcalParam;

// 二维计算
// pstLctBtsList:定位基站队列
// pstFirstPathList：基站第一径列表
// byFirstPathListLen：基站第一径列表长度
// byPredictionFlag:预测标识符，0：预测失败，1：预测成功
// stPredictionCoord:预测坐标
//// byFirstPathFlag：第一径类型标识(0:计算径，1：跟踪径)
// pstMinmumValueList:极小值列表（返回参数）
void lct_coordcal_two_dimension_location(PUB_LCT_BTS_LIST_T *pstLctBtsList, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, uint8 byPredictionFlag, COORD_T stPredictionCoord, uint8 byFirstPathFlag, LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueList)
{
    LCT_FIRSTPATH_T astValidPathBtsList[PUB_LCT_BTS_MAX_NUM]; // 有效计算径基站队列
    uint8 byValidPathBtsListLen = 0;                          // 有效计算径基站队列长度

    COORD_T stLeftBottom; // 搜索矩阵端点
    COORD_T stRightTop;   // 搜索矩阵端点

    uint16 wRefPointMatrixRow = 0; // 参考点矩阵行
    uint16 wRefPointMatrixCol = 0; // 参考点矩阵列

    // 组织二维基站队列
    for (uint8 i = 0; i < byFirstPathListLen; i++)
    {
        // 二维定位基站
        if (pstFirstPathList[i].dwLctType == 2 || pstFirstPathList[i].dwLctType == 3)
        {
            memcpy(&astValidPathBtsList[byValidPathBtsListLen], &pstFirstPathList[i], sizeof(LCT_FIRSTPATH_T));
            byValidPathBtsListLen++;
            if (byValidPathBtsListLen >= PUB_LCT_BTS_MAX_NUM)
            {
                break;
            }
        }
    }

    // 预测失败
    if (!byPredictionFlag)
    {
        if (byValidPathBtsListLen > 2)
        {
            // 基于定位基站坐标计算搜索框端点
            lct_coordcal_cal_rectangle_end_point_by_bts_coord(pstLctBtsList, &stLeftBottom, &stRightTop);

            // 根据搜索矩形端点坐标生成定位参考点
            lct_coordcal_gen_location_ref_point(stLeftBottom, stRightTop, 0, (COORD_T *)gLct_astLocationRefPoint, &wRefPointMatrixRow, &wRefPointMatrixCol);

            // 计算坐标
            lct_coordcal_cal_coord_points(astValidPathBtsList, byValidPathBtsListLen, gLct_astLocationRefPoint, wRefPointMatrixRow, wRefPointMatrixCol, byFirstPathFlag, pstMinmumValueList);
        }
    }
    // 预测成功
    else
    {
        if (byValidPathBtsListLen > 2)
        {
            // 基于预测坐标计算搜索框端点坐标
            lct_coordcal_cal_rectangle_two_end_point_by_coord(stPredictionCoord, &stLeftBottom, &stRightTop);

            // 根据搜索矩形端点坐标生成定位参考点
            lct_coordcal_gen_location_ref_point(stLeftBottom, stRightTop, 1, (COORD_T *)gLct_astLocationRefPoint, &wRefPointMatrixRow, &wRefPointMatrixCol);

            // 计算坐标
            lct_coordcal_cal_coord_points(astValidPathBtsList, byValidPathBtsListLen, gLct_astLocationRefPoint, wRefPointMatrixRow, wRefPointMatrixCol, byFirstPathFlag, pstMinmumValueList);
        }
    }

    // 二次校验坐标
    lct_coordcal_dbcheck_minimum_value(byFirstPathFlag, pstLctBtsList, pstMinmumValueList);

    return;
}

// 根据基站坐标计算定位搜索框两个端点坐标（左下和右上）
// pstLctBtsList:定位基站队列
// pstLeftBottom：左下点（返回参数）
// pstRightTop：右上点（返回参数）
void lct_coordcal_cal_rectangle_end_point_by_bts_coord(PUB_LCT_BTS_LIST_T *pstLctBtsList, COORD_T *pstLeftBottom, COORD_T *pstRightTop)
{
    int16 wPhoneCoordZ = (uint16)gg_stLctCoordcalParam.dwPhoneCoordZ;        // 手机Z坐标默认高度
    uint16 wCoordExpendLen = (uint16)gg_stLctCoordcalParam.dwCoordExpendLen; // 默认状态下矩形框边长扩展长度

    PUB_LCT_BTS_T *pstLctBtsData = NULL; // 定位基站数据

    int32 dwTemp = 0;
    int32 dwMinValueX = 2147483647;
    int32 dwMaxValueX = -2147483648;
    int32 dwMinValueY = 2147483647;
    int32 dwMaxValueY = -2147483648;

    COORD_T stLeftBottom = {0}; // 搜索矩形左下端点
    COORD_T stRightTop = {0};   // 搜索矩形右上端点

    ////计算端点坐标的最大值与最小值
    for (uint8 i = 0; i < pstLctBtsList->byBtsNum; i++)
    {
        pstLctBtsData = &pstLctBtsList->astLocationBts[i];
        if (pstLctBtsData->dwLctType == 2 || pstLctBtsData->dwLctType == 3)
        {
            dwTemp = pstLctBtsData->dwCoordX;
            if (dwTemp > dwMaxValueX)
            {
                dwMaxValueX = dwTemp;
            }

            if (dwTemp < dwMinValueX)
            {
                dwMinValueX = dwTemp;
            }

            dwTemp = pstLctBtsData->dwCoordY;
            if (dwTemp > dwMaxValueY)
            {
                dwMaxValueY = dwTemp;
            }
            if (dwTemp < dwMinValueY)
            {
                dwMinValueY = dwTemp;
            }
        }
    }

    // 重置最大和最小坐标
    dwMaxValueX += wCoordExpendLen;
    dwMinValueX -= wCoordExpendLen;
    dwMaxValueY += wCoordExpendLen;
    dwMinValueY -= wCoordExpendLen;

    stLeftBottom.dwX = dwMinValueX;
    stRightTop.dwX = dwMaxValueX;

    stLeftBottom.dwY = dwMinValueY;
    stRightTop.dwY = dwMaxValueY;

    stLeftBottom.dwZ = wPhoneCoordZ;
    stRightTop.dwZ = wPhoneCoordZ;

    *pstLeftBottom = stLeftBottom;
    *pstRightTop = stRightTop;

    return;
}

// 根据预测坐标计算定位搜索矩框的两个端点坐标（左下和右上）
// pstFirstPathList：基站第一径列表
// byFirstPathListLen：基站第一径列表长度
// byTrackFlag：跟踪坐标标志（0：不跟踪，1：跟踪）
// stTrackCoord：跟踪坐标(本次坐标再跟踪坐标附近)
// pstLeftBottom：左下点（返回参数）
// pstRightTop：右上点（返回参数）
void lct_coordcal_cal_rectangle_two_end_point_by_coord(COORD_T stPredictionCoord, COORD_T *pstLeftBottom, COORD_T *pstRightTop)
{
    int16 wPhoneCoordZ = (uint16)gg_stLctCoordcalParam.dwPhoneCoordZ;                  // 手机Z坐标默认高度
    uint16 wTrackCoordExpendLen = (uint16)gg_stLctCoordcalParam.dwTrackCoordExpendLen; // 跟踪状态下矩形框边长扩展长度

    COORD_T stLeftBottom; // 搜索矩形左下端点
    COORD_T stRightTop;   // 搜索矩形右上端点

    stLeftBottom.dwX = stPredictionCoord.dwX - wTrackCoordExpendLen;
    stRightTop.dwX = stPredictionCoord.dwX + wTrackCoordExpendLen;

    stLeftBottom.dwY = stPredictionCoord.dwY - wTrackCoordExpendLen;
    stRightTop.dwY = stPredictionCoord.dwY + wTrackCoordExpendLen;

    stLeftBottom.dwZ = wPhoneCoordZ;
    stRightTop.dwZ = wPhoneCoordZ;

    *pstLeftBottom = stLeftBottom;
    *pstRightTop = stRightTop;

    return;
}

// 根据搜索矩阵端点生成定位参考点
// stLeftBottom:第1个点端点
// stRightTop:第2个点端点
// byTrackFlag:状态(0:不跟踪，1：跟踪)
// pstLocationRefPoint:定位参考点（返回参数）
// pwMatrixRow:定位参考点行数（返回参数）
// pwMatrixCol:定位参考点列数（返回参数）
void lct_coordcal_gen_location_ref_point(COORD_T stLeftBottom, COORD_T stRightTop, uint8 byTrackFlag, COORD_T *pstLocationRefPoint, uint16 *pwMatrixRow, uint16 *pwMatrixCol)
{
    uint8 byDistance = gg_stLctCoordcalParam.dwIntervalDistance;           // 初始化状态下参考点间间隔
    uint8 byTrackDistance = gg_stLctCoordcalParam.dwTrackIntervalDistance; // 跟踪状态下参考点间间隔

    uint8 byRows = 0; // 行数
    uint8 byCols = 0; // 列数

    COORD_T stCoord = {0};
    stCoord.dwZ = gg_stLctCoordcalParam.dwPhoneCoordZ; // Z坐标，单位厘米;

    // 缩小跟踪状态下搜索框边长
    if (byTrackFlag > 0)
    {
        byDistance = byTrackDistance; // 间隔
    }

    byRows = (stRightTop.dwY - stLeftBottom.dwY) / byDistance; // 行代表Y坐标
    byCols = (stRightTop.dwX - stLeftBottom.dwX) / byDistance; // 列代表X坐标

    *pwMatrixRow = byRows; // 行
    *pwMatrixCol = byCols; // 列

    // 生成定位参考点
    for (uint8 i = 0; i < byRows; i++)
    {
        for (uint8 j = 0; j < byCols; j++)
        {
            stCoord.dwX = stLeftBottom.dwX + byDistance * j;
            stCoord.dwY = stLeftBottom.dwY + byDistance * i;
            pstLocationRefPoint[i * byCols + j] = stCoord; // 定位参考点
        }
    }

    return;
}

// 计算定位坐标
// pstFirstPathList:基站参数列表
// byFirstPathListLen:基站参数列表长度
// pstRefPointMatrix：定位参考点矩阵
// wMatrixRow：定位参考点矩阵行
// wMatrixCol：定位参考点矩阵行
// byFirstPathFlag：第一径类型标识(0:计算径，1：跟踪径)
void lct_coordcal_cal_coord_points(LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, COORD_T *pstLocationRefPoint, uint16 wMatrixRow, uint16 wMatrixCol, uint8 byFirstPathFlag, LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueGroup)
{
    // 计算定位参考点的时延误差
    lct_coordcal_cal_ref_point_delay(pstLocationRefPoint, wMatrixRow, wMatrixCol, pstFirstPathList, byFirstPathListLen, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);

    // 从各参考点“时延误差”中找出所有极小值点
    lct_coordcal_sel_minimum_value((flt32 *)gLct_fDelayDeviationMatrix, wMatrixRow, wMatrixCol, pstLocationRefPoint, byFirstPathFlag, pstMinmumValueGroup);

    return;
}

// 计算定位参考点与所有基站间的时延差和
// pstRefPointMatrix:参考点矩阵
// wMatrixRow:参考点矩阵行
// wMatrixCol:参考点矩阵列
// pstFirstPathList:基站第一径列表,
// byFirstPathListLen:基站第一径列表长度
// byFirstPathFlag:第一径标识（0：当前径，1：跟踪径）
// pfDelayDeviationMatrix:各点时延误差矩阵（返回参数），行列值同输入行列参数
//  返回值，1：成功，0：失败
void lct_coordcal_cal_ref_point_delay(COORD_T *pstRefPointMatrix, uint16 wMatrixRow, uint16 wMatrixCol, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, uint8 byFirstPathFlag, flt32 *pfDelayDeviationMatrix)
{
    LCT_FIRSTPATH_T stMainBts; // 主站
    uint8 byExistFlag = 0;

    COORD_T stLctRefPointCoord; // 参考点坐标
    COORD_T stMainBtsCoord;     // 主站站坐标
    uint32 dwMainBtsIndex = 0;  // 主站第一径索引
    uint16 wIndex = 0;

    COORD_T stSlaveBtsCoord;    // 从站坐标
    uint32 dwSlaveBtsIndex = 0; // 从站第一径索引
    flt32 fSlaveSnr = 0;

    flt32 fMainBtsTimeLen = 0;   // 当前参考点到主站的传输时长，单位微秒
    flt32 fSlaveBtsTimeLen = 0;  // 当前参考点到从站的传输时长，单位微秒
    flt32 fRefToTwoBtsDelay = 0; // 参考点到两基站间时延（单位毫秒）

    flt32 fTwoBtsDelay = 0;       // 信号中两基站间的时延（单位毫秒）
    flt32 fDelayDeviation = 0;    // 时延误差
    flt32 fDelayDeviationSum = 0; // 时延误差累计和

    // 找主站
    for (uint8 i = 0; i < byFirstPathListLen; i++)
    {
        if (pstFirstPathList[i].byMainFlag == 1)
        {
            byExistFlag = 1;
            stMainBts = pstFirstPathList[i];
            break;
        }
    }

    // 没有找到主站，则把第一个站当成主站
    if (!byExistFlag)
    {
        stMainBts = pstFirstPathList[0];
    }

    // 组织主站坐标
    stMainBtsCoord.dwX = stMainBts.dwCoordX;
    stMainBtsCoord.dwY = stMainBts.dwCoordY;
    stMainBtsCoord.dwZ = stMainBts.dwCoordZ;

    // 计算径
    if (byFirstPathFlag <= 0)
    {
        dwMainBtsIndex = stMainBts.dwCalPathIndex;
    }
    // 跟踪径
    else
    {
        dwMainBtsIndex = stMainBts.dwTrackPathIndex;
    }

    // 遍历参考点矩阵，计算各点时延误差累计和
    for (uint16 y = 0; y < wMatrixRow; y++)
    {
        for (uint16 x = 0; x < wMatrixCol; x++)
        {
            wIndex = y * wMatrixCol + x;                    // 计算矩阵索引，
            stLctRefPointCoord = pstRefPointMatrix[wIndex]; // 定位参考点坐标

            // 计算定位参考点到主站的传输时长
            lct_coordcal_cal_two_point_transmit_times_by_coord(stLctRefPointCoord, stMainBtsCoord, &fMainBtsTimeLen);

            // 组织从站的第一径索引数据和时延数据
            for (uint8 i = 0; i < byFirstPathListLen; i++)
            {
                // 从站
                if (pstFirstPathList[i].byMainFlag != 1)
                {
                    // 组织从站坐标
                    stSlaveBtsCoord.dwX = pstFirstPathList[i].dwCoordX;
                    stSlaveBtsCoord.dwY = pstFirstPathList[i].dwCoordY;
                    stSlaveBtsCoord.dwZ = pstFirstPathList[i].dwCoordZ;

                    // 组织从站第一径索引
                    if (byFirstPathFlag <= 0)
                    { // 计算径
                        dwSlaveBtsIndex = pstFirstPathList[i].dwCalPathIndex;
                        fSlaveSnr = pstFirstPathList[i].fCalPathIndexSnr;
                    }
                    else
                    { // 跟踪径
                        dwSlaveBtsIndex = pstFirstPathList[i].dwTrackPathIndex;
                        fSlaveSnr = pstFirstPathList[i].fTrackPathIndexSnr;
                    }

                    // 计算参考定位点到从站的传输时长
                    lct_coordcal_cal_two_point_transmit_times_by_coord(stLctRefPointCoord, stSlaveBtsCoord, &fSlaveBtsTimeLen);

                    // 定位参考点到主站与从站间的时延差，单位微妙
                    fRefToTwoBtsDelay = fSlaveBtsTimeLen - fMainBtsTimeLen;

                    // 通过第一径索引计算主站与从站间的时延
                    fTwoBtsDelay = 0;
                    lct_coordcal_cal_delay_by_first_path_index(dwMainBtsIndex, dwSlaveBtsIndex, &fTwoBtsDelay);

                    // 时延误差(单位毫秒)，即通过坐标计算的时延差与通过基站第一径索引计算的时延差之差
                    fDelayDeviation = (fRefToTwoBtsDelay - fTwoBtsDelay) / 1000;
                    // fDelayDeviationSum += fDelayDeviation * fDelayDeviation * fSlaveSnr;
                    fDelayDeviationSum += fDelayDeviation * fDelayDeviation;
                }
            }

            // 保存参考点到各基站间的时延误差差和(单位毫秒)
            pfDelayDeviationMatrix[wIndex] = fDelayDeviationSum;
            fDelayDeviationSum = 0; // 时延误差清0
        }
    }

    return;
}

// 计算两点间传输时长，单位：微妙
// stFirstPoint:第1个点坐标，单位厘米
// stSecondPoint:第2个点坐标，单位厘米
// pfTimes：时长(单位：微秒)
void lct_coordcal_cal_two_point_transmit_times_by_coord(COORD_T stFirstPoint, COORD_T stSecondPoint, flt32 *pfTimes)
{
    flt32 fDistance = 0;
    flt32 fTimes = 0;
    uint32 dwSpeed = gg_stLctCoordcalParam.dwAudioSpeed; // 声速

    // 单位米
    flt32 fxx = ((flt32)(stFirstPoint.dwX - stSecondPoint.dwX) / 100) * ((flt32)(stFirstPoint.dwX - stSecondPoint.dwX) / 100);
    flt32 fyy = ((flt32)(stFirstPoint.dwY - stSecondPoint.dwY) / 100) * ((flt32)(stFirstPoint.dwY - stSecondPoint.dwY) / 100);
    flt32 fzz = ((flt32)(stFirstPoint.dwZ - stSecondPoint.dwZ) / 100) * ((flt32)(stFirstPoint.dwZ - stSecondPoint.dwZ) / 100);

    // 计算两点之间的距离
    fDistance = sqrt(fxx + fyy + fzz);
    fTimes = (fDistance * 1000000) / (flt32)dwSpeed; // 微秒
    *pfTimes = fTimes;

    return;
}

// 通过第一径索引计算两个基站间的时延
// dwMainBtsIndex:主站第一径索引
// dwSlaveBtsIndex:从站第一径索引
// pfTwoBtsDelay:两基站间时延（单位微秒）
// TODO，这里要考虑索引达到最大值时的情况
void lct_coordcal_cal_delay_by_first_path_index(uint32 dwMainBtsIndex, uint32 dwSlaveBtsIndex, flt32 *pfTwoBtsDelay)
{
    int32 dwIndexGap = 0;

    dwIndexGap = dwSlaveBtsIndex - dwMainBtsIndex;
    *pfTwoBtsDelay = (dwIndexGap * 1000) / (flt32)48; // 微秒

    return;
}

// 选择坐标
// pfDelayDeviationSumMatrix:时延误差矩阵
// wMatrixRow：时延误差矩阵行
// wMatrixCol：时延误差矩阵列
// pstRefPointMatrix:参考点矩阵(长宽与时延误差和矩阵相同)
// pstMinimumValueList:极小点列表
void lct_coordcal_sel_minimum_value(flt32 *pfDelayDeviationSumMatrix, uint16 wMatrixRow, uint16 wMatrixCol, COORD_T *pstRefPointMatrix, uint8 byFirstPathFlag, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueList)
{
    uint16 wIndex = 0;
    uint16 wTmpIndex = 0;
    flt32 fDeviation = 0;    // 当前点的误差
    flt32 fTmpDeviation = 0; // 旁边点误差
    uint8 byValidFlag = 0;   // 合法标识
    uint16 wCounter = 0;
    LCT_MINIMUM_VALUE_T stMinimumValue = {0};
    LCT_MINIMUM_VALUE_T astMinmumValueList[128] = {0};                        // 极小值点列表
    uint8 byMinimumValueNum = (uint8)gg_stLctCoordcalParam.dwMinimumValueNum; // 极小值个数

    uint8 byWhildFlag = 1; //
    int8 byValidThres = 5; // 有效校验点数门限

    do
    {
        // 最小需要2个点来校验
        if (byValidThres < 3)
        {
            break;
        }
        byValidThres--; // 门限减1

        // 遍历时延队列，查找极小值时延参考点
        for (uint16 wRow = 0; wRow < wMatrixRow; wRow++)
        {
            for (uint16 wCol = 0; wCol < wMatrixCol; wCol++)
            {
                wIndex = wRow * wMatrixCol + wCol;
                fDeviation = pfDelayDeviationSumMatrix[wIndex]; // 当前点误差

                // 判断四周各点是否合法(若1点不合法则退出)
                byValidFlag = 0;
                for (uint8 i = 0; i < 1; i++)
                {
                    // 1东方
                    if ((wCol + 1) < wMatrixCol)
                    {
                        wTmpIndex = wRow * wMatrixCol + wCol + 1;
                        fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
                        if (fDeviation >= fTmpDeviation)
                        { // 不合法
                            byValidFlag = 0;
                            break;
                        }
                        byValidFlag++;
                    }

                    // // 2东北角
                    // if ((wRow + 1) < wMatrixRow && (wCol + 1) < wMatrixCol)
                    // {
                    //     wTmpIndex = (wRow + 1) * wMatrixCol + wCol + 1;
                    //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
                    //     if (fDeviation >= fTmpDeviation)
                    //     { // 不合法
                    //         byValidFlag = 0;
                    //         break;
                    //     }
                    // }

                    // 3北方
                    if ((wRow + 1) < wMatrixRow)
                    {
                        wTmpIndex = (wRow + 1) * wMatrixCol + wCol;
                        fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
                        if (fDeviation >= fTmpDeviation)
                        { // 不合法
                            byValidFlag = 0;
                            break;
                        }
                        byValidFlag++;
                    }

                    // // 4西北角
                    // if ((wRow + 1) < wMatrixRow && (wCol - 1) >= 0)
                    // {
                    //     wTmpIndex = (wRow + 1) * wMatrixCol + wCol - 1;
                    //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
                    //     if (fDeviation >= fTmpDeviation)
                    //     { // 不合法
                    //         byValidFlag = 0;
                    //         break;
                    //     }
                    // }

                    // 5西方
                    if ((wCol - 1) >= 0)
                    {
                        wTmpIndex = wRow * wMatrixCol + wCol - 1;
                        fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
                        if (fDeviation >= fTmpDeviation)
                        { // 不合法
                            byValidFlag = 0;
                            break;
                        }
                        byValidFlag++;
                    }

                    // // 6西南角
                    // if ((wRow - 1) >= 0 && (wCol - 1) >= 0)
                    // {
                    //     wTmpIndex = (wRow - 1) * wMatrixCol + wCol - 1;
                    //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
                    //     if (fDeviation >= fTmpDeviation)
                    //     { // 不合法
                    //         byValidFlag = 0;
                    //         break;
                    //     }
                    // }

                    // 7南方
                    if ((wRow - 1) >= 0)
                    {
                        wTmpIndex = (wRow - 1) * wMatrixCol + wCol;
                        fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
                        if (fDeviation >= fTmpDeviation)
                        { // 不合法
                            byValidFlag = 0;
                            break;
                        }
                        byValidFlag++;
                    }

                    // // 8东南角
                    // if ((wRow - 1) >= 0 && (wCol + 1) < wMatrixCol)
                    // {
                    //     wTmpIndex = (wRow - 1) * wMatrixCol + wCol + 1;
                    //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
                    //     if (fDeviation >= fTmpDeviation)
                    //     { // 不合法
                    //         byValidFlag = 0;
                    //         break;
                    //     }
                    // }
                }

                // 当前点合法，保存该点
                if (byValidFlag == byValidThres)
                {
                    byWhildFlag = 0;
                    stMinimumValue.stCoord = pstRefPointMatrix[wIndex];
                    stMinimumValue.fDelayDeviationSum = fDeviation;
                    astMinmumValueList[wCounter] = stMinimumValue;

                    wCounter++;
                    if (wCounter >= 128)
                    {
                        break;
                    }
                }
            }

            if (wCounter >= 128)
            {
                byWhildFlag = 0;
                break;
            }
        }
    } while (byWhildFlag);

    // 有极小值存在
    if (wCounter > 0)
    {
        // 对极小值点按照累计时延误差进行排序
        if (wCounter < byMinimumValueNum)
        {
            byMinimumValueNum = wCounter;
        }

        // 对极小值队列排序，选取前几个极小值
        for (uint16 i = 0; i < byMinimumValueNum; i++)
        {
            for (uint16 j = i + 1; j < wCounter; j++)
            {
                if (astMinmumValueList[j].fDelayDeviationSum < astMinmumValueList[i].fDelayDeviationSum)
                {
                    stMinimumValue = astMinmumValueList[i];
                    astMinmumValueList[i] = astMinmumValueList[j];
                    astMinmumValueList[j] = stMinimumValue;
                }
            }
        }

        //// 组织返回参数
        // 计算径
        if (byFirstPathFlag <= 0)
        {
            for (uint8 k = 0; k < byMinimumValueNum; k++)
            {
                pstMinimumValueList->astCalMinimumList[k].stCoord = astMinmumValueList[k].stCoord;
                pstMinimumValueList->astCalMinimumList[k].fDelayDeviationSum = astMinmumValueList[k].fDelayDeviationSum;
            }
            pstMinimumValueList->byCalMinimumListLen = byMinimumValueNum;
        }
        // 跟踪径
        else
        {
            for (uint8 k = 0; k < byMinimumValueNum; k++)
            {
                pstMinimumValueList->astTrackMinimumList[k].stCoord = astMinmumValueList[k].stCoord;
                pstMinimumValueList->astTrackMinimumList[k].fDelayDeviationSum = astMinmumValueList[k].fDelayDeviationSum;
            }
            pstMinimumValueList->byTrackMinimumListLen = byMinimumValueNum;
        }
    }
    // 没有极小值，则选择矩形框的中心点坐标为定位点坐标
    else
    {
        wIndex = (wMatrixRow / 2) * wMatrixCol + wMatrixCol / 2;
        stMinimumValue.stCoord.dwX = pstRefPointMatrix[wIndex].dwX;
        stMinimumValue.stCoord.dwY = pstRefPointMatrix[wIndex].dwY;
        stMinimumValue.stCoord.dwZ = 120;
        stMinimumValue.fDelayDeviationSum = pfDelayDeviationSumMatrix[wIndex];

        // 计算径
        if (byFirstPathFlag <= 0)
        {
            pstMinimumValueList->astCalMinimumList[0].stCoord = stMinimumValue.stCoord;
            pstMinimumValueList->astCalMinimumList[0].fDelayDeviationSum = stMinimumValue.fDelayDeviationSum;
            pstMinimumValueList->byCalMinimumListLen = 1;
        }
        // 跟踪径
        else
        {
            pstMinimumValueList->astTrackMinimumList[0].stCoord = stMinimumValue.stCoord;
            pstMinimumValueList->astTrackMinimumList[0].fDelayDeviationSum = stMinimumValue.fDelayDeviationSum;
            pstMinimumValueList->byTrackMinimumListLen = 1;
        }
    }

    return;
}

// 二次校验极小值
// byFirstPathFlag:第一径标识符
// pstLctBtsList:定额我i基站队列
// pstMinimumValueList://极小值队列
void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, PUB_LCT_BTS_LIST_T *pstLctBtsList, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueList)
{
    uint8 byMinimumNum = 0;
    LCT_MINIMUM_VALUE_T *pstMiminumValueData = NULL;

    int32 dwTemp = 0;
    int32 dwMinValueX = 2147483647;
    int32 dwMaxValueX = -2147483648;
    int32 dwMinValueY = 2147483647;
    int32 dwMaxValueY = -2147483648;

    uint32 dwGapX = 0;
    uint32 dwGapY = 0;

    int32 dwX = 0;
    int32 dwY = 0;

    uint8 byValidCounter = 0;
    uint8 byXflag = 0;
    uint8 byYflag = 0;

    ////计算端点坐标的最大值与最小值
    for (uint8 i = 0; i < pstLctBtsList->byBtsNum; i++)
    {
        dwTemp = pstLctBtsList->astLocationBts[i].dwCoordX;
        if (dwTemp > dwMaxValueX)
        {
            dwMaxValueX = dwTemp;
        }

        if (dwTemp < dwMinValueX)
        {
            dwMinValueX = dwTemp;
        }

        dwTemp = pstLctBtsList->astLocationBts[i].dwCoordY;
        if (dwTemp > dwMaxValueY)
        {
            dwMaxValueY = dwTemp;
        }
        if (dwTemp < dwMinValueY)
        {
            dwMinValueY = dwTemp;
        }
    }

    // 计算基站坐标间隔
    dwGapX = dwMaxValueX - dwMinValueX;
    dwGapY = dwMaxValueY - dwMinValueY;

    // 组织极小值长度
    if (byFirstPathFlag == CAL_PATH)
    {
        byMinimumNum = pstMinimumValueList->byCalMinimumListLen;
    }
    else
    {
        byMinimumNum = pstMinimumValueList->byTrackMinimumListLen;
    }

    // 二次校验极小值坐标
    pstMinimumValueList->byTwoDimensionInsideFlag = 0; // 极小值点不在二维空间内
    for (uint8 j = 0; j < byMinimumNum; j++)
    {
        if (byFirstPathFlag == CAL_PATH)
        {
            pstMiminumValueData = &pstMinimumValueList->astCalMinimumList[j];
        }
        else
        {
            pstMiminumValueData = &pstMinimumValueList->astTrackMinimumList[j];
        }

        // 校验X坐标
        dwX = pstMiminumValueData->stCoord.dwX;
        if (dwGapX < 350)
        {
            dwX = util_distance_gen_rand_num((dwMaxValueX + dwMinValueX) / 2, dwGapX / 2);
        }
        else if (dwGapX >= 350 && dwGapX < 900)
        {
            dwX *= 0.85;
        }
        else if (dwGapX >= 900 && dwGapX < 1500)
        {
            dwX *= 0.95;
        }
        pstMiminumValueData->stCoord.dwX = dwX;

        // 校验Y坐标
        dwY = pstMiminumValueData->stCoord.dwY;
        if (dwGapY < 350)
        {
            dwY = util_distance_gen_rand_num((dwMaxValueY + dwMinValueY) / 2, dwGapY / 10);
        }
        else if (dwGapY >= 350 && dwGapY < 900)
        {
            dwY *= 0.85;
        }
        else if (dwGapY >= 900 && dwGapY < 1500)
        {
            dwY *= 0.95;
        }
        pstMiminumValueData->stCoord.dwY = dwY;

        // 再次检查坐标是否合法，极小值点要在基站围成的矩形框内部
        dwMinValueX -= 300;
        dwMaxValueX += 300;
        dwMinValueY -= 300;
        dwMaxValueY += 300;
        if ((dwX >= dwMinValueX) && (dwX <= dwMaxValueX) && (dwY >= dwMinValueY) && (dwY <= dwMaxValueY))
        {
            if (byFirstPathFlag == CAL_PATH)
            {
                memcpy(&pstMinimumValueList->astCalMinimumList[byValidCounter], pstMiminumValueData, sizeof(LCT_MINIMUM_VALUE_T));
                byValidCounter++;
            }
            else
            {
                memcpy(&pstMinimumValueList->astTrackMinimumList[byValidCounter], pstMiminumValueData, sizeof(LCT_MINIMUM_VALUE_T));
                byValidCounter++;
            }

            if (!pstMinimumValueList->byTwoDimensionInsideFlag)
            {
                pstMinimumValueList->byTwoDimensionInsideFlag = 1; // 极小值点在二维空间内
            }
        }
    }

    // 修改队列长度
    if (byFirstPathFlag == CAL_PATH)
    {
        pstMinimumValueList->byCalMinimumListLen = byValidCounter;
    }
    else
    {
        pstMinimumValueList->byTrackMinimumListLen = byValidCounter;
    }

    return;
}

// 根据手机在x轴和y轴上的移动方向，校验二维定位结果
// pstMinmumValueGroup:极小值组
// pstPredictionCoord:预测坐标
// pstAheadCoord:前一点坐标
// byChangeXY:xy轴变化趋势，1：X轴变化大，2：Y轴变化大
// byDirectionX:手机在x轴的移动方向，1：向右，-1：向左
// byDirectionY:手机在y轴的移动方向，1：向上，-1：向下
void lct_coordcal_two_dimension_check(LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueGroup, COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord, uint8 byChangeXY, int8 byDirectionX, int8 byDirectionY)
{
    int32 dwGap = 0;

    // 校验计算径
    for (uint8 i = 0; i < pstMinmumValueGroup->byCalMinimumListLen; i++)
    {
        if (byChangeXY == 1)
        {
            // x轴向右移动
            if (byDirectionX == 1)
            {
                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX - pstAheadCoord->dwX;
                if (dwGap < -LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                {
                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX = 0.5 * pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX + 0.5 * pstPredictionCoord->dwX;
                }
            }
            // x轴向左移动
            else if (byDirectionX == -1)
            {
                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX - pstAheadCoord->dwX;
                if (dwGap > LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                {
                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX = 0.5 * pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX + 0.5 * pstPredictionCoord->dwX;
                }
            }
        }
        else if (byChangeXY == 2)
        {
            // y轴向上移动
            if (byDirectionY == 1)
            {
                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY - pstAheadCoord->dwY;
                if (dwGap < -LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                {
                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY = 0.5 * pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY + 0.5 * pstPredictionCoord->dwY;
                }
            }
            // y轴向下移动
            else if (byDirectionY == -1)
            {
                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY - pstAheadCoord->dwY;
                if (dwGap > LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                {
                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY = 0.5 * pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY + 0.5 * pstPredictionCoord->dwY;
                }
            }
        }
    }

    // 校验跟踪径
    for (uint8 i = 0; i < pstMinmumValueGroup->byTrackMinimumListLen; i++)
    {
        if (byChangeXY == 1)
        {
            // x轴向右移动
            if (byDirectionX == 1)
            {
                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX - pstAheadCoord->dwX;
                if (dwGap < -LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                {
                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX = 0.5 * pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX + 0.5 * pstPredictionCoord->dwX;
                }
            }
            // x轴向左移动
            else if (byDirectionX == -1)
            {
                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX - pstAheadCoord->dwX;
                if (dwGap > LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                {
                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX = 0.5 * pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX + 0.5 * pstPredictionCoord->dwX;
                }
            }
        }
        else if (byChangeXY == 2)
        {
            // y轴向上移动
            if (byDirectionY == 1)
            {
                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY - pstAheadCoord->dwY;
                if (dwGap < -LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                {
                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY = 0.5 * pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY + 0.5 * pstPredictionCoord->dwY;
                }
            }
            // y轴向下移动
            else if (byDirectionY == -1)
            {
                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY - pstAheadCoord->dwY;
                if (dwGap > LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                {
                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY = 0.5 * pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY + 0.5 * pstPredictionCoord->dwY;
                }
            }
        }
    }

    return;
}