#include "cprocess_utils.h"
#include "logic.h"
#include "cmallsystem.h"
#include <algorithm>
#include <list>

CProcess_Utils::CProcess_Utils(const QString& name, int32_t id)
    :CMProcess(name, id)
{

}

CProcess_Utils::~CProcess_Utils()
{

}
void CProcess_Utils::Serialize(QDataStream& stream, bool bwrite, int iVersion)
{
    CMProcess::Serialize(stream, bwrite, iVersion);
}

void CProcess_Utils::do_msg(int iID, QVariantList& vlInput, V_MsgOutput* pvlOutput)
{
    switch (iID)
    {
        case CALIDISTANCESTEP:
        {
            CaliDistanceStep(vlInput[0].toInt());
            break;
        }
        case BEFORE_CALI_DISTANCEDATA:
            BeforeCaliDistanceData(vlInput[0].toInt());
            break;
        case AFTER_CALI_DISTANCEDATA:
            AfterCaliDistanceData(vlInput[0].toInt());
            break;
        case AFTER_ALL_CALI:
            _SysExt_Instance.m_SysMsg.SetMsg(QObject::tr("All platform calibration finished"));
            break;
        default:
            break;
    }
    CMProcess::do_msg(iID, vlInput, pvlOutput);
}

double CProcess_Utils::CaliDistanceData()
{
    CMeasHeadDriver* pDistanceMeter = GET_CARD(DistanceSensor_Driver, CMeasHeadDriver);
    Q_ASSERT(pDistanceMeter && "Distance Driver is nullptr");
    bool bFreshed = false;
    QString info = QObject::tr("Can't get distance data");
    for (int i = 0; i < 3; i++)
    {
        try
        {
            pDistanceMeter->freshresult(DistanceSensor);
            bFreshed = true;
            break;
        }
        catch (const QString& ex)
        {
            info = ex;
            MSleep(10);
        }
    }

    if (false == bFreshed)
    {
        MQT_THROW_CODESTR(ALARM_RESERVED, info);
    }

    CMWorkTableDriver* pWorktableDriver = GET_CARD(WORKTABLE_DRIVER, CMWorkTableDriver);
    CMPointd wt;
    pWorktableDriver->CurAxisToWt(DistanceSensor, wt, true);
    CMeasHead* pMeas = GET_CHANNEL(DistanceSensor, DistanceSensor_Driver, CMeasHead);
    double dDistance = pMeas->m_Variants.get_VData(Var_MeasHead_Result_ID)->toDouble();
    info = QObject::tr("Distance:%1,%2,%3").arg(wt.tX).arg(wt.tY).arg(dDistance);
    m_gSystem.SendLogMessage(info, NOTIFY_MSG_TYPE);

    return dDistance;
}

void CProcess_Utils::BeforeCaliDistanceData(int iSide)
{
    m_gSystem.ChangeSide(iSide);

    CMProduct* pProduct = GET_CHDIRECT(PRODUCT, CMProduct);
    CMWorkTableDriver* pWorktableDriver = GET_CARD(WORKTABLE_DRIVER, CMWorkTableDriver);

    double dDefoucs = pProduct->CutDepthToDefoucsDis(0);

    double dZpos = pWorktableDriver->DefoucsDisToZPos(DistanceSensor, dDefoucs);
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    pMotorDriver->absmove(MOTORZ, dZpos);

    CMPointd axisPos;
    pWorktableDriver->WtToAxis(DistanceSensor, CMPointd(0, 0), axisPos);
    m_gSystem.absmove_xy(axisPos.tX, axisPos.tY);

    m_ptList.clear();
    m_lstDistances.clear();
}

