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

#include "../../includes/includes.h"
#include "./lct_public.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;
extern PUB_SYS_STATUS_T gg_stSysStatus;
extern LCT_COORDSEL_MEAN_COORD_T gLct_stMeanLctCoordList;
extern LCT_COORDSEL_ORIGINAL_COORD_T gLct_stOriginalLctCoordList;
extern int8 gLct_byDirectionX;
extern int8 gLct_byDirectionY;
extern int32 gLct_dwSpeedX; // X位移速度
extern int32 gLct_dwSpeedY; // Y位移速度

// 一维计算
// pstLctDataList:定位数据队列
// byPredictionFlag:预测标识符，0：预测失败，1:预测成功
// stPredictionCoord:预测坐标
// pstValidBtsList:有效基站列表
// byValidBtsListLen:有效基站列表长度
// byFirstPathFlag:第一径标识符，0：计算径，1：跟踪径
// pstMinmumValueList:极小值列表（返回参数）
void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, uint8 byPredictionFlag, COORD_T stPredictionCoord, LCT_FIRSTPATH_T *pstValidBtsList, uint8 byValidBtsListLen, uint8 byFirstPathFlag, LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueList)
{
    LCT_COORDCAL_ONE_DIMENSION_BTS_DUAD_LIST_T stBtsDuadList = {0};
    LCT_COORDCAL_ONE_DIMENSION_BTS_DUAD_T stBtsDuadData = {0};
    LCT_LINEAR_MINIMUM_LIST_T stLinearMinmumList = {0};

    uint8 byCounter = 0;
    LCT_FIRSTPATH_T astValidBtsList[2] = {0};     // 1维基站对
    uint8 byValidBtsNum = 0;                      // 有效基站数量
    LCT_FIRSTPATH_T *pstMainBtsFirstPaht = NULL;  // 主站
    LCT_FIRSTPATH_T *pstSlaveBtsFirstPaht = NULL; // 从站

    PUB_LOCATION_DATA_T *pstLctData = NULL;
    uint8 byFreqIndex = 0;
    int16 wIndexGap = 0;
    COORD_T stAheadCoord = {0}; // 上一个点坐标

    uint16 wPoints = 0;
    uint16 wRefPointMatrixRow = 0; // 参考点矩阵行
    uint16 wRefPointMatrixCol = 0; // 参考点矩阵列
    COORD_T stFirstCoord = {0};
    COORD_T stSecondCoord = {0};
    uint8 byInsideFlag = 0;
    LCT_COORDCAL_LINEAR_FUNC_T stLinearFunc = {0}; // 线性方程
    COORD_T stSelRefPoint = {0};                   // 选择参考点
    flt32 fDelayDaviation = 0;                     // 时延误差
    uint8 byResult = 0;

    uint8 byTwoBtsLctFlag = 0; // 0:无双站定位，1：存在双站定位

    // 参数容错
    if (byValidBtsListLen < 1)
    {
        return;
    }

    // 组织1维基站对（两个基站为1对）
    lct_coordcal_organize_one_dimension_bts_duad(byFirstPathFlag, pstValidBtsList, byValidBtsListLen, &stBtsDuadList);

    // 计算线性定位参考点
    stLinearMinmumList.byValidFlag = 0;
    for (uint8 i = 0; i < stBtsDuadList.byBtsDuadNum; i++)
    {
        byCounter = 0;
        stBtsDuadData = stBtsDuadList.astBtsDuadList[i]; // 一对一维基站
        memcpy(astValidBtsList, stBtsDuadData.astBtsList, sizeof(LCT_FIRSTPATH_T) * 2);

        // 基站1
        stFirstCoord.dwX = astValidBtsList[0].dwCoordX;
        stFirstCoord.dwY = astValidBtsList[0].dwCoordY;
        stFirstCoord.dwZ = astValidBtsList[0].dwCoordZ;

        // 基站2
        stSecondCoord.dwX = astValidBtsList[1].dwCoordX;
        stSecondCoord.dwY = astValidBtsList[1].dwCoordY;
        stFirstCoord.dwZ = astValidBtsList[1].dwCoordZ;

        // 系统处于跟踪阶段
        wPoints = 0;
        if (byPredictionFlag)
        {
            // 两基站
            if (stBtsDuadData.byBtsNum == 2)
            {
                byTwoBtsLctFlag = 1; // 两站定位标识符

                // 检查预测坐标是否在一维基站内部
                byInsideFlag = lct_coordcal_check_one_dimension_inside(stFirstCoord, stSecondCoord, stPredictionCoord);
                if (byInsideFlag) // 在内部
                {
                    // 1维定位有效
                    if (!stLinearMinmumList.byValidFlag)
                    {
                        stLinearMinmumList.byValidFlag = 1;
                    }

                    // 构建1维定位的线性方程
                    lct_coordcal_build_linear_func(stFirstCoord, stSecondCoord, &stLinearFunc);

                    // 检查基站第一径的有效性
                    lct_coordcal_check_one_dimension_bts_firstpath_valid(byPredictionFlag, stPredictionCoord, pstLctDataList, byFirstPathFlag, astValidBtsList, &byValidBtsNum);

                    // 两个基站有效，进行1维定位
                    if (byValidBtsNum == 2)
                    {
                        // 生成线性参考点
                        lct_coordcal_build_linear_ref_point(byPredictionFlag, stPredictionCoord, stFirstCoord, stSecondCoord, &stLinearFunc, gLct_astLocationRefPoint, &wRefPointMatrixRow, &wRefPointMatrixCol);
                        wPoints = wRefPointMatrixRow * wRefPointMatrixCol; // 定位参考点数量

                        // 计算参考点时延误差
                        lct_coordcal_cal_ref_point_delay(gLct_astLocationRefPoint, wRefPointMatrixRow, wRefPointMatrixCol, astValidBtsList, 2, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);

                        // 选择线性参考点
                        lct_coordcal_sel_linear_ref_point(gLct_astLocationRefPoint, gLct_fDelayDeviationMatrix, wRefPointMatrixRow, wRefPointMatrixCol, &stLinearFunc, &stSelRefPoint, &fDelayDaviation);
                    }
                    // 一个基站有效，进行0维定位
                    else
                    {
                        if (!byTwoBtsLctFlag)
                        {
                            wPoints = 0;
                            fDelayDaviation = 2;
                            byResult = lct_coordcal_zero_dimension_location(pstLctDataList, astValidBtsList, byFirstPathFlag, &stLinearFunc, &stSelRefPoint);
                            if (byResult)
                            {
                                wPoints = 1;

                                // 二次校验定位坐标

                                // // 调整无效基站的第一径
                                // pstMainBtsFirstPaht = &astValidBtsList[0];
                                // pstSlaveBtsFirstPaht = &pstValidBtsList[1];
                                // lct_coordcal_adjust_first_path_index(pstLctDataList, byFirstPathFlag, &stSelRefPoint, pstMainBtsFirstPaht, pstSlaveBtsFirstPaht);
                            }
                        }
                    }
                }
            }
            // 单基站
            else
            {
                if (!byTwoBtsLctFlag)
                {
                    wPoints = 0;
                    fDelayDaviation = 2;
                    byResult = lct_coordcal_single_bts_location(pstLctDataList, astValidBtsList, byFirstPathFlag, &stSelRefPoint);
                    if (byResult)
                    {
                        wPoints = 1;
                        stLinearMinmumList.byValidFlag = 1;
                    }
                }
            }
        }
        // 系统处于初始化阶段
        else
        {
            // 两级站定位
            if (stBtsDuadData.byBtsNum == 2)
            {
                byTwoBtsLctFlag = 1; // 两站定位标识符

                // 构建1维定位的线性方程
                lct_coordcal_build_linear_func(stFirstCoord, stSecondCoord, &stLinearFunc);

                // 生成线性参考点
                lct_coordcal_build_linear_ref_point(byPredictionFlag, stPredictionCoord, stFirstCoord, stSecondCoord, &stLinearFunc, gLct_astLocationRefPoint, &wRefPointMatrixRow, &wRefPointMatrixCol);
                wPoints = wRefPointMatrixRow * wRefPointMatrixCol; // 定位参考点数量

                // 计算参考点时延误差
                lct_coordcal_cal_ref_point_delay(gLct_astLocationRefPoint, wRefPointMatrixRow, wRefPointMatrixCol, astValidBtsList, 2, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);

                // 选择线性参考点
                lct_coordcal_sel_linear_ref_point(gLct_astLocationRefPoint, gLct_fDelayDeviationMatrix, wRefPointMatrixRow, wRefPointMatrixCol, &stLinearFunc, &stSelRefPoint, &fDelayDaviation);
            }
            // 单基站定位
            else
            {
                if (!byTwoBtsLctFlag)
                {
                    wPoints = 0;
                    fDelayDaviation = 2;
                    byResult = lct_coordcal_single_bts_location(pstLctDataList, astValidBtsList, byFirstPathFlag, &stSelRefPoint);
                    if (byResult)
                    {
                        wPoints = 1;
                        stLinearMinmumList.byValidFlag = 1;
                    }
                }
            }
        }

        // 保存结果
        byCounter = 0;
        if (wPoints > 0)
        {
            if (byFirstPathFlag == CAL_PATH)
            {
                byCounter = stLinearMinmumList.byCalMinimumListLen;
                stLinearMinmumList.astCalMinimumList[byCounter].dwOneDimensionMainBtsId = astValidBtsList[0].dwBtsId;
                stLinearMinmumList.astCalMinimumList[byCounter].dwOneDimensionSlaveBtsId = astValidBtsList[1].dwBtsId;
                memcpy(&stLinearMinmumList.astCalMinimumList[byCounter].astFirstPathList, astValidBtsList, sizeof(LCT_FIRSTPATH_T) * 2);
                stLinearMinmumList.astCalMinimumList[byCounter].stCoord = stSelRefPoint;
                stLinearMinmumList.astCalMinimumList[byCounter].fDelayDeviationSum = fDelayDaviation;
                stLinearMinmumList.byCalMinimumListLen++;
            }
            else
            {
                byCounter = stLinearMinmumList.byTrackMinimumListLen;
                stLinearMinmumList.astTrackMinimumList[byCounter].dwOneDimensionMainBtsId = astValidBtsList[0].dwBtsId;
                stLinearMinmumList.astTrackMinimumList[byCounter].dwOneDimensionSlaveBtsId = astValidBtsList[1].dwBtsId;
                memcpy(&stLinearMinmumList.astTrackMinimumList[byCounter].astFirstPathList, astValidBtsList, sizeof(LCT_FIRSTPATH_T) * 2);
                stLinearMinmumList.astTrackMinimumList[byCounter].stCoord = stSelRefPoint;
                stLinearMinmumList.astTrackMinimumList[byCounter].fDelayDeviationSum = fDelayDaviation;
                stLinearMinmumList.byTrackMinimumListLen++;
            }
        }
    }

    // 保存一维定位的有效性
    if (!pstMinmumValueList->byOneDimensionInsideFlag)
    {
        pstMinmumValueList->byOneDimensionInsideFlag = stLinearMinmumList.byValidFlag;
    }

    // 校验定位坐标合法性
    if (byFirstPathFlag == CAL_PATH)
    {
        // 校验计算径结果
        lct_coordcal_check_linear_lct_coord_valid(1, &stLinearMinmumList);

        // 保存输出参数
        if (stLinearMinmumList.byCalMinimumListLen > 0)
        {
            pstMinmumValueList->byLinearCalMinimumListLen = 1;
            pstMinmumValueList->astLinearCalMinimumList[0].stCoord = stLinearMinmumList.astCalMinimumList[0].stCoord;
            pstMinmumValueList->astLinearCalMinimumList[0].fDelayDeviationSum = stLinearMinmumList.astCalMinimumList[0].fDelayDeviationSum;
        }
    }
    else
    {
        // 校验跟踪径结果
        lct_coordcal_check_linear_lct_coord_valid(2, &stLinearMinmumList);

        // 保存输出参数
        if (stLinearMinmumList.byTrackMinimumListLen > 0)
        {
            pstMinmumValueList->byLinearTrackMinimumListLen = 1;
            pstMinmumValueList->astLinearTrackMinimumList[0].stCoord = stLinearMinmumList.astTrackMinimumList[0].stCoord;
            pstMinmumValueList->astLinearTrackMinimumList[0].fDelayDeviationSum = stLinearMinmumList.astTrackMinimumList[0].fDelayDeviationSum;
        }
    }

    return;
}

