// 秒寻科技
// TOA模块
// zt
// 2023-02-24

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

extern uint32 gg_dwFrameNo; // 全局帧号

// 根据距离计算所有基站的第一径偏移量
// byFirstPathFlag:第一径类型，计算径或者跟踪径
// pstFirstPathList:第一径队列
// pstLctCoord:当前定位坐标
// pstToaFpOffsetList:第一径偏移量队列，输入输出参数
void lct_toa_cal_bts_first_path_index_offset(uint8 byFirstPathFlag, LCT_FIRSTPATH_LIST_T *pstFirstPathList, COORD_T *pstLctCoord, LCT_TOA_FP_OFFSET_LIST_T *pstToaFpOffsetList)
{
    uint8 byToaFpOffsetListLen = pstToaFpOffsetList->byListLen;
    LCT_TOA_FP_OFFSET_T stToaFpOffsetData = {0}; // 第一径偏移量
    LCT_TOA_FP_OFFSET_T *pstToaFpOffsetData = NULL;
    uint32 dwMaxFrameNo = 0;
    uint32 dwMinFrameNo = 0;
    int32 dwFrameGap = 0;                // 帧间距
    uint32 dwFrameNo = gg_dwFrameNo - 1; // 帧号
    int32 dwFpOffsetGap = 0;             // 前后两次偏移量间距

    uint8 byFirstPathNum = pstFirstPathList->byBtsNum;
    LCT_FIRSTPATH_T *pstFirstPathData = NULL;
    COORD_T stBtsCoord = {0};            // 基站坐标
    uint32 dwFirstPahtIndexDistance = 0; // 计算绝对距离计算的第一径索引
    uint32 dwOffset = 0;                 // 基站偏移量
    uint32 dwOffsetSum = 0;              // 累计偏移量
    uint32 dwCurrentOffset = 0;          // 当前偏移量

    // 参数容错
    if (byFirstPathNum <= 0)
    {
        return;
    }

    // 计算当前偏移量
    if (byToaFpOffsetListLen >= LCT_TOA_FP_OFFSET_LIST_MAX_LEN)
    {
        dwMaxFrameNo = pstToaFpOffsetList->astToaFpOffsetList[byToaFpOffsetListLen - 1].dwFrameNo;
        dwMinFrameNo = pstToaFpOffsetList->astToaFpOffsetList[0].dwFrameNo;
        dwFrameGap = dwMaxFrameNo - dwMinFrameNo; // 帧间距

        // 帧间距合法
        if (dwFrameGap == (PUB_SYS_SLOT_NUM * (LCT_TOA_FP_OFFSET_LIST_MAX_LEN - 1)))
        {
            for (uint8 j = 0; j < byToaFpOffsetListLen; j++)
            {
                pstToaFpOffsetData = &pstToaFpOffsetList->astToaFpOffsetList[j];
                dwOffsetSum += pstToaFpOffsetData->dwSecondAvarageOffset;
            }
            dwCurrentOffset = dwOffsetSum / byToaFpOffsetListLen; // 当前一秒偏移量

            // 保存上一次偏移量数据
            pstToaFpOffsetList->dwAheadFrameNo = pstToaFpOffsetList->dwCurrentFrameNo;
            pstToaFpOffsetList->dwAheadOffset = pstToaFpOffsetList->dwCurrentOffset;

            // 保存当前偏移量数据
            pstToaFpOffsetList->dwCurrentFrameNo = dwFrameNo;
            pstToaFpOffsetList->dwCurrentOffset = dwCurrentOffset;

            // 判断当前偏移量是否有效
            dwFpOffsetGap = dwCurrentOffset - pstToaFpOffsetList->dwAheadOffset;
            dwFpOffsetGap = abs(dwFpOffsetGap);
            if (dwFpOffsetGap <= LCT_TOA_FP_OFFSET_VALID_THRES)
            {
                pstToaFpOffsetList->byOffsetValidFlag = 1; // 有效
            }
            else
            {
                pstToaFpOffsetList->byOffsetValidFlag = 0; // 无效
            }
        }
        else
        {
            pstToaFpOffsetList->byOffsetValidFlag = 0;
        }

        pstToaFpOffsetList->byListLen = 0;
        byToaFpOffsetListLen = pstToaFpOffsetList->byListLen;
    }

    // 遍历第一径队列，计算第一径偏移量
    dwOffsetSum = 0;
    for (uint8 i = 0; i < byFirstPathNum; i++)
    {
        pstFirstPathData = &pstFirstPathList->astFirstPath[i];

        // 基于距离计算第一径索引值
        stBtsCoord.dwX = pstFirstPathData->dwCoordX;
        stBtsCoord.dwY = pstFirstPathData->dwCoordY;
        stBtsCoord.dwZ = pstFirstPathData->dwCoordZ;
        dwFirstPahtIndexDistance = lct_toa_cal_one_bts_first_path_value(&stBtsCoord, pstLctCoord); // 距离第一径绝对值

        // 手机到基站间的距离距离有效
        if (byFirstPathFlag == CAL_PATH)
        {
            dwOffset = pstFirstPathData->dwCalPathIndex - dwFirstPahtIndexDistance; // 信号第一径相对值与距离第一径绝对值之间的偏移量
        }
        else
        {
            dwOffset = pstFirstPathData->dwTrackPathIndex - dwFirstPahtIndexDistance; // 信号第一径相对值与距离第一径绝对值之间的偏移量
        }

        dwOffsetSum += dwOffset;
    }

    // 计算本次平均偏移量
    stToaFpOffsetData.dwSecondAvarageOffset = dwOffsetSum / byFirstPathNum;           // 当前一秒基站平均偏移量
    stToaFpOffsetData.dwFrameNo = dwFrameNo;                                          // 帧号
    pstToaFpOffsetList->astToaFpOffsetList[byToaFpOffsetListLen] = stToaFpOffsetData; // 保存当前一秒偏移量
    pstToaFpOffsetList->byListLen++;                                                  // 队列长度加1

    return;
}

// 基于距离计算计算的第一径绝对值
// pstBtsCoord:基站坐标
// pstAheadCoord:上一个定位点坐标
uint32 lct_toa_cal_one_bts_first_path_value(COORD_T *pstBtsCoord, COORD_T *pstAheadCoord)
{
    uint32 dwFirstPathIndex = 0;
    flt32 fDistance = 0;
    uint32 dwXX = (pstBtsCoord->dwX - pstAheadCoord->dwX) * (pstBtsCoord->dwX - pstAheadCoord->dwX);
    uint32 dwYY = (pstBtsCoord->dwY - pstAheadCoord->dwY) * (pstBtsCoord->dwY - pstAheadCoord->dwY);
    uint32 dwZZ = (pstBtsCoord->dwZ - pstAheadCoord->dwZ) * (pstBtsCoord->dwZ - pstAheadCoord->dwZ);
    uint32 dwSum = dwXX + dwYY + dwZZ;
    fDistance = sqrt((flt64)dwSum) / 100;

    dwFirstPathIndex = (uint32)(fDistance * PUB_SIGNAL_SAMPLE_RATIO / PUB_AUDIO_SPEED);

    return dwFirstPathIndex;
}