void CProcess_Utils::AfterCaliDistanceData(int iSide)
{
    QDateTime time = QDateTime::currentDateTime();

    if (m_ptList.size() != m_lstDistances.size())
    {
        MQT_THROW_CODESTR(0, QObject::trUtf8("Distance data size err. %1 != %2").arg(m_ptList.size()).arg(m_lstDistances.size()));
    }

    QString strPath = CMakePath::make_run_path("CheckData", (iSide == SIDE_A ? "SideA" : "SideB") + time.toString("yyyyMMddhhmmss"));
    QString strDataPath = CMakePath::make_run_path("CheckData", (iSide == SIDE_A ? "SideA" : "Offset-SideB") + time.toString("yyyyMMddhhmmss(Offset)")) + ".csv";
    QFile file(strPath + ".csv");
    if (!file.open(QIODevice::WriteOnly))
    {
        MQT_THROW_STR(QObject::trUtf8("open path %1 failed")
                      .arg(strPath));
    }
    QTextStream stream(&file);
    int iSize = m_lstDistances.size();
    stream << iSize << "\n";
    std::map<int, std::list<double>> mapData;
    //int iMin = std::numeric_limits<int>::max();
    for (int i = 0; i < m_lstDistances.size(); i++)
    {
        stream << m_ptList[i].tX << ",";
        stream << m_ptList[i].tY << ",";
        stream << QString::number(m_lstDistances[i], 'f', 4) << ",";
        stream << "\n";
        mapData[m_ptList[i].tY].push_back(m_lstDistances[i]);
    }

    
    std::ofstream offsetOut(strDataPath.toLocal8Bit().data());
    offsetOut << "Y,Average,Count,datas\n";
    for (auto iter = mapData.rbegin(); iter != mapData.rend(); iter++)
    {

        double dSum = std::accumulate((*iter).second.begin(), (*iter).second.end(), 0.0);
        double dAvg = dSum / (*iter).second.size();
        offsetOut << (*iter).first << "," << dAvg << "," << (*iter).second.size();
        for (auto dVal : (*iter).second)
        {
            offsetOut << "," << dVal;
        }

        offsetOut << "\n";
    }
}

void CProcess_Utils::CaliDistanceStep(int iStep)
{
#if 0  // 竖向寻找
    if (iStep == 0)
    {
        int iClipDistance = 20;
        int iRadius = 100;
        int iEdge = iRadius - 20;
        int iStepY = 5;
        int iStepX = 15;
        int iTop = 0;
        int iLeft = 0;
        //找到最上面的点
        while (iLeft > -(iRadius - iClipDistance))
        {
            iLeft -= iStepX;
        }

        int iRightLimit = iRadius - iClipDistance;
        while (iLeft <= iRightLimit)
        {
            int iTopLimit = sqrt(iRadius * iRadius - iLeft * iLeft) - iClipDistance;

            //找到最左边的点
            int iBottom = -iTopLimit;
            while (iBottom <= iTopLimit)
            {
                //生成测量的点
                m_ptList.push_back(CMPointd(iLeft, iBottom));

                iBottom += iStepY;
            }

            iLeft += iStepX;
        }
        return;
    }
#endif
    if (iStep == 0)
    {
        int iClipDistance = 20;
        int iRadius = 100;
        int iEdge = iRadius - iClipDistance;
        int iStepY = 5;
        int iStepX = 10;
        int iTop = 0;
        int iLeft = 0;
        //找到最上面的点
        while (iLeft < -(iRadius - iClipDistance))
        {
            iLeft -= iStepX;
        }

        while (iTop < iRadius - iClipDistance)
        {
            iTop += iStepY;
        }

        int iBottomLimit = -(iRadius - iClipDistance);

        while (iTop >= iBottomLimit)
        {
            int iRightLimit = sqrt(iRadius * iRadius - iTop * iTop) - iClipDistance;

            //找到最左边的点
            int iLeft = 0;
            while (iLeft >= -iRightLimit)
            {
                iLeft -= iStepX;
            }
            iLeft += iStepX;

            while (iLeft <= iRightLimit)
            {
                //生成测量的点
                CMPointInt pt(iLeft, iTop);
                m_ptList.push_back(pt);

                iLeft += iStepX;
            }

            iTop -= iStepY;
        }
        return;
    }
    if (m_ptList.size() < iStep)
    {
        return;
    }



    //移动并测量数据,每个点采样9次
    std::list<double> lstDistances;
    CMPointInt pt = m_ptList[iStep - 1];

    //移动Z位置
    MoveZOffset(CMPointd(pt.tX, pt.tY));

    double dRange = 1;
    for (int dX = 0; dX <= 0; ++dX)
    {
        int dY = 0;
        //for (int dY = -1; dY <= 1; ++dY)
        {
            MovePt(CMPointd(pt.tX + dX * dRange, pt.tY + dY * dRange));
            double dDistance = CaliDistanceData();
            lstDistances.push_back(dDistance);
        }
    }

    double dSum = std::accumulate(lstDistances.begin(), lstDistances.end(), 0.0);

    //去掉平均值异常点
    double dAverage = dSum / lstDistances.size();
    dSum = 0;
    int iCount = 0;
    double dMin = std::numeric_limits<double>::max();
    std::for_each(lstDistances.begin(), lstDistances.end(), [&](double& dValue)
    {
        if (dValue < dAverage - 0.003 || dValue > dAverage + 0.003)
        {
            QString str = QString("Err:X:%1 Y:%2 Aver:%3 Val:%4").arg(pt.tX).arg(pt.tY).arg(dAverage).arg(dValue);
            m_gSystem.SendLogMessage(str, WARN_MSG_TYPE);
            return;
        }

        dSum += dValue;
        iCount++;
    });

    double dDistance = dAverage;
    if (iCount > 0)
    {
        dDistance = dSum / iCount;
    }
    else
    {
        //取中值
        lstDistances.sort();
        auto it = std::next(lstDistances.begin(), lstDistances.size() / 2 - 1);
        dDistance = *it;
    }

    m_lstDistances.push_back(dDistance);
}

