#include "cprocess_laserpowercali_pi.h"
#include "logic.h"
#include "cpowermeter.h"
#include "cpowermeterdriver.h"
#include "claserpathdriver.h"
//#include "claserpathdriver_epulse.h"
#include "cpowermeterdriver_lp.h"
#include "claserdriver_sp.h"
//#include "claserpathdriver_thorlab.h"
CProcess_LaserPowerCaliPI::CProcess_LaserPowerCaliPI(const QString& name,
                                                 int32_t id,
                                                 int32_t iLaserPathID)
    :CProcess_LaserPowerCali(name, id, iLaserPathID)
{
}


void CProcess_LaserPowerCaliPI::before_cali_power(int iFreq, int iEpulse, double dBeamWidth)
{
    Q_UNUSED(dBeamWidth);

	CLaserDriver_Base* pLaserDriver = GET_CARD(LASER_DRIVER, CLaserDriver_Base);
    Q_ASSERT(pLaserDriver);


    CLaserPathDriver* pLPathDriver = GET_CARD(LASERPATH_DRIVER, CLaserPathDriver);
    Q_ASSERT(pLPathDriver);
	if (!_Sys_Instance_.IsSimulator())
	{
		MoveCaliPos();
		//开光
		pLaserDriver->OpenLaserGate(LASER, false);
	}

    CLaserPath* pLpath = GET_CHDIRECT(m_iLaserPathID, CLaserPath);
    Q_ASSERT(pLpath);
    POWERPOSVECTOR Param;
    bool b = pLpath->FindPowerPosVector(iFreq, iEpulse, dBeamWidth, Param);
    if(!b) {
        int iWidth = dBeamWidth * 1000;
        if(m_iLaserPathID != LASERPATH_D_WIDE) {
            iWidth = 0;
        }
        QPair<CPowerIndex, POWERPOSVECTOR> ParamPair(CPowerIndex(iFreq, iEpulse, iWidth), Param);
        pLpath->m_mPowerCali.push_back(ParamPair);
    } else {
        Param.clear();
    }

    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    //移动到光路位置
    pLPathDriver->MoveBeamWidthPos(m_iLaserPathID, dBeamWidth);

    pLaserDriver->SetFreq(LASER, iFreq);
    pLpath->m_vPowerPos.clear();
}
void CProcess_LaserPowerCaliPI::cali_power(int iFreq, int iEpulse, double dWidth, int iStep)
{
    //开光
	CLaserDriver_Base* pLaserDriver = GET_CARD(LASER_DRIVER, CLaserDriver_Base);
	CLaserPathDriver* pPathDriver = GET_CARD(LASERPATH_DRIVER, CLaserPathDriver);
	Q_ASSERT(pPathDriver);
	pPathDriver->OpenShutter(m_iLaserPathID, true);

    CPowerMeter* pPowerMeterHead = GET_CHDIRECT(POWERMETERHEAD, CPowerMeter);
    Q_ASSERT(pPowerMeterHead);
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorDriver);
    CLaserPath* pLpath = GET_CHDIRECT(m_iLaserPathID, CLaserPath);
    Q_ASSERT(pLpath);
    double dStep = m_Variants.get_VData(Var_Process_LaserPowerCali_Step)
        ->toDouble();
    if(DEQUAL0(dStep)) {
		pPathDriver->OpenShutter(m_iLaserPathID, false);
        MQT_THROW_CODESTR(ALARM_POWERCALI_PARAMETER, QObject::trUtf8("Cali laser power err. Step equal 0"));
    }

	double dAngle = 0;
#ifndef CALIBLASERPOWER_PICK_G2
	double dPower = dStep*iStep;
	pLaserDriver->SetPowerRate(LASER, dPower);
#else
	double dPowerMaxAngle = pLpath->m_Variants.get_VData(Var_LaserPath_PowerMaxAngle_ID)
		->toDouble();
	double dAngleOffset = (iStep - 1) * dStep;
	dAngle = dPowerMaxAngle - 45 + dAngleOffset;

	pMotorDriver->absmove(MOTORG2_POWER, dAngle);
#endif
    if(!m_gSystem.IsSimulator()) {
        WaitMeasStable();
    }

    double dPowerR = pPowerMeterHead->m_Variants.get_VData(Var_PowerMeter_MeasResult_ID)
        ->toDouble();

    if(dPowerR > 0.1) {
        PowerParamIndex PosParam;
        PosParam.dIndex = dAngle;
        PosParam.dPower = dPowerR;
        PosParam.dPower_RT = dPowerR / iFreq;
        pLpath->m_vPowerPos.push_back(PosParam);
    }

    m_dLastPower = dPowerR;
    m_iCurFreq = iFreq;
    m_dCurWidth = dWidth;
    m_iCurEpulse = dStep*iStep;
	pPathDriver->OpenShutter(m_iLaserPathID, false);
}

