#include "cprocess_cut.h"
#include "logic.h"
#include "cmproduct.h"
#include "cmworktabledriver.h"
#include "cproductside.h"
#include "cproductsidedriver.h"
#include "claserpathdriver.h"
#include "camatchncc_h.h"
#include "cmotordriver_acs.h"
#include "cprocess_level.h"
CProcess_Cut::CProcess_Cut(const QString &name, int32_t id)
    :CMProcess(name, id)
{
    m_Variants.add_Variant(Var_Calibration_POSX,
                           QObject::trUtf8("Calibration_POSX"), double(0), false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Calibration_POSY,
                           QObject::trUtf8("Calibration_POSY"), double(0), false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Calibration_POSZ,
                           QObject::trUtf8("Calibration_POSZ"), double(0), false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Calibration_OffsetX,
                           QObject::trUtf8("Calibration_OffsetX"), double(0), false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Calibration_OffsetY,
                           QObject::trUtf8("Calibration_OffsetY"), double(0), false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Calibration_Angle,
                           QObject::trUtf8("Calibration_Angle"), double(0), false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Calibration_Adjust_Threshold,
                           QObject::trUtf8("Calibration_AdjustThreshold"), double(0), false,
                           Variant::TOXML);
}

//划线
int CProcess_Cut::CutLine(int iSideID, int iLineID, int iLayerID, int iTimes)
{
    CMWorkTableDriver* pWorkTableDriver = GET_CARD(WORKTABLE_DRIVER,
                                                   CMWorkTableDriver);
    Q_ASSERT(pWorkTableDriver);
    CProductSide* pProductSide = GET_CHDIRECT(iSideID, CProductSide);
    Q_ASSERT(pProductSide);
    if (iSideID != m_gSystem.m_iCurSideID) {
        MQT_THROW_CODESTR(ALARM_MAIN_SAFE_PROTECT, QObject::trUtf8("Cut Side :%1 not match cur side:%2")
                          .arg(m_gSystem.m_mapNameList[iSideID])
                          .arg(m_gSystem.m_mapNameList[m_gSystem.m_iCurSideID]));
    }
    try {
        m_gSystem.m_lineTime.StartTime();

        m_gSystem.m_iCurLine = iLineID;
        m_gSystem.m_iCurLayer = iLayerID;
        int iSizeParam = pProductSide->m_vCutParam.size();
        if (OUTOFRANGE(iLayerID, 0, iSizeParam))return MSG_FINISHED;
        int iSizeLineVector = pProductSide->m_CutLineVector.size();
        if (OUTOFRANGE(iLineID, 0, iSizeLineVector)) return MSG_FINISHED;
        SINGLECUTPARAM Param = pProductSide->m_vCutParam[iLayerID];
        CUTLINE* pCutLine = pProductSide->GetCutLine(iLineID);
        Q_ASSERT(pCutLine);
        CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
        Q_ASSERT(pMotorDriver);
        //检查是否开真空状态
        m_gSystem.AssertCheckWtVaccum();

        //检查安全门关闭
        try {
            m_gSystem.SafeDoorCheck();
        } catch (const QString& ex) {
            m_gSystem.CloseSafeDoor();

            throw ex;
        }

        m_gSystem.m_iCurChID = Param.laserpathID;
        //光路移动到设定的功率
        CLaserPath * pLPath = GET_CHDIRECT(Param.laserpathID, CLaserPath);
        Q_ASSERT(pLPath);

        CLaserPathDriver* pPathDriver = GET_CARD(LASERPATH_DRIVER, CLaserPathDriver);
        Q_ASSERT(pPathDriver);
        pPathDriver->MovePathAngle(Param.laserpathID,
                                   Param.iLaserEpulse, Param.dBeamWidth,
                                   Param.iLaserFreq, Param.dLaserPower);

        g_SecsManager.SetVariable(SVID_CuttingSpeed, QString::number(Param.dCutSpd, 'f', 2).toStdString());


        CMProduct* pProduct = GET_CHDIRECT(PRODUCT, CMProduct);
        //计算激光焦距并移动到焦距位置
        double dDefoucs = pProduct->CutDepthToDefoucsDis(Param.dCutDepth);
        double dZpos = pWorkTableDriver->DefoucsDisToZPos(Param.laserpathID, dDefoucs);


        //生成起始和结束点
        //X,Y变向了，所以实际使用的点是Y点
        //Y 是定位 X是Location
        double dStartPos = pCutLine->m_WPPair.ptNegative.tX;
        double dEndPos = pCutLine->m_WPPair.ptPostive.tX;
        double dPosLocation = pCutLine->m_WPPair.ptPostive.tY;
        double dOffset = Param.dOffset;
        double dOverCut = Param.dOvercut;
        int iDelayTime = 0; //Param.iSleepTime
        //重复多次切割,切割次数
        pMotorDriver->setio(IO_OUTPUT, OUT_BLUE, TRUE);
        pMotorDriver->setio(IO_OUTPUT, OUT_LENS_Blow_Inner, TRUE);
        pMotorDriver->setio(IO_OUTPUT, OUT_LENS_Blow_Outter, TRUE);
        pMotorDriver->setspeed(MOTORX, Param.dCutSpd, 0, 0, 0, 0);

        QString info = QString("\tCut:%1 Line:%2 Pass:%3\t").arg(SIDE_A == iSideID ? "CH1" : "CH2")
            .arg(iLineID + 1, 3).arg(iLayerID + 1);
        if (0 == iLineID && 0 == iTimes) {
            AddProductData(QString("Start Cut Side:%1 at %2").arg(SIDE_A == iSideID ? "CH1" : "CH2")
                           .arg(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss zzz")));
            AddProductData(QString("Angle:%1 CenterX:%2 CenterY:%3").arg(pProductSide->m_dLevelAngle)
                           .arg(pProductSide->m_realCenter.tX).arg(pProductSide->m_realCenter.tY));

            AddProductData(QString("Param:\n\tDepth:%1 OverCut:%2 Speed:%3 Power:%4 Freq:%5 Times:%6 YOffset:%7")
                           .arg(Param.dCutDepth).arg(Param.dOvercut).arg(Param.dCutSpd).arg(Param.dLaserPower)
                           .arg(Param.iLaserFreq).arg(Param.iCutCount).arg(Param.dOffset));
        }
        iTimes = max(0, iTimes);

        LOG_INFO(QString("Cut Line:SideId %1 LineID:%2 LayerId:%3 Times:%4").arg(iSideID == SIDE_A ? "CH1" : "CH2").arg(iLineID + 1).arg(iLayerID + 1).arg(iTimes).toLocal8Bit().data());
        for (int i = iTimes; i < Param.iCutCount; i++) {
            //X轴进行切割,移动到位并且使用PEG功能划线
            //dOffsetY 为光路关联的偏移补偿
            //m_bDisableCutlineLaser 如果禁用划线，则划线时不会出光
            //记录划线的过程数据
            WriteRecoveredData(iSideID, iLineID, iLayerID, i, false);
            //if (RTN_SUCCESS1 == m_gSystem.CutPosX(dStartPos, dEndPos, dPosY, dZpos, iDelayTime,
            //                                      Param.laserpathID, dOffset, Param.dCutSpd,
            //                                      iSideID, -1, m_gEnbaleLaserOpen && (!m_gSystem.m_bDisableCutlineLaser),
            //                                      0, false, false, false)) {
            if (RTN_SUCCESS1 == m_gSystem.CutPos(dStartPos, dEndPos, dPosLocation, dZpos,
                                                 dOffset, dOverCut,
                                                 Param.laserpathID, true, Param.dCutSpd, 0,
                                                 (!m_gSystem.m_bDisableCutlineLaser))) {
                //记录划线的过程数据
                //if(!m_bEMG)
                //如果按了急停,则不记录结束
                WriteRecoveredData(iSideID, iLineID, iLayerID, i, true);
                CProcess_Level* pLevel = GET_PROCESS(PROCESS_LEVELING, SM_MAIN, CProcess_Level);
                int iEndLine = pLevel->m_Variants.get_VData(Var_Process_ProductSide_CutEndA)->toInt();
                if (iSideID != SIDE_A) {
                    iEndLine = pLevel->m_Variants.get_VData(Var_Process_ProductSide_CutEndB)->toInt();
                }
                WriteRealtimeData(iSideID, iLineID, iLayerID, i, iSizeLineVector, iSizeParam, Param.iCutCount, iEndLine);
            } else {
                AddProductData(QString("\tERR\tCut Line Err at: %1").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss zzz")));
            }
        }
        CLaserDriver* pLaserDriver = GET_CARD(LASER_DRIVER, CLaserDriver);
        pLaserDriver->OpenLaserGate(LASER, FALSE);
        //设置默认速度
        pMotorDriver->setdefaultspeed(MOTORX);
        pCutLine->m_bHasCut = iLayerID + 1;
        LOG_INFO(fmt::format("Cut Line Time:{} S index : {}  Length : {}", m_gSystem.m_lineTime.GetTime(), pCutLine->m_index + 1, fabs(dStartPos - dEndPos)));
        m_gSystem.m_lineTime.StopTime();
        //AddProductData(QString("\tCut Line Finish Time: %1S Length:%2").arg(m_gSystem.m_lineTime.GetTime()).arg(fabs(dStartPosX-dEndPosX)));
        info += QString("SpendTime:%1S Length:%2").arg(m_gSystem.m_lineTime.GetTime(), 0, 'f', 2).arg(fabs(dStartPos - dEndPos));
        AddProductData(info);
        pMotorDriver->setio(IO_OUTPUT, OUT_BLUE, FALSE);
        //pMotorDriver->setio(IO_OUTPUT, OUT_Shutter_All_Open, FALSE);
    } catch (QString& s) {
        m_gSystem.AddProductInfo(PROCESS_CUT, "CutPos Error" + s);
        m_gSystem.m_lineTime.StopTime();
        throw(s);
    }
    return MSG_FINISHED;
}

//按位置切割
int CProcess_Cut::CutPos(int iSideID, double dStartPos, double dEndPos, double dPosLocation, int iLayerID, bool bXDirect/*=true*/)
{
    CMWorkTableDriver* pWorkTableDriver = GET_CARD(WORKTABLE_DRIVER, CMWorkTableDriver);
    Q_ASSERT(pWorkTableDriver);
    CProductSide* pProductSide = GET_CHDIRECT(iSideID, CProductSide);
    Q_ASSERT(pProductSide);
    m_gSystem.m_lineTime.StartTime();
    //m_gSystem.m_iCurLine=-1;
    m_gSystem.m_iCurLayer = iLayerID;
    int iSizeParam = pProductSide->m_vCutParam.size();
    if (OUTOFRANGE(iLayerID, 0, iSizeParam))return MSG_FINISHED;
    SINGLECUTPARAM Param = pProductSide->m_vCutParam[iLayerID];

    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorDriver);
    try {

        if (!m_gSystem.IsSimulator()) {
            m_gSystem.AssertCheckWtVaccum();

            //检查安全门关闭
            try {
                m_gSystem.SafeDoorCheck();
            } catch (const QString& ex) {
                m_gSystem.CloseSafeDoor();

                throw ex;
            }
        }

        MqDebug() << "get_iodata Time:" << m_gSystem.m_lineTime.GetTime() << "S";

        //      CLaserDriver* pLaserDriver=GET_CARD(LASERDRIVER,CLaserDriver);
        m_gSystem.m_iCurChID = Param.laserpathID;

        CLaserPath * pLPath = GET_CHDIRECT(Param.laserpathID, CLaserPath);
        Q_ASSERT(pLPath);
        CLaserPathDriver* pPathDriver = GET_CARD(LASERPATH_DRIVER, CLaserPathDriver);
        Q_ASSERT(pPathDriver);
        //激光光路的移动，用于设定激光功率
        pPathDriver->MovePathAngle(Param.laserpathID, Param.iLaserEpulse, Param.dBeamWidth,
                                   Param.iLaserFreq, Param.dLaserPower);
        m_gSystem.AddProductInfo(PROCESS_CUT, fmt::format("MovePathAngle Time:{} S", m_gSystem.m_lineTime.GetTime()).c_str());
        CMProduct* pProduct = GET_CHDIRECT(PRODUCT, CMProduct);
        double dDefoucs = pProduct->CutDepthToDefoucsDis(Param.dCutDepth);
        double dZpos = pWorkTableDriver->DefoucsDisToZPos(Param.laserpathID, dDefoucs);

        int iDelayTime = 0;//Param.iSleepTime;
        double dOffsetY = Param.dOffset;
        for (int i = 0; i < Param.iCutCount; i++) {
            /*m_gSystem.CutPosX(dStartPos, dEndPos, dPosLocation, dZpos, iDelayTime,
                              Param.laserpathID, dOffsetY, Param.dCutSpd, iSideID, -1, m_gEnbaleLaserOpen);*/
            m_gSystem.CutPos(dStartPos, dEndPos, dPosLocation, dZpos, dOffsetY, Param.dOvercut,
                             Param.laserpathID, bXDirect, Param.dCutSpd, 0, true);
        }
        m_gSystem.m_lineTime.StopTime();

        //pMotorDriver->setio(IO_OUTPUT, OUT_Shutter_All_Open, FALSE);

        m_gSystem.AddProductInfo(PROCESS_CUT, fmt::format("CutPos Line Time: {} S", m_gSystem.m_lineTime.GetTime()).c_str());
    } catch (QString& s) {
        m_gSystem.m_lineTime.StopTime();
        m_gSystem.AddProductInfo(PROCESS_CUT, "CutPos Error" + s);
        throw(s);
    }
    return MSG_FINISHED;
}

int CProcess_Cut::Calibration(int32_t iCamID)
{
    //设置光源亮度
    //SetLevelLight(iSideID,iCamID);

    //MSleep(m_Variants.get_VData(Var_Process_Level_BeforeSearchSleep)
    //	->toInt());

    CMImage Image;
    CCam* pCam = GET_CHDIRECT(iCamID, CCam);
    Q_ASSERT(pCam);
    CAMatchNCC_H* pMatch = (CAMatchNCC_H*)pCam->Find_Algorithm(M_CALIBRATION_NCC);
    Q_ASSERT(pMatch);

    CMWorkTableDriver* pWorkTableDriver = GET_CARD(WORKTABLE_DRIVER, CMWorkTableDriver);
    Q_ASSERT(pWorkTableDriver);

    /*ModelData *pModelData=GetModelData(iSideID,iCamID);
    Q_ASSERT(pModelData);*/
    //if(pModelData->iModelID==-1)
    //{
    //	MQT_THROW_STR(QObject::trUtf8("%1 %2 has not make pattern")
    //		.arg(iSideID==SIDE_A?"A_SIDE":"B_SIDE")
    //		.arg(m_gSystem._strCompName[iCamID]));
    //}

    /*MODEL_INFO &info=pSide->get_Model_info(iCamID);*/

    pMatch->m_Variants.get_VData(Var_Match_ThresScore_ID)->setValue(0.8);

    int  width, height;
    pCam->GetPixelSize(width, height);
    pMatch->m_rect_ROI.m_center = pCam->get_camImageCenter();

    bool benableROI = false;
    if (benableROI) {
        double roirate = m_Variants.get_VData(Var_Process_Level_ROIParent)
            ->toDouble();
        /*pMatch->m_rect_ROI.m_height=pModelData->ModelImage.m_height+2;
        pMatch->m_rect_ROI.m_width=pModelData->ModelImage.m_width*roirate+2;*/

    } else {
        pMatch->m_rect_ROI.m_height = height*0.9;
        pMatch->m_rect_ROI.m_width = width*0.9;
    }

    CImageContainer* pContainer = &pCam->m_imageContainer;
    if (pContainer) {
        pMatch->showROI(pContainer);
    }
    CCamDriver* pCamDriver = GET_CARD(CAM_DRIVER, CCamDriver);
    Q_ASSERT(pCamDriver);


    pCamDriver->grab_image(iCamID, &Image);
    //pMatch->setModelData(pModelData);
    //
    pMatch->exe(&Image);
    pMatch->showresult(&pCam->m_imageContainer);
    if (pContainer) {
        pMatch->showresult_shape(pContainer);
    }

    if (!pMatch->lcenters.size()) {
        return false;
    }
    CMPointd PixelResult;
    int32_t idx;
    CMPointd ptCenter = pCam->get_camImageCenter();
    pMatch->get_mostnear_point(PixelResult, idx, ptCenter);
    //PixelResult返回值应该是以Center为基准的偏移
    CMPointd WTResult;
    pWorkTableDriver->CurCamToWt(iCamID, PixelResult, WTResult, true);
    //if(adjustToRefPt)
    //{
    //	WTResult.tX-=
    //		pSide->m_Variants.get_VData(Var_ProductSide_TemplateOffsetX)
    //		->toDouble();
    //	WTResult.tY-=
    //		pSide->m_Variants.get_VData(Var_ProductSide_TemplateOffsetY)
    //		->toDouble();
    //}
    double dAngle = pMatch->lresults[idx].angle;

    //MSleep(m_Variants.get_VData(Var_Process_Level_AfterSearchSleep)
    //	->toInt());
    return MSG_FINISHED;
}

int32_t CProcess_Cut::decode_msg(int32_t id, const char *pmsg)
{
    return RTN_FAILED1;
    /*char s[COMM_LINE_LEN]={0};
    m_strncpy(s,pmsg,COMM_LINE_LEN);
    QList<char *> pos;
    spliteMsg(s,pos);

    int32_t isize=pos.size();
    switch(id)
    {
    case CUT_CUTLINE:
        Q_ASSERT(isize==4);
        if(isize==4)
        {
            CutLine(atoi(pos[0]),atoi(pos[1]),atoi(pos[2]),atoi(pos[3]));
        }
        break;
    case CUTPOS:
        if(isize==5)
        {
            CutPos(atoi(pos[0]),atof(pos[1]),atof(pos[2]),
                    atof(pos[3]),atoi(pos[4]));
        }
        break;
    }*/
    return CProcess::decode_msg(id, pmsg);
}

int CProcess_Cut::MoveCutPos(double dFirstX, double dFirstY)
{
    m_gSystem.changeChannel(CAM6X, true);

    CMPointd WPDPStart(dFirstX, dFirstY);
    CMPointd AxisStart;
   
    CMWorkTableDriver* pWorkTableDriver = GET_CARD(WORKTABLE_DRIVER, CMWorkTableDriver);
    pWorkTableDriver->WtToAxis(CAM6X, WPDPStart, AxisStart);
    Q_ASSERT(pWorkTableDriver);
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorDriver);
    pMotorDriver->absmove(MOTORX, AxisStart.tX, FALSE);
    pMotorDriver->absmove(MOTORY, AxisStart.tY, FALSE);

    pMotorDriver->wait_move_finished(MOTORX);
    pMotorDriver->wait_move_finished(MOTORY);

    return 0;
}

void CProcess_Cut::do_msg(int iID, QVariantList &vlInput,
                          V_MsgOutput *pvlOutput)
{
    int32_t isize = vlInput.size();
    switch (iID) {
    case CUT_CUTLINE:
        Q_ASSERT(isize >= 4);
        if (isize >= 4) {
            CutLine(vlInput[0].toInt(), vlInput[1].toInt(),
                    vlInput[2].toInt(), vlInput[3].toInt());
        }
        break;
    case CUTPOS:
        if (isize >= 5) {
            CutPos(vlInput[0].toInt(), vlInput[1].toDouble(),
                   vlInput[2].toDouble(), vlInput[3].toDouble(),
                   vlInput[4].toInt(), vlInput[5].toBool());
        }
        break;
    case CALIBRATION:
    {
        Calibration(vlInput[0].toInt());
    }
    break;
    case MOVECUTPOS:
    {
        MoveCutPos(vlInput[0].toDouble(), vlInput[1].toDouble());
    }
    break;
    }
    CMProcess::do_msg(iID, vlInput, pvlOutput);
}

void CProcess_Cut::ClearRecoveredData(int iSideId)
{
    QString filePath = m_gSystem.MakeRecoverPath(QString("CutLine%1.txt").arg(iSideId == SIDE_A ? "CH1" : "CH2"));
    if (QFile::exists(filePath)) {
        QFile::remove(filePath);
    }
}

void CProcess_Cut::ClearRealtimeData(int iSideId)
{
    QString filePath = m_gSystem.MakeRecoverPath(QString("CutLine%1.dat").arg(iSideId == SIDE_A ? "CH1" : "CH2"));
    if (QFile::exists(filePath)) {
        QFile::remove(filePath);
    }
}

bool CProcess_Cut::WriteRecoveredData(int iSideId, int iLineId, int iLayer, int iTimes, bool bFinished)
{
    try {
        QString filePath = m_gSystem.MakeRecoverPath(QString("CutLine%1.txt").arg(iSideId == SIDE_A ? "CH1" : "CH2"));
        QFile file(filePath);
        if (!file.open(QIODevice::Append)) {
            LOG_ERR(fmt::format("create WriteRecoveredData file failed {}", filePath.toLocal8Bit().data()));
            return false;
        }

        QTextStream stream(&file);
        stream << iSideId << "," << iLineId << "," << iLayer << "," << iTimes << "," << bFinished << "\n";
        file.close();
    } catch (std::exception ex) {
        LOG_ERR("create WriteRecoveredData failed ");
        return false;
    }

    return true;
}

bool CProcess_Cut::WriteRealtimeData(int iSideId, int iLineId, int iLayer, int iTimes, int iLineSize, int iLayerSize, int iTimesSize, int iEndLine)
{
    try {
        QString filePath = m_gSystem.MakeRecoverPath(QString("CutLine%1.dat").arg(iSideId == SIDE_A ? "CH1" : "CH2"));
        QFile file(filePath);
        if (!file.open(QIODevice::WriteOnly)) {
            QString err = file.errorString();
            LOG_ERR(fmt::format("create WriteRecoveredData file failed {}", filePath.toLocal8Bit().data()));
            return false;
        }

        QDataStream stream(&file);
        stream << iSideId << iLineId << iLayer << iTimes << iLineSize << iLayerSize << iTimesSize << iEndLine;
        file.close();
    } catch (std::exception ex) {
        LOG_ERR("create WriteRecoveredData failed ");
        return false;
    }

    return true;
}
bool CProcess_Cut::ReadRealtimeData(int iSideId, int& iLineId, int& iLayer, int& iTimes, int& iLineSize, int& iLayerSize, int& iTimesSize, int& iEndLine)
{
    try {
        iLineId = 0;
        iLayer = 0;
        iTimes = -1;

        QString filePath = m_gSystem.MakeRecoverPath(QString("CutLine%1.dat").arg(iSideId == SIDE_A ? "CH1" : "CH2"));
        if (!QFile::exists(filePath)) {
            return false;
        }

        QFile file(filePath);
        if (!file.open(QIODevice::ReadOnly)) {
            LOG_ERR(fmt::format("create ReadRealtimeData file failed {}", filePath.toLocal8Bit().data()));
            return false;
        }

        QDataStream stream(&file);
        stream >> iSideId >> iLineId >> iLayer >> iTimes >> iLineSize >> iLayerSize >> iTimesSize >> iEndLine;
        file.close();
    } catch (std::exception ex) {
        LOG_ERR("create ReadRealtimeData failed ");
        return false;
    }

    m_gSystem.LogInfo("Read Recovered data success");
    return true;
}
