#include "claserpathdriver.h"
#include "claserpath.h"
#include "logic_local_def.h"
#include "cmotordriver.h"
#include "claserdriver_sp.h"
#include "callsystem.h"
#include "logic_io_define.h"
#include "cdhgripper_rs485.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");
}

CLaserPathDriver::~CLaserPathDriver()
{

}

int32_t CLaserPathDriver::MovePathAngle(int pathid, int iEpulse, double dBeamWidth, int iFreq, double dPower, bool bWait/*=true*/)
{
    //if(m_gSystem.IsDebug()) return RTN_SUCCESS1;

    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);

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

    
    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;
    }

    //设置Epulse频率
    CLaserDriver_SP* pLaserDriverSp = static_cast<CLaserDriver_SP*>(pLaserDriver);
    if (nullptr != pLaserDriverSp)
    {
        pLaserDriverSp->SetEpulseFreq(LASER_DRIVER, 1000 * iEpulse);
    }

    //G2偏移到功率位置

    //double dLaserWidth = dDIndex;
    double dPowerAngle = 0;     //功率角度
    double dDIndexAngle = 0;    //只有一个调整的玻片

    //设置功率
    dPowerAngle = pLaserPath->PowerToAngle(iFreq, iEpulse, dBeamWidth, dPower);
    pMotorDriver->absmove(MOTORG2_POWER, dPowerAngle, FALSE);

    //设置线宽
    if (pathid == LASERPATH_D_NARROW || pathid == LASERPATH_S_NARROW)
    {
        DIndexParam param;
        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);
    }

    pMotorDriver->wait_move_finished(MOTORG2_POWER);

    return RTN_SUCCESS1;
}
int32_t CLaserPathDriver::MovePathInitAngle(int pathid)
{
#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::OpenShutter(int pathid, bool bOpen, bool bWait/*=true*/)
{
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    if(bOpen) {
        pMotorDriver->set_cylinder(OUT_Shutter_All_Open, IN_LASER_Shutter_All_Down);

        if(pathid == LASERPATH_D_NARROW || pathid == LASERPATH_S_NARROW) {
            if(!pMotorDriver->get_iodata(IO_INPUT, IN_LASER_Shutter_Narrow_Down, true)) {
                pMotorDriver->set_cylinder(OUT_Shutter_Narrow_Open, IN_LASER_Shutter_Narrow_Down);
                pMotorDriver->set_cylinder(OUT_Shutter_Wide_Open, IN_LASER_Shutter_Wide_Up, FALSE);
            }
        } else if(pathid == LASERPATH_D_WIDE || pathid == LASERPATH_L_NARROW) {
            if(!pMotorDriver->get_iodata(IO_INPUT, IN_LASER_Shutter_Wide_Down, true)) {
                pMotorDriver->set_cylinder(OUT_Shutter_Wide_Open, IN_LASER_Shutter_Wide_Down);
                pMotorDriver->set_cylinder(OUT_Shutter_Narrow_Open, IN_LASER_Shutter_Narrow_Up, FALSE);
            }
        }
    } else {
        //关闭Shutter
        pMotorDriver->set_cylinder(OUT_Shutter_All_Open, IN_LASER_Shutter_All_Down, FALSE);
        pMotorDriver->set_cylinder(OUT_Shutter_Narrow_Open, IN_LASER_Shutter_Narrow_Up, FALSE);
        pMotorDriver->set_cylinder(OUT_Shutter_Wide_Open, IN_LASER_Shutter_Wide_Up, FALSE);
    }
    return RTN_SUCCESS1;
}

int32_t CLaserPathDriver::MoveBeamPos(int pathid, double dWidth)
{
    if(pathid != LASERPATH_D_WIDE) return RTN_SUCCESS1;

    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorDriver);
    CLaserPath* pLaserPath = (CLaserPath*)FindChannel(pathid);
    Q_ASSERT(pLaserPath);
    int iPos = static_cast<int>(pLaserPath->GetBeamPos(dWidth));

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

    return RTN_SUCCESS1;
}

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

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

    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;
    }
    }
    CDriverCard::do_msg(iID, vlInput, pvlOutput);
}
