#include "claserpathdriver.h"
#include "claserpath.h"
#include "logic_local_def.h"
#include "cmotordriver_base.h"
#include "claserdriver_sp.h"
#include "csystem_external.h"
#include "logic_io_define.h"
#include "cdhgripper_rs485.h"
#include "base.h"
#include "logic_eap.h"
CLaserPathDriver::CLaserPathDriver(const QString& name, int32_t id)
    :CDriverCard(name, id)
{
    m_Variants.add_Variant(Var_GateStartDelay,
                           QObject::trUtf8("GateStartDelay"), double(3.08), false,
                           Variant::TOXML, QStringList(), "ms");

    m_Variants.add_Variant(Var_GateEndDelay,
                           QObject::trUtf8("GateEndDelay"), double(3.82), false,
                           Variant::TOXML, QStringList(), "ms");

    m_Variants.add_Variant(Var_LensFLow_VolumeCheck_WaitTime,
                           QObject::trUtf8("LensFlowVolumeCheckWaitTime"), int(200), false,
                           Variant::TOXML, QStringList(), "ms");

    m_Variants.add_Variant(Var_LensFlow_Volume_MinThres,
                           QObject::trUtf8("LensFlowVolumeMinThres"), double(45), false,
                           Variant::TOXML);

    m_Variants.add_Variant(Var_LensFlow_Volume_MaxThres,
                           QObject::trUtf8("LensFlowVolumeMaxThres"), double(48), false,
                           Variant::TOXML);
}

CLaserPathDriver::~CLaserPathDriver()
{

}

int32_t CLaserPathDriver::MovePathAngle(int pathid, int iEpulse, double dBeamWidth, int iFreq, double dPower, bool bWait/*=true*/)
{
#ifndef __USE_D_NARROW__
    if (pathid == LASERPATH_D_NARROW)
    {
        MQT_THROW_CODESTR(ALARM_RESERVED, QObject::tr("Not support") + _SysExt_Instance.m_mapNameList[pathid]);
    }
#endif
#ifndef __USE_S_NARROW__
    if (pathid == LASERPATH_S_NARROW)
    {
        MQT_THROW_CODESTR(ALARM_RESERVED, QObject::tr("Not support") + _SysExt_Instance.m_mapNameList[pathid]);
    }
#endif
#ifndef __USE_L_NARROW__
    if (pathid == LASERPATH_L_NARROW)
    {
        MQT_THROW_CODESTR(ALARM_RESERVED, QObject::tr("Not support") + _SysExt_Instance.m_mapNameList[pathid]);
    }
#endif

#ifndef __USE_D_WIDE__
    if (pathid == LASERPATH_D_WIDE)
    {
        MQT_THROW_CODESTR(ALARM_RESERVED, QObject::tr("Not support") + _SysExt_Instance.m_mapNameList[pathid]);
    }
#endif
    //if(m_gSystem.IsDebug()) return RTN_SUCCESS1;
    if (pathid == LASERPATH_L_NARROW || pathid == LASERPATH_S_NARROW)
    {
        dBeamWidth = 0;
    }

    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorDriver);
    CLaserPath* pLaserPath = GET_CHDIRECT(pathid, CLaserPath);
    Q_ASSERT(pLaserPath);
    CLaserDriver* pLaserDriver = GET_CARD(LASER_DRIVER, CLaserDriver);
    Q_ASSERT(pLaserDriver);

    //! 控制光闸
	OpenShutter(pathid, true);

	int iOldFreq = iFreq;
	int iOldEpulse = iEpulse;
    //设置频率
    //是否使用固定频率
    if (pLaserPath->m_Variants.get_VData(Var_LaserPath_UseFixFreq)->toBool())
    {
        iFreq = pLaserPath->m_Variants.get_VData(Var_LaserPath_FixFreqValue)->toDouble();
    }
    pLaserDriver->SetFreq(LASER, iFreq);


    //设置Epulse频率
    if (true == pLaserPath->m_Variants.get_VData(Var_LaserPath_UseFixEpulse)->toBool())
    {
        double dEpulse = pLaserPath->m_Variants.get_VData(Var_LaserPath_FixEpulseValue)
            ->toDouble();
        iEpulse = (int)dEpulse;
    }
    CLaserDriver_Base* pLaserDriverBase = static_cast<CLaserDriver_Base*>(pLaserDriver);
    if (nullptr != pLaserDriverBase)
    {
        pLaserDriverBase->SetEpulseFreq(LASER_DRIVER, 1000 * iEpulse);
    }

	iEpulse = iOldEpulse;
	iFreq = iOldFreq;
