// 秒寻科技
// 预测坐标模块
// zt
// 2025-02-24

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

// 导入全局变量
extern PUB_SYS_STATUS_T gg_stSysStatus;
extern LCT_COORDSEL_MEAN_COORD_T gLct_stMeanLctCoordList;
extern LCT_COORDSEL_ORIGINAL_COORD_T gLct_stOriginalLctCoordList;
extern LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T gLct_astPredictionDirectionDataList[PUB_SYS_CARRIER_NUM];

// 根据历史坐标，预测当前一秒的坐标
// dwBulidId:大楼编号
// dwLayerId:楼层编号
// pstPredictionCoord:预测坐标（返回参数）
// pbyPredictionFlag:预测标识（返回参数），0：预测失败，1：预测成功
// pstAheadPointCoord:前一个点坐标（返回参数）
// pdwSpeedX:x轴位移速度（返回参数）
// pdwSpeedY:y轴位移速度（返回参数）
// pbyChangeXY:X轴或Y轴变化大小趋势（返回参数），1：X轴变化大，2：Y轴变化大
void lct_prediction_current_coord_by_history(int32 dwBulidId, int32 dwLayerId, COORD_T *pstPredictionCoord, uint8 *pbyPredictionFlag, COORD_T *pstAheadPointCoord, int32 *pdwSpeedX, int32 *pdwSpeedY, uint8 *pbyChangeXY)
{
    LCT_COORDSEL_MEAN_COORD_T *pstData = (LCT_COORDSEL_MEAN_COORD_T *)&gLct_stMeanLctCoordList; // 平滑定位点队列
    uint8 byMeanListLen = pstData->byListLen;                                                   // 平滑定位点队列长度

    LCT_COORDSEL_ORIGINAL_COORD_T *pstOriginalLctCoordList = &gLct_stOriginalLctCoordList; // 原始定位点队列
    uint8 byOriginalListLen = pstOriginalLctCoordList->byListLen;                          // 原始定位点队列长度

    COORD_T stPrediction = {0};
    COORD_T *pstBaseCoord = NULL;
    uint8 byChangeXY = 0; // X轴或Y轴变化大，1：X轴变化大，2：Y轴变化大

    // 处于同一大楼和楼层
    if (dwBulidId == gg_stSysStatus.dwBuildId && dwLayerId == gg_stSysStatus.dwLayerId)
    {
        // 系统处于重定位阶段
        if (gg_stSysStatus.byReLocationFlag)
        {
            memset(&gLct_stOriginalLctCoordList, 0, sizeof(LCT_COORDSEL_ORIGINAL_COORD_T)); // 原始坐标队列复位
            memset(&gLct_stMeanLctCoordList, 0, sizeof(LCT_COORDSEL_MEAN_COORD_T));         // 平滑坐标队列复位
            gg_stSysStatus.byBtsCaptureFlag = 0;                                            // 系统进入基站捕获状态

            // 预测失败
            *pbyPredictionFlag = 0;
            return;
        }

        // 平滑队列中的定位点不够
        if (byMeanListLen < LCT_COORDSEL_MEAN_COORD_LIST_LEN)
        {
            // 预测失败
            *pbyPredictionFlag = 0;
            return;
        }

        // 计算基础速度
        // 最近两次位移的加权平均（3:1权重）
        int32 dx = (3 * (pstData->astCoordList[3].dwX - pstData->astCoordList[2].dwX) + (pstData->astCoordList[2].dwX - pstData->astCoordList[1].dwX)) / 4;
        int32 dy = (3 * (pstData->astCoordList[3].dwY - pstData->astCoordList[2].dwY) + (pstData->astCoordList[2].dwY - pstData->astCoordList[1].dwY)) / 4;

        // 计算加速度
        // 计算二阶差分（加速度）
        int32 ddx = (pstData->astCoordList[3].dwX - 2 * pstData->astCoordList[2].dwX + pstData->astCoordList[1].dwX);
        int32 ddy = (pstData->astCoordList[3].dwY - 2 * pstData->astCoordList[2].dwY + pstData->astCoordList[1].dwY);

        // 计算预测坐标（包括加速度补偿）
        stPrediction.dwX = pstData->astCoordList[3].dwX + dx + ddx / 2; // 加速度补偿
        stPrediction.dwY = pstData->astCoordList[3].dwY + dy + ddy / 2; // 加速度补偿
        stPrediction.dwZ = 120;

        // 在X轴和Y轴上的位移
        *pdwSpeedX = (int32)dx + ddx / 2; // 位移速度
        *pdwSpeedY = (int32)dy + ddy / 2; // 位移速度

        // 保存预测坐标
        *pstPredictionCoord = stPrediction;

        // 保存基准点坐标
        pstBaseCoord = &pstData->astCoordList[byMeanListLen - 3];

        // 保存上一个定位点坐标
        *pstAheadPointCoord = pstData->astCoordList[byMeanListLen - 1];

        // 预测成功
        *pbyPredictionFlag = 1;

        // 坐标轴变化大小趋势
        *pbyChangeXY = abs(pstAheadPointCoord->dwX - pstBaseCoord->dwX) > abs(pstAheadPointCoord->dwY - pstBaseCoord->dwY) ? 1 : 2;
    }
    // 不在同一大楼和楼层，系统进入基站捕获状态
    else
    {
        memset(&gLct_stOriginalLctCoordList, 0, sizeof(LCT_COORDSEL_ORIGINAL_COORD_T)); // 原始坐标队列复位
        memset(&gLct_stMeanLctCoordList, 0, sizeof(LCT_COORDSEL_MEAN_COORD_T));         // 平滑坐标队列复位
        gg_stSysStatus.byBtsCaptureFlag = 0;                                            // 系统进入基站捕获状态

        // 预测失败
        *pbyPredictionFlag = 0;
    }

    return;
}