void CProcess_LaserPowerCaliPI::after_cali_power(int iFreq, int iEpulse, double dWidth)
{
	CLaserDriver_Base* pLaserDriver = GET_CARD(LASER_DRIVER, CLaserDriver_Base);
    Q_ASSERT(pLaserDriver);
	CLaserPathDriver* pPathDriver = GET_CARD(LASERPATH_DRIVER, CLaserPathDriver);
	Q_ASSERT(pPathDriver);
    //关闭光闸
	pPathDriver->OpenShutter(m_iLaserPathID, false);

    m_gSystem.m_SysMsg.SetMsg(QObject::trUtf8("%1KHz power cali. "
                                              "finish")
                              .arg(iEpulse));

    CLaserPath* pLpath = GET_CHDIRECT(m_iLaserPathID, CLaserPath);
    Q_ASSERT(pLpath);
    bool b = pLpath->AddPowerPosVector(iFreq, iEpulse, dWidth, pLpath->m_vPowerPos);
    UNUSED(b);
    LOG_INFO(QObject::trUtf8("%1KHz power cali. finish").arg(iFreq).toLocal8Bit().data());
    Q_ASSERT(b);
}

void CProcess_LaserPowerCaliPI::check_power_diff(int iFreq, int iEpulse, double dBeamWidth,
                                               double dRealPower, bool report)
{
    QString msg;
    bool bCanStart;
	CLaserDriver_Base* pLaser = GET_CARD(LASER_DRIVER, CLaserDriver_Base);
    if(!m_gSystem.m_bEnbaleLaserOpen) {
        msg = QObject::tr("Not enable laser");
        MQT_THROW_CODESTR(ALARM_POWERMETER_LASERNOTREADY, msg);
    }

    if(!pLaser->m_bInited) {
        msg = QObject::tr("Laser is not connected");
        MQT_THROW_CODESTR(ALARM_POWERMETER_LASERNOTREADY, msg);
    }

    if(pLaser->m_bError) {
        msg = QObject::tr("Laser is error, please contact manager");
        MQT_THROW_CODESTR(ALARM_POWERMETER_LASERNOTREADY, msg);
    }

    if(pLaser->m_bPreheat) {
        msg = QObject::tr("Laser is preheating,please waiting");
        MQT_THROW_CODESTR(ALARM_POWERMETER_LASERNOTREADY, msg);
    }

    if(!pLaser->m_bEmissionOn) {
        msg = QObject::tr("Laser is not opened");
        MQT_THROW_CODESTR(ALARM_POWERMETER_LASERNOTREADY, msg);
    }

    if(!pLaser->m_bInternalGate) {
        msg = QObject::tr("Laser Inner Gate not opened");
        MQT_THROW_CODESTR(ALARM_POWERMETER_LASERNOTREADY, msg);
    }

    g_SecsManager.SendEvent(EVENT_PowerCheckStarted);


    CLaserPathDriver* pPathDriver = GET_CARD(LASERPATH_DRIVER,
                                             CLaserPathDriver);
    Q_ASSERT(pPathDriver);
    CLaserDriver* pLaserDriver = GET_CARD(LASER_DRIVER, CLaserDriver);

    pLaserDriver->OpenLaserGate(LASER, FALSE);


    MoveCaliPos();
    //pLaserDriver->SetGateSource(LASER,TRUE);
    pPathDriver->MovePathAngle(m_iLaserPathID, iEpulse, dBeamWidth,
                               iFreq, dRealPower);

    //重置0
    CPowerMeterDriver_lp* pPowerMeterDriver = GET_CARD(POWERMETERDRIVER,
                                                       CPowerMeterDriver_lp);
    Q_ASSERT(pPowerMeterDriver);
    CPowerMeter* pPowerMeterHead = GET_CHDIRECT(POWERMETERHEAD,
                                                CPowerMeter);
    Q_ASSERT(pPowerMeterHead);

    //功率计归零
    pPowerMeterDriver->freshresult(POWERMETERHEAD);
    double dPower = pPowerMeterHead->m_Variants.get_VData(Var_PowerMeter_MeasResult_ID)
        ->toDouble();
#ifdef __SIMULATOR__
#else
    if(dPower > 0.2) {
        MQT_THROW_CODESTR(ALARM_POWERMETER_NOTREADY, QObject::trUtf8("Power recheck.Powermeter not ready"));
    }
#endif // __SIMULATOR__

    if(fabs(dPower) > 0.025 || dPower < -0.01) {
        pPowerMeterDriver->SetZero(POWERMETERHEAD);
        for(int i = 0; i < 5; i++) {
            MSleep(1000);
            pPowerMeterDriver->freshresult(POWERMETERHEAD);
            dPower = pPowerMeterHead->m_Variants.get_VData(Var_PowerMeter_MeasResult_ID)
                ->toDouble();
            if(fabs(dPower) < fabs(0.01))
                break;
        }
    }

    CPreciseTime tm;
    tm.StartTime();
	pPathDriver->OpenShutter(m_iLaserPathID, true);

    //关闭吹风
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    pMotorDriver->setio(IO_OUTPUT, OUT_LENS_Blow_Inner, false);
    pMotorDriver->setio(IO_OUTPUT, OUT_LENS_Blow_Outter, false);

    WaitMeasStable();

    MSleep(m_Variants.get_VData(Var_Process_LaserPowerCali_MeasTime)
           ->toInt());

    try {
        pPowerMeterDriver->freshresult(POWERMETERHEAD);
    } catch(const QString& ex) {
		pPathDriver->OpenShutter(m_iLaserPathID, false);
        MQT_THROW_CODESTR(ALARM_POWERMETER_COMMUNICATION, ex);
    }

    tm.StopTime();
    dPower = pPowerMeterHead->m_Variants.get_VData(Var_PowerMeter_MeasResult_ID)
        ->toDouble();

//检查完了关闭激光
	pPathDriver->OpenShutter(m_iLaserPathID, false);

    double percent = fabs(dPower - dRealPower) / dRealPower * 100;
    QString str = QObject::trUtf8("TM:%1,"
                                  "Freq:%2,PWInput:%3W,PWOutput:%4W,"
                                  "Percent:%5%")
        .arg(tm.FormatTime())
        .arg(iFreq)
        .arg(dRealPower)
        .arg(dPower).arg(percent);
    m_gSystem.EvtLog(QObject::trUtf8("PWCHKD"), str);


    g_SecsManager.SetVariable(SVID_LaserCheckPower, QString::number(dPower, 'f', 2).toStdString());
    g_SecsManager.SetVariable(SVID_LaserDetectedPower, QString::number(dRealPower, 'f', 2).toStdString());
    g_SecsManager.SendEvent(EVENT_PowerCheckStarted);
    AddProductData(str);

    double dthres = m_Variants.get_VData(Var_Process_LaserPowerCali_PowerCheck_Thres)->toDouble();
    double dthresPercent = m_Variants.get_VData(Var_Process_LaserPowerCali_PowerCheck_ThresPercent)->toDouble();

    if(fabs(dPower - dRealPower) > dthres && percent > dthresPercent) {
        str += QObject::trUtf8("\nPower check over threshold:%1W Percent:%2%")
            .arg(dthres).arg(dthresPercent);
        MQT_THROW_CODESTR(ALARM_PowerCheckOutrange, str);
    } else {
        m_Variants.get_VData(Var_Process_PowerChekDate)->setValue(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss"));
        m_iLastCheckPowerWafer = 1;
    }
    if(report) {
        m_gSystem.m_SysMsg.SetMsg(str);
    }
}

void CProcess_LaserPowerCaliPI::before_cali_MaxPower(int iFreq, double dWidth)
{
    Q_UNUSED(dWidth);
    m_curMaxPower = 0;
    m_lastCaliPower = 0;
    m_bCaliMaxPowerUp = true;
    m_findMaxPower = false;

	CLaserDriver_Base* pLaserDriver = GET_CARD(LASER_DRIVER, CLaserDriver_Base);
    Q_ASSERT(pLaserDriver);
    CLaserPath* pLpath = GET_CHDIRECT(m_iLaserPathID, CLaserPath);
    Q_ASSERT(pLpath);

    MoveCaliPos();
    //开光
    pLaserDriver->OpenLaserGate(LASER, true);
    //pLaserDriver->SetGateSource(LASER,true);

    if(RTN_FAILED1 == pLaserDriver->SetFreq(LASER, iFreq)) {
        MQT_THROW_CODESTR(ALARM_LASER_COMMUNICATION, QObject::tr("Before cali. max power set laser freq parameter failed."));
    }
}

void CProcess_LaserPowerCaliPI::cali_MaxPower(int iFreq, double dWidth, int iStep)
{
	CLaserDriver_Base* pLaserDriver = GET_CARD(LASER_DRIVER, CLaserDriver_Base);
	Q_ASSERT(pLaserDriver);
    double dStep = m_Variants.get_VData(Var_Process_LaserPowerCali_Step)->toDouble();

    //if(m_findMaxPower) return;
    //if(iStep>1 && iStep%int(1000/(dStep-0.001)/1000)!=0 && !m_bCaliMaxPowerUp) return; //如果在下降过程中，则以1度的速度，快速下降
    UNUSED(iFreq); UNUSED(dWidth);
    CPowerMeter* pPowerMeterHead = GET_CHDIRECT(POWERMETERHEAD, CPowerMeter);
    Q_ASSERT(pPowerMeterHead);
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorDriver);
    CLaserPath* pLpath = GET_CHDIRECT(m_iLaserPathID, CLaserPath);
    Q_ASSERT(pLpath);
    //步进

    double dBaseAngle = 0;
    dBaseAngle = pLpath->m_Variants.get_VData(Var_LaserPath_PowerMaxAngle_ID)
        ->toDouble();
    if(DEQUAL0(dStep)) {
        MQT_THROW_STR(QObject::trUtf8("Cali laser power err. Step equal 0"));
    }
#ifndef CALIBLASERPOWER_PICK_G2
    //TODO:衰减器运动，直到检测到最大值
    double dAngleOffset = dStep * (iStep - 1);

	pLaserDriver->SetPowerRate(LASER, dAngleOffset);
#else
	double dAngle = 0;
	double dAngleOffset = -45 + dStep * (iStep - 1);
	dAngle = dBaseAngle + dAngleOffset;
	pMotorDriver->absmove(MOTORG2_POWER, dAngle);
#endif
    WaitMeasStable();

    CPowerMeterDriver* pPowerMeterDriver = GET_CHDIRECT(POWERMETERDRIVER, CPowerMeterDriver);
    Q_ASSERT(pPowerMeterHead);
    pPowerMeterDriver->freshresult(POWERMETERHEAD);
    double dPower = pPowerMeterHead->m_Variants.get_VData(Var_PowerMeter_MeasResult_ID)
        ->toDouble();


    PowerParamIndex PosParam;
    PosParam.dIndex = dAngleOffset;
    PosParam.dPower = dPower;

    pLpath->m_vPowerPos.push_back(PosParam);

    m_iCurFreq = iFreq;
    m_dCurWidth = dWidth;
}