#pragma region 计算G2位置
    //double dLaserWidth = dDIndex;
    double dPowerAngle = 0;     //功率角度
    //设置功率角度
    //Wide宽度为0时，使用L_Narrow的光路替代
    //S_Narrow宽度为0时，D_Narrow的光路替代， 注意：S_Narrow，D_Narrow的功率点检数据不与宽度相关联S_Narrow理论不需要点检数据
    if (true == pLaserPath->m_Variants.get_VData(Var_LaserPath_UseFixPowerSetting)->toBool())
    {
        dPowerAngle = pLaserPath->m_Variants.get_VData(Var_LaserPath_PowerSet)->toDouble();
    }
    else
    {
        if (DEQUAL0(dBeamWidth))
        {
            CLaserPath* pLaserPathInvoker = pLaserPath;
            if (pathid == LASERPATH_D_WIDE)
            {
#ifdef __USE_L_NARROW__
                pLaserPathInvoker = GET_CHDIRECT(LASERPATH_L_NARROW, CLaserPath);
#endif
            }
            else if (pathid == LASERPATH_S_NARROW)
            {
#ifdef __USE_D_NARROW__
                pLaserPathInvoker = GET_CHDIRECT(LASERPATH_D_NARROW, CLaserPath);
#endif
            }

            dPowerAngle = pLaserPathInvoker->PowerToAngle(iFreq, iEpulse, dBeamWidth, dPower);
        }
        else
        {
            dPowerAngle = pLaserPath->PowerToAngle(iFreq, iEpulse, dBeamWidth, dPower);
        }
    }
#pragma endregion

    //设置光路玻片位置

    pMotorDriver->absmove(MOTORG2_POWER, dPowerAngle, FALSE);

    //光路切换到BeamWidth位置
    MoveBeamWidthPos(pathid, dBeamWidth);

    pMotorDriver->wait_move_finished(MOTORG2_POWER);

    return RTN_SUCCESS1;
}
int32_t CLaserPathDriver::MovePathInitAngle(int pathid)
{
    if (pathid == LASERPATH_L_NARROW || pathid == LASERPATH_D_WIDE) {
        CDHGripperRS485* pHgripDriver = GET_CARD(HGRIPPER_DRIVER, CDHGripperRS485);
        Q_ASSERT(pHgripDriver);
        pHgripDriver->do_msg(CDHGripperRS485::kReset, QVariantList(), nullptr);

        QString str = QObject::tr("GripperDriver Reset");
        _SysExt_Instance.AddProductInfo(m_iID, str, true);

    }

#ifdef __USE_MOTOR_PATH__
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorDriver);
    CLaserPath* pLaserPath = (CLaserPath*)FindChannel(pathid);
    Q_ASSERT(pLaserPath);
    double dPowerMaxAngle = pLaserPath->m_Variants.get_VData(Var_LaserPath_PowerMaxAngle_ID)
        ->toDouble();
    pMotorDriver->absmove(MOTORG2_POWER, dPowerMaxAngle, FALSE);

    if(pathid == LASERPATH_D_WIDE) {
        MoveBeamPos(pathid, 0);
    }
    //pMotorDriver->wait_move_finished(MOTORG1_PCHANGE);
    pMotorDriver->wait_move_finished(MOTORG2_POWER);

#endif
    return RTN_SUCCESS1;
}