// 预测手机相对于各基站的移动趋势，包括是远离、靠近和不确定三种趋势，1：靠近，2：远离，0：不确定
// pstPredictionDirectionDataList:预测方向的数据队列
void lct_prediction_prediction_phone_move_tendency(LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList)
{
    LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionData = NULL; // 预测移动方向的数据

    uint8 byLen = 0;
    LCT_FIRSTPATH_BACKUP_T *pstCurrentData = NULL; // 当前数据
    LCT_FIRSTPATH_BACKUP_T *pstAheadData = NULL;   // 前一帧数据
    uint32 dwFrameGap = 0;                         // 帧间隔，间隔为8方才合法

    // 运动趋势，1：靠近，2：远离，0：趋势不一致
    uint8 byAheadTendency = 0;   // 前一次运动趋势
    uint8 byCurrentTendency = 0; // 当前运动趋势

    for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++)
    {
        pstPredictionData = &pstPredictionDirectionDataList[i];   // 预测移动方向的数据
        byAheadTendency = pstPredictionData->byDirectionTendency; // 上一次趋势
        pstPredictionData->byDirectionTendency = 0;               // 方向趋势复位

        // 遍历方向数据队列
        byLen = pstPredictionData->dwFirstPathBackupIndex;
        if (byLen >= LCT_FIRST_PATH_BACKUP_NUM)
        {
            pstCurrentData = &pstPredictionData->astFirstPathBackupList[byLen - 1];
            pstAheadData = pstPredictionData->astFirstPathBackupList;
            dwFrameGap = pstCurrentData->dwFrameNo - pstAheadData->dwFrameNo;

            if (dwFrameGap == 8 && pstPredictionData->byValidFlag) // 数据有效
            {
                // “第一径维度”靠近该基站
                if ((pstCurrentData->dwCalPathIndex <= pstAheadData->dwCalPathIndex) || (pstCurrentData->dwTrackPathIndex <= pstAheadData->dwTrackPathIndex))
                {
                    pstPredictionData->byDirectionTendency = 1; // 靠近

                    // 上一次是靠近该基站
                    if (byAheadTendency == 1)
                    {
                        // 前一秒是面向基站，当前一秒从基站下方走过变成背向基站，幅度快速下降
                        if (pstCurrentData->fMaxAmplValue < (0.65 * pstAheadData->fMaxAmplValue))
                        {
                            pstPredictionData->byDirectionTendency = 2; // 远离
                        }
                    }
                    // 上一次是远离该基站
                    else if (byAheadTendency == 2)
                    {
                        // 前一秒是背向远离基站，当前一秒是面向靠近基站（掉头或者转弯），幅度快速增加
                        if (pstCurrentData->fMaxAmplValue > (1.35 * pstAheadData->fMaxAmplValue))
                        {
                            pstPredictionData->byDirectionTendency = 1; // 靠近
                        }
                        else
                        {
                            pstPredictionData->byDirectionTendency = 0; // 方向不确定
                        }
                    }
                    // 上一次趋势不确定
                    else
                    {
                        // 第一径是靠近趋势，幅度也是靠近趋势
                        if (pstCurrentData->fMaxAmplValue > pstAheadData->fMaxAmplValue)
                        {
                            pstPredictionData->byDirectionTendency = 1; // 靠近
                        }
                        else
                        {
                            pstPredictionData->byDirectionTendency = 0; // 方向不确定
                        }
                    }
                }
                // "第一径维度"远离该基站
                else
                {
                    pstPredictionData->byDirectionTendency = 2; // 远离

                    // 前一秒是面向靠近基站，当前第一径远离背向基站，相当于进行了一次掉头或者拐弯，幅度会快速下降
                    if (byAheadTendency == 1)
                    {
                        // 幅度变化剧烈，掉头或者拐弯
                        if (pstCurrentData->fMaxAmplValue < (0.6 * pstAheadData->fMaxAmplValue))
                        {
                            pstPredictionData->byDirectionTendency = 2; // 远离
                        }
                        else
                        {
                            pstPredictionData->byDirectionTendency = 0; // 方向不确定
                        }
                    }
                    else if (byAheadTendency == 2)
                    {
                        pstPredictionData->byDirectionTendency = 2; // 远离
                    }
                    else
                    {
                        // 第一径远离，幅度远离
                        if (pstCurrentData->fMaxAmplValue < pstAheadData->fMaxAmplValue)
                        {
                            pstPredictionData->byDirectionTendency = 2; // 远离
                        }
                        else
                        {
                            pstPredictionData->byDirectionTendency = 0; // 方向不确定
                        }
                    }
                }
            }
        }
    }
}