// 组织1维基站对(2个基站为1对)
// byFirstPathType:第一径类型，0：计算径，1：跟踪径
// pstValidBtsList:有效基站队列
// byValidBtsListLen:有效基站队列长度
// pstBtsDuadList:基站对队列
void lct_coordcal_organize_one_dimension_bts_duad(uint8 byFirstPathType, LCT_FIRSTPATH_T *pstValidBtsList, uint8 byValidBtsListLen, LCT_COORDCAL_ONE_DIMENSION_BTS_DUAD_LIST_T *pstBtsDuadList)
{
    uint8 byBtsDuadCounter = 0; // 计算对计数器
    LCT_COORDCAL_ONE_DIMENSION_BTS_DUAD_T *pstBtsDuadData = NULL;
    LCT_COORDCAL_ONE_DIMENSION_BTS_DUAD_T stBtsDuadData = {0};
    uint8 byExistFlag = 0;

    LCT_FIRSTPATH_T *pstMainBtsPath = NULL;
    uint32 dwMainBtsId = 0;
    uint8 byOneDimensionBtsNum = 0;

    LCT_FIRSTPATH_T *pstSlaveBtsPath = NULL;
    uint32 dwSlaveBtsId = 0;

    flt32 fSnr = 0;
    flt32 fMaxValue = 0;

    // 组织1维基站对（两个基站为1对）
    for (uint8 i = 0; i < byValidBtsListLen; i++)
    {
        pstMainBtsPath = &pstValidBtsList[i]; // 主站第一径
        dwMainBtsId = pstMainBtsPath->dwBtsId;

        // 主站定位类型，1：1维定位，2：二维定位，3：1维和2维定位
        byOneDimensionBtsNum = 0;
        fSnr = 0;
        fMaxValue = 0;
        if (pstMainBtsPath->dwLctType == 1 || pstMainBtsPath->dwLctType == 3)
        {
            fMaxValue = pstMainBtsPath->fMaxAmplValue; // 最大值

            // 保存基站snr
            if (byFirstPathType == CAL_PATH)
            {
                fSnr = pstMainBtsPath->fCalPathIndexSnr;
            }
            else
            {
                fSnr = pstMainBtsPath->fTrackPathIndexSnr;
            }

            // 遍历要与主站进行两站定位的从站
            for (uint8 j = 0; j < pstMainBtsPath->dwTwoBtsNum; j++)
            {
                byOneDimensionBtsNum = 1;
                dwSlaveBtsId = pstMainBtsPath->adwTwoBtsList[j]; // 从站ID
                for (uint8 k = 0; k < byValidBtsListLen; k++)
                {
                    pstSlaveBtsPath = &pstValidBtsList[k]; // 从站第一径
                    if (pstSlaveBtsPath->dwBtsId != dwMainBtsId && pstSlaveBtsPath->dwBtsId == dwSlaveBtsId)
                    {
                        byOneDimensionBtsNum++;
                        // 过滤重复基站对
                        if (pstBtsDuadList->byBtsDuadNum > 0)
                        {
                            // 遍历基站对列表
                            byExistFlag = 0;
                            for (uint8 l = 0; l < pstBtsDuadList->byBtsDuadNum; l++)
                            {
                                pstBtsDuadData = &pstBtsDuadList->astBtsDuadList[l];
                                if ((pstBtsDuadData->dwFirstBtsId == dwMainBtsId && pstBtsDuadData->dwSecondBtsId == dwSlaveBtsId) || (pstBtsDuadData->dwFirstBtsId == dwSlaveBtsId && pstBtsDuadData->dwSecondBtsId == dwMainBtsId))
                                {
                                    byExistFlag = 1;
                                    break;
                                }
                            }

                            // 不存在，则添加到队列中
                            if (!byExistFlag)
                            {
                                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].dwFirstBtsId = dwMainBtsId;
                                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].dwSecondBtsId = dwSlaveBtsId;
                                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].byBtsNum = 2;
                                memcpy(&pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0], pstMainBtsPath, sizeof(LCT_FIRSTPATH_T));
                                memcpy(&pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1], pstSlaveBtsPath, sizeof(LCT_FIRSTPATH_T));

                                // 修改主站标识符
                                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].byMainFlag = 0;
                                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1].byMainFlag = 0;
                                if (pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].fCalPathIndexSnr > pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1].fCalPathIndexSnr)
                                {
                                    pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].byMainFlag = 1;
                                }
                                else
                                {
                                    pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1].byMainFlag = 1;
                                }

                                // 保存基站snr
                                if (byFirstPathType == CAL_PATH)
                                {
                                    fSnr += pstSlaveBtsPath->fCalPathIndexSnr;
                                    fMaxValue += pstSlaveBtsPath->fMaxAmplValue;
                                }
                                else
                                {
                                    fSnr += pstSlaveBtsPath->fTrackPathIndexSnr;
                                    fMaxValue += pstSlaveBtsPath->fMaxAmplValue;
                                }
                                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].fSnr = fSnr;
                                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].fAmplMaxValue = fMaxValue;

                                pstBtsDuadList->byBtsDuadNum++;
                                byBtsDuadCounter++;
                            }
                        }
                        else
                        {
                            pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].dwFirstBtsId = dwMainBtsId;
                            pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].dwSecondBtsId = dwSlaveBtsId;
                            pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].byBtsNum = 2;
                            memcpy(&pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0], pstMainBtsPath, sizeof(LCT_FIRSTPATH_T));
                            memcpy(&pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1], pstSlaveBtsPath, sizeof(LCT_FIRSTPATH_T));

                            // 修改主站标识符
                            pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].byMainFlag = 0;
                            pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1].byMainFlag = 0;
                            if (pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].fCalPathIndexSnr > pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1].fCalPathIndexSnr)
                            {
                                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].byMainFlag = 1;
                            }
                            else
                            {
                                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1].byMainFlag = 1;
                            }

                            // 保存基站snr
                            if (byFirstPathType == CAL_PATH)
                            {
                                fSnr += pstSlaveBtsPath->fCalPathIndexSnr;
                                fMaxValue += pstSlaveBtsPath->fMaxAmplValue;
                            }
                            else
                            {
                                fSnr += pstSlaveBtsPath->fTrackPathIndexSnr;
                                fMaxValue += pstSlaveBtsPath->fMaxAmplValue;
                            }
                            pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].fSnr = fSnr;
                            pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].fAmplMaxValue = fMaxValue;

                            pstBtsDuadList->byBtsDuadNum++;
                            byBtsDuadCounter++;
                        }
                    }
                }
            }

            // 一维定位，但只有一个基站(实现单站定位)
            // if (byOneDimensionBtsNum <= 1 && pstBtsDuadList->byBtsDuadNum <= 0)
            if (byOneDimensionBtsNum <= 1)
            {
                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].dwFirstBtsId = dwMainBtsId;
                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].byBtsNum = 1;
                memcpy(&pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0], pstMainBtsPath, sizeof(LCT_FIRSTPATH_T));

                // 修改主站标识符
                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].byMainFlag = 1;

                // 保存snr
                pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].fSnr = fSnr;

                pstBtsDuadList->byBtsDuadNum++;
                byBtsDuadCounter++;
            }
        }
    }

    // 对最大值进行排序
    for (uint8 i = 0; i < pstBtsDuadList->byBtsDuadNum; i++)
    {
        for (uint8 j = i + 1; j < pstBtsDuadList->byBtsDuadNum; j++)
        {
            if (pstBtsDuadList->astBtsDuadList[j].fAmplMaxValue > pstBtsDuadList->astBtsDuadList[i].fAmplMaxValue)
            {
                stBtsDuadData = pstBtsDuadList->astBtsDuadList[i];
                pstBtsDuadList->astBtsDuadList[i] = pstBtsDuadList->astBtsDuadList[j];
                pstBtsDuadList->astBtsDuadList[j] = stBtsDuadData;
            }
        }
    }

    // if (pstBtsDuadList->byBtsDuadNum > 1)
    // {
    //     pstBtsDuadList->byBtsDuadNum = 1;
    // }

    return;
}