void CProcess_Utils::ReadRecipe(ConfigStreamReader& streamReader, int iVersion)
{
    CMProcess::ReadRecipe(streamReader, iVersion);
}

void CProcess_Utils::WriteRecipe(ConfigStreamWriter& streamWriter, int iVersion)
{
    CMProcess::WriteRecipe(streamWriter, iVersion);
}

void CProcess_Utils::MovePt(const CMPointd& wt)
{
    CMPointd axisPos;
    CMWorkTableDriver* pWorktableDriver = GET_CARD(WORKTABLE_DRIVER, CMWorkTableDriver);
    pWorktableDriver->WtToAxis(DistanceSensor, wt, axisPos);
    m_gSystem.absmove_xy(axisPos.tX, axisPos.tY);
}

void CProcess_Utils::MoveZOffset(const CMPointd& wt)
{
#if 0
    CMProduct* pProduct = GET_CHDIRECT(PRODUCT, CMProduct);
    double dDefoucs = pProduct->CutDepthToDefoucsDis(0);
    CMWorkTableDriver* pWorktableDriver = GET_CARD(WORKTABLE_DRIVER, CMWorkTableDriver);
    double dZpos = pWorktableDriver->DefoucsDisToZPos(DistanceSensor, dDefoucs);

    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    double dPosW = pMotorDriver->get_posdata(MOTORW, true);

    //判断是否在加工范围
    int offsetChId = -1;
    {
        //A面
        CProductSide* pSide = GET_CHDIRECT(SIDE_A, CProductSide);
        double dInitPos = pSide->m_Variants.get_VData(Var_ProductSide_InitAngle)->toDouble();
        if (dPosW > dInitPos - 5
            && dPosW < dInitPos + 5)
        {
            offsetChId = WTOFFSET_Z_CH1;
        }
    }
    {
        //B面
        CProductSide* pSide = GET_CHDIRECT(SIDE_B, CProductSide);
        double dInitPos = pSide->m_Variants.get_VData(Var_ProductSide_InitAngle)->toDouble();
        if (dPosW > dInitPos - 5
            && dPosW < dInitPos + 5)
        {
            offsetChId = WTOFFSET_Z_CH2;
        }
    }

    double dLocationPos = wt.tY;
    double dWorktableOffsetZ = 0;
    if (offsetChId != -1)
    {
        //处理Z轴补偿
        CWorktableOffset* pOffset = GET_CHDIRECT(offsetChId, CWorktableOffset);
        bool bEnable = pOffset->m_Variants.get_VData(Var_WorkTable_Offset_EnableOffset)
            ->toBool();
        if (bEnable)
        {
            //偏移定位方向，平台位置补偿,默认禁用
            CWorktableOffsetDriver* pOffsetDriver = GET_CARD(WTOFFSET_DRIVER, CWorktableOffsetDriver);
            dWorktableOffsetZ = pOffsetDriver->GetOffset(offsetChId, dLocationPos);
            //MqDebug() << "Z Offset:" << dWorktableOffsetZ << " at Location:" << dLocationPos;
            if (fabs(dWorktableOffsetZ) > 0.1)
            {
                MQT_THROW_CODESTR(ALARM_RESERVED, QString("Z Offset too large Loc:%1 Offset:%2").arg(dLocationPos).arg(dWorktableOffsetZ));
            }
            dZpos += dWorktableOffsetZ;
        }
    }

    //移动Z向偏移
    double dCurPosZ = pMotorDriver->get_posdata(MOTORZ, true);
    if (!INRANGE(dCurPosZ, dZpos - 0.001, dZpos + 0.001))
    {
        m_gSystem.SendLogMessage(QString("Move Offset:(X:%1,Y:%2) (%3)").arg(wt.tX).arg(wt.tY).arg(dWorktableOffsetZ), WARN_MSG_TYPE);
        pMotorDriver->absmove(MOTORZ, dZpos);
    }
#endif
}