int32_t CLaserPathDriver::MoveBeamWidthPos(int pathid, double dBeamWidth)
{
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorDriver);
    CLaserPath* pLaserPath = GET_CHDIRECT(pathid, CLaserPath);
    Q_ASSERT(pLaserPath);

    //设置线宽
    if (pathid == LASERPATH_D_NARROW || pathid == LASERPATH_S_NARROW)
    {
        DIndexParam param;
        CLaserPath* pLaserPathInvoker = GET_CHDIRECT(LASERPATH_L_NARROW, CLaserPath);
        pLaserPath->GetLineWidthParam(dBeamWidth, param);

        pMotorDriver->absmove(MOTORG5_NARROWINDEX, param.dAngle1, false);
        pMotorDriver->absmove(MOTORG6_NARROW5050, param.dAngle2, false);

        pMotorDriver->wait_move_finished(MOTORG5_NARROWINDEX);
        pMotorDriver->wait_move_finished(MOTORG6_NARROW5050);
    }
    else if (pathid == LASERPATH_D_WIDE || pathid == LASERPATH_L_NARROW)
    {
        MoveBeamPos(pathid, dBeamWidth);
    }

    return RTN_SUCCESS1;
}

int32_t CLaserPathDriver::OpenShutter(int pathid, bool bOpen, bool bWait/*=true*/)
{
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    if (true == bOpen)
    {
        pMotorDriver->set_cylinder(OUT_Shutter_All_Open, IN_LASER_Shutter_All_Down, bOpen);
    }
    else
    {
        pMotorDriver->set_cylinder(OUT_Shutter_All_Open, IN_LASER_Shutter_All_Up, bOpen);
    }
#ifdef __LOWK_SOLUTION__
	if (pathid == LASERPATH_D_NARROW || pathid == LASERPATH_S_NARROW)
	{
		if (!pMotorDriver->get_iodata(IO_INPUT, IN_LASER_Shutter_Narrow_Down, bOpen))
		{
            pMotorDriver->set_cylinder(OUT_Shutter_Wide_Open, IN_LASER_Shutter_Wide_Up, !bOpen);
			pMotorDriver->set_cylinder(OUT_Shutter_Narrow_Open, IN_LASER_Shutter_Narrow_Down, bOpen);
		}
	}
	else if (pathid == LASERPATH_D_WIDE || pathid == LASERPATH_L_NARROW)
	{
		if (!pMotorDriver->get_iodata(IO_INPUT, IN_LASER_Shutter_Wide_Down, bOpen))
		{
			pMotorDriver->set_cylinder(OUT_Shutter_Wide_Open, IN_LASER_Shutter_Wide_Down, bOpen);
			pMotorDriver->set_cylinder(OUT_Shutter_Narrow_Open, IN_LASER_Shutter_Narrow_Up, !bOpen);
		}
	}
#endif
    return RTN_SUCCESS1;
}

int32_t CLaserPathDriver::MoveBeamPos(int pathid, double dBeamWidth)
{
    //只有以下光路通过档板
    Q_ASSERT(pathid == LASERPATH_D_WIDE || pathid == LASERPATH_L_NARROW);

    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorDriver);
    CLaserPath* pLaserPath = (CLaserPath*)FindChannel(pathid);
    Q_ASSERT(pLaserPath);

    int iPos = 1000;
    //判断时否使用固定的卡抓角度
    //TODO:L_Narrow是否需要配置文件
    if (pLaserPath->m_Variants.get_VData(Var_LaserPath_UseFixGripper)->toBool())
    {
        iPos = pLaserPath->m_Variants.get_VData(Var_LaserPath_FixGripperPos)->toInt();
    }
    else
    {
        iPos = static_cast<int>(pLaserPath->GetBeamPos(dBeamWidth));
    }


    CDHGripperRS485* pHgripDriver = GET_CARD(HGRIPPER_DRIVER, CDHGripperRS485);
    Q_ASSERT(pHgripDriver);
    pHgripDriver->SetPosValue(iPos);

    return RTN_SUCCESS1;
}