// 检查一维基站第一径的有效性
// byPredictionFlag:负责坐标标识符，0：失败，1：成功
// stPredictionCoord:预测坐标
// pstLctDataList:定位数据队列
// byFirstpathFlag:第一径标识符，0：计算径，1：跟踪径
// pstValidBtsList:有效基站队列
// pbyValidBtsCounter:有效站数量，0：没有基站有效，1：一个基站有效，2：两个基站有效
void lct_coordcal_check_one_dimension_bts_firstpath_valid(uint8 byPredictionFlag, COORD_T stPredictionCoord, PUB_LOCATION_DATA_T *pstLctDataList, uint8 byFirstpathFlag, LCT_FIRSTPATH_T *pstValidBtsList, uint8 *pbyValidBtsCounter)
{
    LCT_FIRSTPATH_T *pstFirstPath = NULL; // 第一径
    LCT_FIRSTPATH_T stFirstPathDataTmp = {0};

    PUB_LOCATION_DATA_T *pstLctData = NULL; // 基站数据
    uint8 byFreqIndex = 0;                  // 基站频率

    flt32 fFirstPathValueThres = (flt32)gg_stLctCoordcalParam.dwOneFirstPahtValueThres;
    flt32 fFirstPathSnrThres = (flt32)gg_stLctCoordcalParam.dwOneFirstPahtSnrThres / 10;
    uint16 wFirstPathIndexJumpThres = gg_stLctCoordcalParam.dwOneFirstPathIndexJumpThres;
    uint16 wIndexGap = 0;
    uint8 byCounter = 0;

    COORD_T stFirstBts = {0}; // 第一个基站坐标
    flt32 fDistFirstBts = 0;
    COORD_T stSecondBts = {0}; // 第二个基站坐标
    flt32 fDistSecondBts = 0;

    // 检查基站有效性
    for (uint8 i = 0; i < 2; i++)
    {
        pstFirstPath = &pstValidBtsList[i];
        byFreqIndex = pstFirstPath->byFreqIndex;
        pstLctData = &pstLctDataList[byFreqIndex];

        if (byFirstpathFlag == CAL_PATH)
        {
            // 检查计算径基站Snr和ampl value是否符合定位要求
            if (pstFirstPath->fCalPathIndexSnr > fFirstPathSnrThres && pstFirstPath->fFrameCalPathValue > fFirstPathValueThres)
            {
                if (pstLctData->wAheadFrameCalPahtIndex > 0 && !gg_stSysStatus.byReLocationFlag)
                {
                    // 第一径跳变检查
                    wIndexGap = abs(pstFirstPath->wFrameCalIndex - pstLctData->wAheadFrameCalPahtIndex);
                    if (wIndexGap < wFirstPathIndexJumpThres)
                    {
                        memcpy(&pstValidBtsList[byCounter], pstFirstPath, sizeof(LCT_FIRSTPATH_T));
                        byCounter++;
                    }
                    else
                    {
                        pstValidBtsList[byCounter].wFrameCalIndex = 0; // 计算径无效
                    }
                }
                else
                {
                    memcpy(&pstValidBtsList[byCounter], pstFirstPath, sizeof(LCT_FIRSTPATH_T));
                    byCounter++;
                }
            }
        }
        else
        {
            // 检查跟踪径基站Snr和ampl value是否符合定位要求
            if (pstFirstPath->fTrackPathIndexSnr > fFirstPathSnrThres && pstFirstPath->fFrameTrackPathValue > fFirstPathValueThres)
            {
                if (pstLctData->wAheadFrameTrackPathIndex > 0 && !gg_stSysStatus.byReLocationFlag)
                {
                    // 第一径跳变检查
                    wIndexGap = abs(pstFirstPath->wFrameTrackIndex - pstLctData->wAheadFrameTrackPathIndex);
                    if (wIndexGap < wFirstPathIndexJumpThres)
                    {
                        memcpy(&pstValidBtsList[byCounter], pstFirstPath, sizeof(LCT_FIRSTPATH_T));
                        byCounter++;
                    }
                    else
                    {
                        pstValidBtsList[byCounter].wFrameTrackIndex = 0; // 跟踪径无效
                    }
                }
                else
                {
                    memcpy(&pstValidBtsList[byCounter], pstFirstPath, sizeof(LCT_FIRSTPATH_T));
                    byCounter++;
                }
            }
        }
    }

    // 没有有效基站，则选择身前的基站作为有效基站
    if (byCounter <= 0)
    {
        // 基站1在身前，基站2在身后
        if (pstValidBtsList[0].fMaxAmplValue > pstValidBtsList[1].fMaxAmplValue)
        {
            byCounter = 1;
        }
        // 基站2在身前，基站1在身后
        else
        {
            memcpy(&stFirstPathDataTmp, pstValidBtsList, sizeof(LCT_FIRSTPATH_T));
            memcpy(&pstValidBtsList[0], &pstValidBtsList[1], sizeof(LCT_FIRSTPATH_T));
            memcpy(&pstValidBtsList[1], &stFirstPathDataTmp, sizeof(LCT_FIRSTPATH_T));
            byCounter = 1;
        }
    }

    *pbyValidBtsCounter = byCounter; // 有效基站数量，可能1，也可能2

    return;
}