void CProcess_LaserPowerCaliPI::after_cali_MaxPower(int iFreq, double dWidth)
{
	CLaserDriver_Base* pLaserDriver = GET_CARD(LASER_DRIVER, CLaserDriver_Base);
    Q_ASSERT(pLaserDriver);
    CLaserPath* pLpath = GET_CHDIRECT(m_iLaserPathID, CLaserPath);
    Q_ASSERT(pLpath);
	CLaserPathDriver* pPathDriver = GET_CARD(LASERPATH_DRIVER, CLaserPathDriver);
	Q_ASSERT(pPathDriver);
    double dBaseAngle = 0;
    dBaseAngle = pLpath->m_Variants.get_VData(Var_LaserPath_PowerMaxAngle_ID)
        ->toDouble();

	pPathDriver->OpenShutter(m_iLaserPathID, false);

    int isize = pLpath->m_vPowerPos.size();
    if(!isize) {
        MQT_THROW_CODESTR(ALARM_POWERCALI_PARAMETER, QObject::trUtf8("have not any cali data"));
    }
    double dmax = pLpath->m_vPowerPos[0].dPower;
    int imax = 0;
    for(int i = 0; i < isize; i++) {
        if(pLpath->m_vPowerPos[i].dPower > dmax) {
            imax = i;
            dmax = pLpath->m_vPowerPos[i].dPower;
        }
    }
    double dangle = dBaseAngle + pLpath->m_vPowerPos[imax].dIndex;
    pLpath->m_Variants.get_VData(Var_LaserPath_PowerMaxAngle_ID)->setValue(dangle);

    m_gSystem.m_SysMsg.SetMsg(QObject::trUtf8("%1KHz %2mm Width MaxPower angle cali. "
                                              "finish")
                              .arg(iFreq).arg(dWidth));
}


