#include "CasicDevPhase.h"

casic::birmm::tflab::CasicDevPhase::CasicDevPhase()
{
    QVector<double> zero(6, 0.0);
    for (int i = 0; i < PHASE_MESSURE_CHANNEL; i++)
    {
        channelDelay.append(0.0); // 初始化各个通道的时延值

        lastSigmaVector.append(0.0); // 初始化计算稳定度时需要的上一个平方和值

        phaseVector.append(zero);
    }

    frameId = 0; // 帧编号初始化为0
    frameCount = 0; // 帧计数初始化为0
}

int casic::birmm::tflab::CasicDevPhase::frameParse(QByteArray rawBytes, qulonglong** frameSN, QList<double>* channelValue, QList<double>* channelExtra)
{
    int retCode = RETURN_STATUS::FAIL;

    if (PhaseProtocolBM::checkFrame(rawBytes) == false)
    {
        // 数据帧尾和长度不对则返回解析失败
        return retCode;
    }

    PhaseDataDto * phaseData = new PhaseDataDto();
    bool parse = PhaseProtocolBM::parseMessureData(rawBytes, phaseData);

    // 解析成功
    if (parse == true)
    {
        frameCount++;

        // 返回帧计数
        frameId = phaseData->frameId.toULongLong(0, 16);
        *frameSN = &frameId;

        // 返回补偿后的测量结果
        for (int i = 0; i < PHASE_MESSURE_CHANNEL; i++)
        {
            double value = phaseData->channelBeforeDelay.at(i) + channelDelay.at(i);
            channelValue->append(value);
        }

        if (frameCount <= 5)
        {
            // 总计小于5帧时无法计算稳定度
            retCode = RETURN_STATUS::INVALID;

            for (int i = 0; i < PHASE_MESSURE_CHANNEL; i++)
            {
                phaseVector[i][frameCount - 1] = phaseData->channelBeforeDelay.at(i);
            }
        } else
        {
            // 开始计算稳定度
            for (int i = 0; i < PHASE_MESSURE_CHANNEL; i++)
            {
                phaseVector[i][5] = phaseData->channelBeforeDelay.at(i); // 最新的一帧数据

                double allen = calcAllen(i, frameCount);
                channelExtra->append(allen);

                // 保留最新的五组数据
                phaseVector[i][0] = phaseVector[i][1];
                phaseVector[i][1] = phaseVector[i][2];
                phaseVector[i][2] = phaseVector[i][3];
                phaseVector[i][3] = phaseVector[i][4];
                phaseVector[i][4] = phaseVector[i][5];
            }

            retCode = RETURN_STATUS::SUCCESS;
        }
    }

    delete phaseData;
    return retCode;
}

int casic::birmm::tflab::CasicDevPhase::setChannelDelay(QList<double> channelDelay)
{
    this->channelDelay = channelDelay;

    return RETURN_STATUS::SUCCESS;
}
QList<double> casic::birmm::tflab::CasicDevPhase::getChannelDelay()
{
    return this->channelDelay;
}

double casic::birmm::tflab::CasicDevPhase::calcAllen(int index, int aN)
{
    int i = 0;
    int d = 1;  //此处只计算秒稳
    double tau0 = 1;    //tau0是基本采样间隔，由计数器或比相仪的最小采样间隔决定，最小为1s

    double allan = 0.0;
    double *y = new double[4];
    double tau_2 = pow(d * tau0, 2);				//pow是计算x的y次幂
    double sum = lastSigmaVector.at(index);

    for (int j = 4; j > 0; j--)
    {
        i = 6 - 2*d - j;
        double x2 = phaseVector[index][i+2*d];
        double x1 = phaseVector[index][i+d];
        double x = phaseVector[index][i];
        y[4 - j] = pow(x2 - 2 * x1 + x, 2);

        sum = sum + y[4 - j];
    }

    lastSigmaVector[index] += y[0];

    allan = sqrt(sum/(2*tau_2*(aN-2*d))); //delta

    delete[] y;
    return allan;
}