// 检查预测坐标是否在一维基站对内部
// stBts1Coord:1号基站坐标
// stBts2Coord:2号基站坐标
// stPredictionCoord:预测坐标
// 返回值，0：不在内部，1：在内部
uint8 lct_coordcal_check_one_dimension_inside(COORD_T stBts1Coord, COORD_T stBts2Coord, COORD_T stPredictionCoord)
{
    uint32 dwBtsEdgeExpendLen = gg_stLctCoordcalParam.dwOneDimensionSquareExpendLen; // 一维计算是扩展
    int32 dwMinValueX = 2147483647;
    int32 dwMaxValueX = -2147483648;
    int32 dwMinValueY = 2147483647;
    int32 dwMaxValueY = -2147483648;

    // X坐标
    if (stBts1Coord.dwX > stBts2Coord.dwX)
    {
        dwMaxValueX = stBts1Coord.dwX;
        dwMinValueX = stBts2Coord.dwX;
    }
    else
    {
        dwMaxValueX = stBts2Coord.dwX;
        dwMinValueX = stBts1Coord.dwX;
    }

    // Y坐标
    if (stBts1Coord.dwY > stBts2Coord.dwY)
    {
        dwMaxValueY = stBts1Coord.dwY;
        dwMinValueY = stBts2Coord.dwY;
    }
    else
    {
        dwMaxValueY = stBts2Coord.dwY;
        dwMinValueY = stBts1Coord.dwY;
    }

    // 预测坐标在基站矩形框内(基站坐标向外扩展XX米)
    dwMinValueX -= dwBtsEdgeExpendLen;
    dwMaxValueX += dwBtsEdgeExpendLen;
    dwMinValueY -= dwBtsEdgeExpendLen;
    dwMaxValueY += dwBtsEdgeExpendLen;
    if ((stPredictionCoord.dwX >= dwMinValueX) && (stPredictionCoord.dwX <= dwMaxValueX) && (stPredictionCoord.dwY >= dwMinValueY) && (stPredictionCoord.dwY <= dwMaxValueY))
    {
        return 1;
    }

    return 0;
}

// 构建一维线性方程
// stBts1Coord:1号基站坐标
// stBts2Coord:2号基站坐标
// pstRefPointMitrix:参考点矩阵队列
// pwMitrixRow:行
// pwMitrixCol:列
// pstLinearFunc:线性方程
void lct_coordcal_build_linear_func(COORD_T stBts1Coord, COORD_T stBts2Coord, LCT_COORDCAL_LINEAR_FUNC_T *pstLinearFunc)
{
    int32 dwDeltaX = 0;
    int32 dwDeltaY = 0;
    int32 dwX = 0;
    int32 dwY = 0;
    flt32 fK = 0;
    flt32 fB = 0;
    uint8 byVerticalFlag = 0; // 0:不垂直，1：垂直于X轴，2：垂直于Y轴
    uint8 byDirection = 1;    // 1:X轴延伸，2：Y轴延伸

    // 计算坐标变量
    dwDeltaX = stBts2Coord.dwX - stBts1Coord.dwX;
    dwDeltaY = stBts2Coord.dwY - stBts1Coord.dwY;

    // 垂直于X轴的直线
    if (dwDeltaX == 0)
    {
        byVerticalFlag = 1;
        dwX = stBts1Coord.dwX;
    }
    // 垂直于Y轴的直线
    else if (dwDeltaY == 0)
    {
        byVerticalFlag = 2;
        dwY = stBts1Coord.dwY;
    }
    // 不垂直于坐标轴 y = k*x + b
    else
    {
        byVerticalFlag = 0;
        fK = (flt32)dwDeltaY / dwDeltaX;             // 斜率k
        fB = stBts1Coord.dwY - fK * stBts1Coord.dwX; // 常量b
    }

    //// 判断坐标沿哪个方向延伸，1：X轴延展，2：Y轴延展
    byDirection = abs(dwDeltaX) >= abs(dwDeltaY) ? 1 : 2;

    // 组织返回数据
    pstLinearFunc->byVerticalFlag = byVerticalFlag;
    pstLinearFunc->byDirection = byDirection;
    pstLinearFunc->dwValueCoordX = dwX; // X坐标
    pstLinearFunc->dwValueCoordY = dwY; // Y坐标
    pstLinearFunc->fK = fK;
    pstLinearFunc->fB = fB;

    return;
}