int32_t CLaserPathDriver::OpenLensBlow(int pathid, bool bOpen)
{
	//开关吹风
	CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
	Q_ASSERT(pMotorDriver);
	pMotorDriver->setio(IO_OUTPUT, OUT_LENS_Blow_Inner, bOpen);
	pMotorDriver->setio(IO_OUTPUT, OUT_LENS_Blow_Outter, bOpen);

    return RTN_SUCCESS1;
}

int32_t CLaserPathDriver::CheckLensBlow(int pathid)
{
#ifndef __LOWK_SOLUTION__

    OpenLensBlow(pathid, true);

    int iWaitTime = m_Variants.get_VData(Var_LensFLow_VolumeCheck_WaitTime)->toInt();
    
    if (iWaitTime > 0) {
        MSleep(iWaitTime);
    }

    CMotorDriver_Base* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver_Base);
	float dFlowVolume = pMotorDriver->get_iodata_analog(IO_INPUT_ANALOG, IN_ANALOG_BEAM_LENS_AIR, true);
	OpenLensBlow(pathid, false);
    double dFlowMin = m_Variants.get_VData(Var_LensFlow_Volume_MinThres)->toDouble();
    double dFlowMax = m_Variants.get_VData(Var_LensFlow_Volume_MaxThres)->toDouble();
    if (!INRANGE(dFlowVolume, dFlowMin, dFlowMax)) {
        MQT_THROW_CODESTR(ALARM_PowerCheckOutrange, QObject::tr("Lens protect module, FlowVolume %1 is out range (%2, %3)").arg(dFlowVolume).arg(dFlowMin).arg(dFlowMax));
	}

#endif
    return RTN_SUCCESS1;
}

int32_t CLaserPathDriver::MoveBeamPosCali(int pathid, double dBeamWidth)
{
	CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
	Q_ASSERT(pMotorDriver);
	CLaserPath* pLaserPath = (CLaserPath*)FindChannel(pathid);
	Q_ASSERT(pLaserPath);

    int iPos = 900;
    if (!DEQUAL0(dBeamWidth))
    {
        LPATHBEAMPOSMAP::iterator  ii = pLaserPath->m_mBeamPos.find(int(dBeamWidth * 1000));
        if (ii != pLaserPath->m_mBeamPos.end())
        {
            iPos = static_cast<int>(*ii);
        }
    }

    CDHGripperRS485* pHgripDriver = GET_CARD(HGRIPPER_DRIVER, CDHGripperRS485);
    Q_ASSERT(pHgripDriver);
    pHgripDriver->SetPosValue(iPos);

    return RTN_SUCCESS1;
}

void CLaserPathDriver::do_msg(int iID, QVariantList& vlInput,
                              V_MsgOutput* pvlOutput)
{
    int isize = vlInput.size();
    switch (iID) {
        case MOVEPATHANGLE:
        {
            if (isize >= 5) {
                MovePathAngle(
                    vlInput[0].toInt(),
                    vlInput[1].toInt(),
                    vlInput[2].toDouble(),
                    vlInput[3].toInt(),
                    vlInput[4].toDouble()
                    , true);
            }
            break;
        }
        case MOVEPATHINITANGLE:
        {
            if (isize >= 1) {
                MovePathInitAngle(vlInput[0].toInt());
            }
            break;
        }
        case MOVEBEAMPOS:
        {
            if (isize >= 2) {
                MoveBeamPos(vlInput[0].toInt(),
                    vlInput[1].toDouble());
            }
            break;
        }
        case OPENLENSPROTECT:
        {
            if (isize >= 2) {
                OpenLensBlow(vlInput[0].toInt(),
                    vlInput[1].toBool());
            }
            break;
        }
        case CHECKLESNPROTECT:
            if (isize >= 1) {
                CheckLensBlow(vlInput[0].toInt());
            }
            break;
    }
    CDriverCard::do_msg(iID, vlInput, pvlOutput);
}