// 预测手机在X/Y轴移动方向
// pstPredictionDirectionDataList:预测移动方向的数据队列
// pbyDirectionX:X轴移动方向
// pbyDirectionY:Y轴移动方向
void lct_prediction_prediction_phone_x_y_move_direction(LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList, COORD_T stAheadCoord, int8 *pbyDirectionX, int8 *pbyDirectionY)
{
    LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstCurrentBtsDirectionData = NULL; // 当前基站方向预测数据
    LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstOtherBtsDirectionData = NULL;   // 其它基站方向预测数据
    int8 byDirectionX = 0;
    int8 byDirectionY = 0;

    // 结合前一个定位点坐标判断手机在x轴和y轴上的移动方向
    for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++)
    {
        // 当前基站
        pstCurrentBtsDirectionData = &pstPredictionDirectionDataList[i];
        if (pstCurrentBtsDirectionData->byValidFlag && pstCurrentBtsDirectionData->byDirectionTendency)
        {
            for (uint8 j = 0; j < PUB_SYS_CARRIER_NUM; j++)
            {
                if (i != j)
                {
                    pstOtherBtsDirectionData = &pstPredictionDirectionDataList[j];
                    if (pstOtherBtsDirectionData->byValidFlag && pstOtherBtsDirectionData->byDirectionTendency)
                    {
                    }
                }
            }

            // 靠近该基站
            if (pstCurrentBtsDirectionData->byDirectionTendency == 1)
            {
                // 前一个点的X轴坐标在基站右侧
                if (stAheadCoord.dwX > pstCurrentBtsDirectionData->dwCoordX)
                {
                    pstCurrentBtsDirectionData->byDirectionX = -1; // 向左移动
                }
                else
                {
                    pstCurrentBtsDirectionData->byDirectionX = 1; // 向右移动
                }

                // 前一个点的Y轴坐标在基站上方
                if (stAheadCoord.dwY > pstCurrentBtsDirectionData->dwCoordY)
                {
                    pstCurrentBtsDirectionData->byDirectionY = -1; // 向下移动
                }
                else
                {
                    pstCurrentBtsDirectionData->byDirectionY = 1; // 向上移动
                }
            }
            // 远离该基站
            else if (pstCurrentBtsDirectionData->byDirectionTendency == 2)
            {
                // 前一个点的X轴坐标在基站右侧
                if (stAheadCoord.dwX > pstCurrentBtsDirectionData->dwCoordX)
                {
                    pstCurrentBtsDirectionData->byDirectionX = 1; // 向右移动
                }
                else
                {
                    pstCurrentBtsDirectionData->byDirectionX = -1; // 向左移动
                }

                // 前一个点的Y轴坐标在基站上方
                if (stAheadCoord.dwY > pstCurrentBtsDirectionData->dwCoordY)
                {
                    pstCurrentBtsDirectionData->byDirectionY = 1; // 向上移动
                }
                else
                {
                    pstCurrentBtsDirectionData->byDirectionY = -1; // 向下移动
                }
            }

            byDirectionX += pstCurrentBtsDirectionData->byDirectionX;
            byDirectionY += pstCurrentBtsDirectionData->byDirectionY;
        }
    }

    if (byDirectionX >= 1)
    {
        byDirectionX = 1;
    }
    else if (byDirectionX <= -1)
    {
        byDirectionX = -1;
    }

    if (byDirectionY >= 1)
    {
        byDirectionY = 1;
    }
    else if (byDirectionY <= -1)
    {
        byDirectionY = -1;
    }

    *pbyDirectionX = byDirectionX;
    *pbyDirectionY = byDirectionY;

    return;
}