// 二次校验极小值坐标
// pstLinearFunc:线性方程系数
// pstCoord:坐标
void lct_coordcal_recal_linear_ref_point(LCT_COORDCAL_LINEAR_FUNC_T *pstLinearFunc, COORD_T *pstCoord)
{
    int32 dwTmpCoord = 0;

    //// 二次修正极小值
    // 不垂直于坐标轴
    if (pstLinearFunc->byVerticalFlag == 0)
    {
        // 沿X轴延伸
        if (pstLinearFunc->byDirection == 1)
        {
            pstCoord->dwY = util_distance_gen_rand_num(pstCoord->dwY, 50);
        }
        // 沿Y轴延伸
        else
        {
            pstCoord->dwX = util_distance_gen_rand_num(pstCoord->dwX, 50);
        }
    }
    // 垂直于X轴
    else if (pstLinearFunc->byVerticalFlag == 1)
    {
        pstCoord->dwX = util_distance_gen_rand_num(pstCoord->dwX, 50);
    }
    // 垂直于Y轴
    else if (pstLinearFunc->byVerticalFlag == 2)
    {
        pstCoord->dwY = util_distance_gen_rand_num(pstCoord->dwY, 50);
    }

    return;
}

// 选择线性参考点
// pstRefPointMatrix:参考点
// pfDelayDeviationSumMatrix:时延误差
// wMatrixRow:行
// wMatrixCol:列
// pstCoord:最优参考点
// pfDelayDeviation:累计误差
void lct_coordcal_sel_linear_ref_point(COORD_T *pstRefPointMatrix, flt32 *pfDelayDeviationSumMatrix, uint16 wMatrixRow, uint16 wMatrixCol, LCT_COORDCAL_LINEAR_FUNC_T *pstLinearFunc, COORD_T *pstLctCoord, flt32 *pfDelayDeviation)
{
    uint16 wListLen = wMatrixRow * wMatrixCol;
    flt32 fMinValue = FLT_MAX;
    uint16 wIndex = 0;
    COORD_T stSelRefPoint = {0};

    // 查找延时最小的参考点
    for (uint16 i = 0; i < wListLen; i++)
    {
        if (pfDelayDeviationSumMatrix[i] < fMinValue)
        {
            fMinValue = pfDelayDeviationSumMatrix[i];
            wIndex = i;
        }
    }

    // 二次校验线性参考定位点
    stSelRefPoint = pstRefPointMatrix[wIndex];
    lct_coordcal_recal_linear_ref_point(pstLinearFunc, &stSelRefPoint);

    // 输出参数
    *pstLctCoord = stSelRefPoint;
    *pfDelayDeviation = pfDelayDeviationSumMatrix[wIndex];

    return;
}

