// 秒寻科技
// 变频模块
// zt
// 2023-03-05

#include "../../includes/includes.h"
extern flt32 gDmdl_afTmpDataList[PUB_FFT_FRAME_LEN];

// 信号变频(在指定的频率上解调信号)
// pwData:音频采样数据
// wLen:音频采样数据长度
// dwFrequency:变频频率
// dwIndex:变频索引
// pfOutSignalReal:返回参数，变频后的信号实部
// pfOutSignalImag:返回参数，变频后的信号虚部
void dmdl_ddc(int16 *pwData, uint16 wLen, uint32 dwFrequency, uint32 dwIndex, flt32 *pfOutSignalReal, flt32 *pfOutSignalImag)
{
    flt64 fDdcFactor = 0;   // 变频因子
    flt64 fDdcReal = 0.0;   // 变频实部
    flt64 fDdcImag = 0.0;   // 变频虚部
    flt64 fDdcRadian = 0.0; // 弧度

    flt32 *pfRealHead = pfOutSignalReal;
    flt32 *pfImagHead = pfOutSignalImag;

    fDdcFactor = -1 * PUB_2PI * dwFrequency / PUB_SIGNAL_SAMPLE_RATIO; // 变频因子
    for (uint16 i = 0; i < wLen; i++)
    {
        fDdcRadian = fDdcFactor * (dwIndex + i); // 相位因子
        fDdcRadian = fmod(fDdcRadian, PUB_2PI);  // 计算小数部分
        fDdcReal = cos(fDdcRadian);              // 实部
        fDdcImag = sin(fDdcRadian);              // 虚部

        // 复数乘法(信号虚部为0)
        *pfRealHead = (*pwData) * fDdcReal;
        *pfImagHead = (*pwData) * fDdcImag;

        pwData++;
        pfRealHead++;
        pfImagHead++;
    }

    return;
}

// 信号变频(在指定的频率上解调信号)
// pwData:音频采样数据
// dwFrequency:频率
// pdwDdcIndexRemainder:变频索引余数
// pfDdcSignalReal:返回参数，变频后的信号实部
// pfDdcSignalImag:返回参数，变频后的信号虚部
void dmdl_ddc_main(int16 *pwData, uint32 dwFrequency, uint32 *pdwDdcIndexRemainder, flt32 *pfDdcSignalReal, flt32 *pfDdcSignalImag)
{
    uint16 wDdcIndex = *pdwDdcIndexRemainder;
    flt64 f2PI = -2 * PUB_M_PI;
    flt64 fDdcFactor = 0; // 变频因子

    flt64 fDdcReal = 0.0; // 变频实部
    flt64 fDdcImag = 0.0; // 变频虚部

    flt32 *pfRealHead = pfDdcSignalReal;
    flt32 *pfImagHead = pfDdcSignalImag;

    for (uint16 i = 0; i < PUB_SLOT_FRAME_LEN; i++)
    {
        wDdcIndex += dwFrequency;
        wDdcIndex %= PUB_SIGNAL_SAMPLE_RATIO; // 采样率
        fDdcFactor = f2PI * wDdcIndex;
        fDdcFactor /= PUB_SIGNAL_SAMPLE_RATIO;

        fDdcReal = cos(fDdcFactor); // 实部
        fDdcImag = sin(fDdcFactor); // 虚部

        // 复数乘法(信号虚部为0)
        *pfRealHead = (flt32)((*pwData) * fDdcReal);
        *pfImagHead = (flt32)((*pwData) * fDdcImag);

        pwData++;
        pfRealHead++;
        pfImagHead++;
    }
    *pdwDdcIndexRemainder = wDdcIndex; // 保存变频索引，以便下一帧使用

    return;
}

// 组织变频后的信号
// dwFrequency:载波中心频率
// fDdcFactor:变频因子，固定为16384/48000
// pdwFrequencyFactor:频率因子，模12000的余数
// pfFftRealData:频域实部数据
// pfFftImagData:频域虚部数据
// pfSignalRealData:返回参数，信号频域实部数据
// pfSignalImagData:返回参数，信号频域虚部数据
void dmdl_ddc_organize_data(uint32 dwFrequency, flt32 fDdcFactor, uint32 *pdwFrequencyFactor, flt32 *pfFftRealData, flt32 *pfFftImagData, flt32 *pfSignalRealData, flt32 *pfSignalImagData)
{
    uint16 wCenterIndex = roundf(dwFrequency * fDdcFactor); // 4舍5入
    uint16 wDataLen = 0;
    flt32 fReal = 0;
    flt32 fImag = 0;
    flt32 fCosValue = cos(PUB_2PI * (*pdwFrequencyFactor) / PUB_FFT_FRAME_LEN);
    flt32 fSinValue = -sin(PUB_2PI * (*pdwFrequencyFactor) / PUB_FFT_FRAME_LEN);

    flt32 *pfRealHead = pfSignalRealData;
    flt32 *pfImagHead = pfSignalImagData;

    wDataLen = PUB_FFT_FRAME_LEN - wCenterIndex;

    // FFT校准
    for (uint16 i = 0; i < PUB_FFT_FRAME_LEN; i++)
    {
        fReal = (*pfFftRealData) * fCosValue - (*pfFftImagData) * fSinValue;
        fImag = (*pfFftRealData) * fSinValue + fCosValue * (*pfFftImagData);
        *pfRealHead = fReal;
        *pfImagHead = fImag;

        pfFftRealData++;
        pfFftImagData++;

        pfRealHead++;
        pfImagHead++;
    }

    // 组织实部数据
    memset(gDmdl_afTmpDataList, 0, sizeof(flt32) * PUB_FFT_FRAME_LEN);
    memcpy(gDmdl_afTmpDataList, &pfSignalRealData[wCenterIndex], sizeof(flt32) * wDataLen); // 前段
    memcpy(&gDmdl_afTmpDataList[wDataLen], pfSignalRealData, sizeof(flt32) * wCenterIndex); // 后段

    memset(pfSignalRealData, 0, sizeof(flt32) * PUB_FFT_FRAME_LEN);
    memcpy(pfSignalRealData, gDmdl_afTmpDataList, sizeof(flt32) * PUB_FFT_FRAME_LEN);

    // 组织虚部数据
    memset(gDmdl_afTmpDataList, 0, sizeof(flt32) * PUB_FFT_FRAME_LEN);
    memcpy(gDmdl_afTmpDataList, &pfSignalImagData[wCenterIndex], sizeof(flt32) * wDataLen); // 前段
    memcpy(&gDmdl_afTmpDataList[wDataLen], pfSignalImagData, sizeof(flt32) * wCenterIndex); // 后段

    memset(pfSignalImagData, 0, sizeof(flt32) * PUB_FFT_FRAME_LEN);
    memcpy(pfSignalImagData, gDmdl_afTmpDataList, sizeof(flt32) * PUB_FFT_FRAME_LEN);

    *pdwFrequencyFactor = ((*pdwFrequencyFactor) + wCenterIndex * PUB_SLOT_FRAME_LEN) % PUB_FFT_FRAME_LEN;

    return;
}