// 检查线性定位结果
// byMinmumValueType:极小值类型，1：计算径，2：跟踪径
// pstLinearMinmumList:线性极小值队列，返回参数
void lct_coordcal_check_linear_lct_coord_valid(uint8 byMinmumValueType, LCT_LINEAR_MINIMUM_LIST_T *pstLinearMinmumList)
{
    uint8 byMinimumListLen = 0;
    COORD_T stLctCoord = {0};
    flt32 fDelayDeviation = 0;
    LCT_FIRSTPATH_T *pstFirstBtsFirstPath = NULL;  // 线性定位的第一个站
    LCT_FIRSTPATH_T *pstSecondBtsFirstPath = NULL; // 线性定位的第二个站
    flt32 fDistance1 = 0;
    flt32 fDistance2 = 0;

    LCT_FIRSTPATH_T *pstFirstPaht = NULL;
    flt32 fDistance = 0;
    uint8 byValidFlag = 0;
    uint32 dwBottomBtsId = 0; // 所处正下方基站ID

    uint32 dwBaseBtsId1 = 0; // 所处正下方基站ID2
    uint32 dwBaseBtsId2 = 0; // 所处正下方基站ID2

    COORD_T stBottomLctCoord = {0}; // 所处正下方定位坐标
    uint32 dwMainBtsId = 0;
    uint32 dwSlaveBtsId = 0;

    LCT_LINEAR_LCT_COORD_T astLinearLctCoord[LCT_MINIMUM_NUM] = {0};
    uint8 byCounter = 0;
    COORD_T stCoord = {0};
    flt32 fDeviation = 0;
    flt32 fMinDeviation = FLT_MAX;
    uint8 byMinIndex = 0;
    flt32 fX = 0;
    flt32 fY = 0;
    flt32 fZ = 0;

    // 极小值长度
    if (byMinmumValueType == 1)
    {
        byMinimumListLen = pstLinearMinmumList->byCalMinimumListLen;
    }
    else
    {
        byMinimumListLen = pstLinearMinmumList->byTrackMinimumListLen;
    }

    // 遍历极小值队列
    for (uint16 i = 0; i < byMinimumListLen; i++)
    {
        // 组织线性定位基站数据和定位结果数据
        if (byMinmumValueType == 1)
        {
            pstFirstBtsFirstPath = &pstLinearMinmumList->astCalMinimumList[i].astFirstPathList[0];
            pstSecondBtsFirstPath = &pstLinearMinmumList->astCalMinimumList[i].astFirstPathList[1];
            stLctCoord = pstLinearMinmumList->astCalMinimumList[i].stCoord;
            fDelayDeviation = pstLinearMinmumList->astCalMinimumList[i].fDelayDeviationSum;
        }
        else
        {
            pstFirstBtsFirstPath = &pstLinearMinmumList->astTrackMinimumList[i].astFirstPathList[0];
            pstSecondBtsFirstPath = &pstLinearMinmumList->astTrackMinimumList[i].astFirstPathList[1];
            stLctCoord = pstLinearMinmumList->astTrackMinimumList[i].stCoord;
            fDelayDeviation = pstLinearMinmumList->astTrackMinimumList[i].fDelayDeviationSum;
        }
        dwBaseBtsId1 = pstFirstBtsFirstPath->dwBtsId;
        dwBaseBtsId2 = pstSecondBtsFirstPath->dwBtsId;

        // 计算定位点到基站1的距离
        fDistance1 = util_distance_cal_p2p_distance(stLctCoord.dwX, stLctCoord.dwY, pstFirstBtsFirstPath->dwCoordX, pstFirstBtsFirstPath->dwCoordY);

        // 计算定位点到基站2的距离
        fDistance2 = util_distance_cal_p2p_distance(stLctCoord.dwX, stLctCoord.dwY, pstSecondBtsFirstPath->dwCoordX, pstSecondBtsFirstPath->dwCoordY);

        if (fDistance1 < fDistance2)
        {
            pstFirstPaht = pstFirstBtsFirstPath;
            fDistance = fDistance1;
        }
        else
        {
            pstFirstPaht = pstSecondBtsFirstPath;
            fDistance = fDistance2;
        }

        // 判断参考定位点是否在基站正下方，参考定位点到基站的距离小于2.5则判定为在基站正下方
        if (fDistance <= 3)
        {
            byValidFlag = 1;
            dwBottomBtsId = pstFirstPaht->dwBtsId; // 所处正下方基站

            // 再次遍历极小值队列，判断与该基站联合进行一维定位的结果是否也在该基站正下方
            for (uint8 k = 0; k < byMinimumListLen; k++)
            {
                if (byMinmumValueType == 1)
                {
                    dwMainBtsId = pstLinearMinmumList->astCalMinimumList[k].astFirstPathList[0].dwBtsId;
                    dwSlaveBtsId = pstLinearMinmumList->astCalMinimumList[k].astFirstPathList[1].dwBtsId;
                    stBottomLctCoord = pstLinearMinmumList->astCalMinimumList[k].stCoord;
                }
                else
                {
                    dwMainBtsId = pstLinearMinmumList->astTrackMinimumList[k].astFirstPathList[0].dwBtsId;
                    dwSlaveBtsId = pstLinearMinmumList->astTrackMinimumList[k].astFirstPathList[1].dwBtsId;
                    stBottomLctCoord = pstLinearMinmumList->astTrackMinimumList[k].stCoord;
                }

                // 排除产生定位参考点的一对基站
                if ((dwMainBtsId == dwBaseBtsId1 || dwMainBtsId == dwBaseBtsId2) && (dwSlaveBtsId == dwBaseBtsId1 || dwSlaveBtsId == dwBaseBtsId2))
                { // 自身
                }
                else
                {
                    if (dwMainBtsId == dwBottomBtsId || (dwSlaveBtsId == dwBottomBtsId))
                    {
                        fDistance = util_distance_cal_p2p_distance(stBottomLctCoord.dwX, stBottomLctCoord.dwY, pstFirstPaht->dwCoordX, pstFirstPaht->dwCoordY);

                        // 该参考定位点不在基站正下方，则说明定位点不合法
                        if (fDistance > 3)
                        {
                            byValidFlag = 0;
                            break;
                        }
                    }
                }
            }
        }
        else
        {
            byValidFlag = 1;
        }

        // 该点定位有效，则保存该定位坐标
        if (byValidFlag)
        {
            astLinearLctCoord[byCounter].stCoord = stLctCoord;
            astLinearLctCoord[byCounter].fDelayDeviation = fDelayDeviation;
            byCounter++;
        }
    }

    // 存在多个极小值，则计算均值
    if (byCounter > 1)
    {
        // 求均值
        for (uint8 l = 0; l < byCounter; l++)
        {
            fDeviation += astLinearLctCoord[l].fDelayDeviation / byCounter;
            fX += astLinearLctCoord[l].stCoord.dwX / (flt32)byCounter;
            fY += astLinearLctCoord[l].stCoord.dwY / (flt32)byCounter;
        }

        astLinearLctCoord[0].stCoord.dwX = (int32)fX;
        astLinearLctCoord[0].stCoord.dwY = (int32)fY;
        astLinearLctCoord[0].fDelayDeviation = fDeviation;
        byMinIndex = 0;

        byCounter = 1;
    }

    // 计算径
    if (byMinmumValueType == 1)
    {
        pstLinearMinmumList->byCalMinimumListLen = byCounter;
        pstLinearMinmumList->astCalMinimumList[0].stCoord = astLinearLctCoord[byMinIndex].stCoord;
        pstLinearMinmumList->astCalMinimumList[0].fDelayDeviationSum = astLinearLctCoord[byMinIndex].fDelayDeviation;
    }
    else
    {
        pstLinearMinmumList->byTrackMinimumListLen = byCounter;
        pstLinearMinmumList->astTrackMinimumList[0].stCoord = astLinearLctCoord[byMinIndex].stCoord;
        pstLinearMinmumList->astTrackMinimumList[0].fDelayDeviationSum = astLinearLctCoord[byMinIndex].fDelayDeviation;
    }

    return;
}

// 构建线性定位参考点
// byPredictionFlag:负责坐标标识符，0：失败，1：成功
// stPredictionCoord:预测坐标
// stFirstCoord:第一个坐标
// stSecondCoord:第二个点坐标
// pstLinearFunc:线性方程
// psRefPoint:参考点（返回参数）
// pwMatrixRow:行（返回参数）
// pwMatrixCol:列（返回参数）
void lct_coordcal_build_linear_ref_point(uint8 byPredictionFlag, COORD_T stPredictionCoord, COORD_T stFirstCoord, COORD_T stSecondCoord, LCT_COORDCAL_LINEAR_FUNC_T *pstLinearFunc, COORD_T *pstRefPoint, uint16 *pwMatrixRow, uint16 *pwMatrixCol)
{
    uint32 dwTrackCoordExpendLen = gg_stLctCoordcalParam.dwTrackCoordExpendLen;     // 跟踪状态下矩形框边长扩展长度
    uint32 dwOneDimensionExpendLen = gg_stLctCoordcalParam.dwOneDimensionExpendLen; // 一维定位时向外扩展长度
    uint8 byDistance = gg_stLctCoordcalParam.dwIntervalDistance;                    // 初始化状态下参考点间间隔
    uint16 wRows = 0;                                                               // 行数
    uint16 wCols = 0;                                                               // 列数
    int32 dwMinX = 0;
    int32 dwMaxX = 0;
    int32 dwMinY = 0;
    int32 dwMaxY = 0;

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

    // 跟踪搜获框边界
    int32 dwPreMinX = 0;
    int32 dwPreMinY = 0;
    int32 dwPreMaxX = 0;
    int32 dwPreMaxY = 0;

    uint16 wCounter = 0; // 参考定位点数量

    // 被除数容错处理
    if (byDistance <= 0)
    {
        byDistance = 50;
    }

    // 计算预测搜索框边界
    if (byPredictionFlag)
    {
        dwPreMinX = stPredictionCoord.dwX - dwTrackCoordExpendLen;
        dwPreMaxX = stPredictionCoord.dwX + dwTrackCoordExpendLen;

        dwPreMinY = stPredictionCoord.dwY - dwTrackCoordExpendLen;
        dwPreMaxY = stPredictionCoord.dwY + dwTrackCoordExpendLen;
    }

    // 计算行数
    if (stFirstCoord.dwY >= stSecondCoord.dwY)
    {
        dwMinY = stSecondCoord.dwY - dwOneDimensionExpendLen;
        dwMaxY = stFirstCoord.dwY + dwOneDimensionExpendLen;
        wRows = (dwMaxY - dwMinY) / byDistance; // 行代表Y坐标
    }
    else
    {
        dwMinY = stFirstCoord.dwY - dwOneDimensionExpendLen;
        dwMaxY = stSecondCoord.dwY + dwOneDimensionExpendLen;
        wRows = (dwMaxY - dwMinY) / byDistance; // 行代表Y坐标
    }

    // 计算列数
    if (stFirstCoord.dwX >= stSecondCoord.dwX)
    {
        dwMinX = stSecondCoord.dwX - dwOneDimensionExpendLen;
        dwMaxX = stFirstCoord.dwX + dwOneDimensionExpendLen;
        wCols = (dwMaxX - dwMinX) / byDistance; // 列代表X坐标
    }
    else
    {
        dwMinX = stFirstCoord.dwX - dwOneDimensionExpendLen;
        dwMaxX = stSecondCoord.dwX + dwOneDimensionExpendLen;
        wCols = (dwMaxX - dwMinX) / byDistance; // 列代表X坐标
    }

    //// 计算参考点坐标
    // 不垂直于坐标轴
    if (pstLinearFunc->byVerticalFlag == 0)
    {
        for (uint16 j = 0; j < wCols; j++)
        {
            if (!byPredictionFlag)
            {
                stCoord.dwX = dwMinX + byDistance * j;                             // X坐标
                stCoord.dwY = pstLinearFunc->fK * stCoord.dwX + pstLinearFunc->fB; // Y坐标
                pstRefPoint[wCounter] = stCoord;                                   // 参考点坐标
                wCounter++;
            }
            else
            {
                stCoord.dwX = dwMinX + byDistance * j;                             // X坐标
                stCoord.dwY = pstLinearFunc->fK * stCoord.dwX + pstLinearFunc->fB; // Y坐标
                if ((stCoord.dwX >= dwPreMinX && stCoord.dwX <= dwPreMaxX) && (stCoord.dwY >= dwPreMinY && stCoord.dwY <= dwPreMaxY))
                {
                    pstRefPoint[wCounter] = stCoord; // 参考点坐标
                    wCounter++;
                }
            }
        }

        wRows = 1;
        wCols = wCounter;
    }
    // 垂直于X轴
    else if (pstLinearFunc->byVerticalFlag == 1)
    {
        stCoord.dwX = stFirstCoord.dwX;
        for (uint16 i = 0; i < wRows; i++)
        {
            if (!byPredictionFlag)
            {
                stCoord.dwY = dwMinY + byDistance * i; // Y坐标
                pstRefPoint[wCounter] = stCoord;       // 参考点坐标
                wCounter++;
            }
            else
            {
                stCoord.dwY = dwMinY + byDistance * i; // Y坐标
                if (stCoord.dwY >= dwPreMinY && stCoord.dwY <= dwPreMaxY)
                {
                    pstRefPoint[wCounter] = stCoord; // 参考点坐标
                    wCounter++;
                }
            }
        }
        wRows = wCounter;
        wCols = 1;
    }
    // 垂直于Y轴
    else if (pstLinearFunc->byVerticalFlag == 2)
    {

        stCoord.dwY = stFirstCoord.dwY;
        for (uint16 i = 0; i < wCols; i++)
        {
            if (!byPredictionFlag)
            {
                stCoord.dwX = dwMinX + byDistance * i; // X坐标
                pstRefPoint[wCounter] = stCoord;       // 参考点坐标
                wCounter++;
            }
            else
            {
                stCoord.dwX = dwMinX + byDistance * i; // X坐标
                if (stCoord.dwX >= dwPreMinX && stCoord.dwX <= dwPreMaxX)
                {
                    pstRefPoint[wCounter] = stCoord; // 参考点坐标
                    wCounter++;
                }
            }
        }

        wRows = 1;
        wCols = wCounter;
    }

    *pwMatrixRow = wRows; // 行
    *pwMatrixCol = wCols; // 列

    return;
}

// 0维计算（在一维定位中只有一个基站有效）
// pstLctDataList:定位数据队列
// pstValidBtsData:有效基站数据
// byFirstPathFlag:第一径标识符，0：计算径，1：跟踪径
// pstLinearFunc:线性方程参数
// pstRefPoint:选择参考点
// 返回值:0：失败，1：成功
uint8 lct_coordcal_zero_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPATH_T *pstValidBtsData, uint8 byFirstPathFlag, LCT_COORDCAL_LINEAR_FUNC_T *pstLinearFunc, COORD_T *pstRefPoint)
{
    PUB_LOCATION_DATA_T *pstLctData = NULL;
    COORD_T stAheadCoord = {0};
    uint8 byFreqIndex = 0;
    int16 wIndexGap = 0;
    uint8 byValidFlag = 0;
    flt32 fOffsetDistance = 0;  // 前后两点偏移距离
    flt32 fOffsetDistanceX = 0; // X轴偏移距离
    flt32 fOffsetDistanceY = 0; // X轴偏移距离
    flt32 fFactor = 0;
    uint8 byResult = 0;

    // 平滑坐标队列中有数据
    if (gLct_stMeanLctCoordList.byListLen > 0)
    {
        stAheadCoord = gLct_stMeanLctCoordList.astCoordList[gLct_stMeanLctCoordList.byListLen - 1];

        // 该频点定位数据
        byFreqIndex = pstValidBtsData->byFreqIndex;
        pstLctData = &pstLctDataList[byFreqIndex];

        // 计算径偏移量
        if (byFirstPathFlag == CAL_PATH)
        {
            if (pstLctData->wAheadFrameCalPahtIndex > 0)
            {
                wIndexGap = pstValidBtsData->wFrameCalIndex - pstLctData->wAheadFrameCalPahtIndex;
                byValidFlag = 1;
            }
        }
        // 跟踪径偏移量
        else
        {
            if (pstLctData->wAheadFrameTrackPathIndex > 0)
            {
                wIndexGap = pstValidBtsData->wFrameTrackIndex - pstLctData->wAheadFrameTrackPathIndex;
                byValidFlag = 1;
            }
        }

        // 第一径偏移量有效
        if (byValidFlag)
        {
            // 偏移距离，可正也可负
            fOffsetDistance = abs(wIndexGap) * 0.7;
            if (fOffsetDistance > 250 || fOffsetDistance < 120)
            {
                fOffsetDistance = 185;
            }

            // 轴速度不为0
            if (gLct_dwSpeedY != 0 && gLct_dwSpeedX != 0)
            {
                fFactor = abs(gLct_dwSpeedY) / abs(gLct_dwSpeedX);
                fOffsetDistance = fOffsetDistance * fOffsetDistance;
                fOffsetDistance /= fFactor * fFactor + 1;

                fOffsetDistanceX = fOffsetDistance;
                fOffsetDistanceY = fOffsetDistance * fFactor;
            }
            else if (gLct_dwSpeedX == 0)
            {
                fOffsetDistanceX = 0;
                fOffsetDistanceY = fOffsetDistance;
            }
            else if (gLct_dwSpeedY == 0)
            {
                fOffsetDistanceX = fOffsetDistance;
                fOffsetDistanceY = 0;
            }

            //// 根据线性方程计算X轴和Y轴坐标
            // 不垂直于坐标轴
            if (pstLinearFunc->byVerticalFlag == 0)
            {
                // X轴轴距大
                if (pstLinearFunc->byDirection == 1)
                {
                    stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistanceX; // 移动距离是Y轴的3倍
                    stAheadCoord.dwY = pstLinearFunc->fK * stAheadCoord.dwX + pstLinearFunc->fB;
                }
                // Y轴轴距大
                else
                {
                    stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistanceY; // 移动距离是X轴的3倍
                    stAheadCoord.dwX = (int32)(stAheadCoord.dwY - pstLinearFunc->fB) / pstLinearFunc->fK;
                }
            }
            // 垂直于X坐标轴
            else if (pstLinearFunc->byVerticalFlag == 1)
            {
                stAheadCoord.dwX = pstLinearFunc->dwValueCoordX;
                stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistanceY;
            }
            // 垂直于Y坐标轴
            else if (pstLinearFunc->byVerticalFlag == 2)
            {
                stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistanceX;
                stAheadCoord.dwY = pstLinearFunc->dwValueCoordY;
            }
        }
        // 第一径偏移量无效
        else
        {
            // 计算定位点坐标
            if (gLct_byDirectionX != 0 && gLct_byDirectionY != 0)
            {
                stAheadCoord.dwX += gLct_dwSpeedX;
                stAheadCoord.dwY += gLct_dwSpeedY;
            }
            else if (gLct_byDirectionX != 0)
            {
                stAheadCoord.dwX += gLct_dwSpeedX;
            }
            else if (gLct_byDirectionY != 0)
            {
                stAheadCoord.dwY += gLct_dwSpeedY;
            }
        }

        // 保存数据
        memcpy(pstRefPoint, &stAheadCoord, sizeof(COORD_T));
        byResult = 1;
    }

    return byResult;
}

// 0维中修改从站第一径帧内索引
void lct_coordcal_adjust_first_path_index(PUB_LOCATION_DATA_T *pstLctDataList, uint8 byFirstPathFlag, COORD_T *pstLctCoord, LCT_FIRSTPATH_T *pstMainBtsFirstPath, LCT_FIRSTPATH_T *pstSlaveBtsFirstPath)
{
    flt32 fMainBtsDist = 0;
    flt32 fSlaveBtsDist = 0;
    flt32 fTimeDelay = 0;
    int32 dwIndexDiff = 0;           // 相位差
    int32 dwSlaveFirstPahtIndex = 0; // 从站第一径
    uint8 byFreqIndex = 0;
    PUB_LOCATION_DATA_T *pstLctData = NULL;

    // 计算定位坐标到基站的距离
    fMainBtsDist = util_distance_cal_p2p_distance(pstLctCoord->dwX, pstLctCoord->dwY, pstMainBtsFirstPath->dwCoordX, pstMainBtsFirstPath->dwCoordY);
    fSlaveBtsDist = util_distance_cal_p2p_distance(pstLctCoord->dwX, pstLctCoord->dwY, pstSlaveBtsFirstPath->dwCoordX, pstSlaveBtsFirstPath->dwCoordY);

    // 基站从站帧内索引
    fTimeDelay = ((fMainBtsDist - fSlaveBtsDist) / 343) * 1000 * 1000; // 单位微妙
    dwIndexDiff = (fTimeDelay * 48) / 1000;                            // 相位差

    // 修正从站第一径索引
    byFreqIndex = pstSlaveBtsFirstPath->byFreqIndex;
    pstLctData = &pstLctDataList[byFreqIndex];
    if (byFirstPathFlag == CAL_PATH)
    {
        dwSlaveFirstPahtIndex = pstMainBtsFirstPath->dwCalPathIndex - dwIndexDiff;
        pstSlaveBtsFirstPath->wFrameCalIndex = dwSlaveFirstPahtIndex - pstLctData->dwSignalSlotSyncOffset - pstLctData->dwSignalSyncIndex;
    }
    else
    {
        dwSlaveFirstPahtIndex = pstMainBtsFirstPath->dwTrackPathIndex - dwIndexDiff;
        pstSlaveBtsFirstPath->wFrameTrackIndex = dwSlaveFirstPahtIndex - pstLctData->dwSignalSlotSyncOffset - pstLctData->dwSignalSyncIndex;
    }
}

// 单基站定位（只有一个基站）
// pstLctDataList:定位数据队列
// pstValidBtsData:有效基站数据
// byFirstPathFlag:第一径标识符，0：计算径，1：跟踪径
// pstRefPoint:选择参考点
// 返回值:0：失败，1：成功
uint8 lct_coordcal_single_bts_location(PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPATH_T *pstValidBtsData, uint8 byFirstPathFlag, COORD_T *pstRefPoint)
{
    uint8 byFreqIndex = 0;
    PUB_LOCATION_DATA_T *pstLctData = NULL;

    int16 wIndexGap = 0;
    uint8 byValidFlag = 0;
    flt32 fOffsetDistance = 0; // 前后两点偏移距离
    uint8 byMoveDirection = 0; // 1:X轴，2：Y轴
    COORD_T stAheadCoord = {0};
    uint8 byResult = 0;

    // 平滑坐标队列中至少有2个点
    if (gLct_stMeanLctCoordList.byListLen > 1)
    {
        // 前一个点坐标
        stAheadCoord = gLct_stMeanLctCoordList.astCoordList[gLct_stMeanLctCoordList.byListLen - 1];

        // 判断运动距离大的方向，1:X轴，2：Y轴
        byMoveDirection = abs(gLct_dwSpeedX) > abs(gLct_dwSpeedY) ? 1 : 2;

        // 频点定位数据
        byFreqIndex = pstValidBtsData->byFreqIndex;
        pstLctData = &pstLctDataList[byFreqIndex];

        // 计算径偏移量
        if (byFirstPathFlag == CAL_PATH)
        {
            if (pstLctData->wAheadFrameCalPahtIndex > 0)
            {
                wIndexGap = pstValidBtsData->wFrameCalIndex - pstLctData->wAheadFrameCalPahtIndex;
                byValidFlag = 1;
            }
        }
        // 跟踪径偏移量
        else
        {
            if (pstLctData->wAheadFrameTrackPathIndex > 0)
            {
                wIndexGap = pstValidBtsData->wFrameTrackIndex - pstLctData->wAheadFrameTrackPathIndex;
                byValidFlag = 1;
            }
        }

        // 第一径偏移量有效
        if (byValidFlag)
        {
            fOffsetDistance = abs(wIndexGap) * 0.7; // 偏移距离，可正也可负
            if (fOffsetDistance > 250 || fOffsetDistance < 150)
            {
                fOffsetDistance = 200;
            }

            // 计算定位点坐标
            if (gLct_byDirectionX != 0 && gLct_byDirectionY != 0)
            {
                fOffsetDistance = fOffsetDistance * fOffsetDistance;
                fOffsetDistance /= 10;
                fOffsetDistance = sqrt((flt64)fOffsetDistance);

                // X轴移动距离大
                if (byMoveDirection == 1)
                {
                    stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistance * 3; // 移动距离是Y轴的3倍
                    stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistance;
                }
                // Y轴移动距离大
                else
                {
                    stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistance;
                    stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistance * 3; // 移动距离是X轴的3倍
                }
            }
            else if (gLct_byDirectionX != 0)
            {
                stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistance;
            }
            else if (gLct_byDirectionY != 0)
            {
                stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistance;
            }
        }
        // 第一径偏移量无效
        else
        {
            // 计算定位点坐标
            if (gLct_byDirectionX != 0 && gLct_byDirectionY != 0)
            {
                stAheadCoord.dwX += gLct_dwSpeedX;
                stAheadCoord.dwY += gLct_dwSpeedY;
            }
            else if (gLct_byDirectionX != 0)
            {
                stAheadCoord.dwX += gLct_dwSpeedX;
            }
            else if (gLct_byDirectionY != 0)
            {
                stAheadCoord.dwY += gLct_dwSpeedY;
            }
        }

        // 保存数据
        memcpy(pstRefPoint, &stAheadCoord, sizeof(COORD_T));
        byResult = 1;
    }

    return byResult;
}