#include "cmworktabledriver.h"
#include "logic_local_def.h"
#include "cmworktable.h"
//#include "array_str.h"
#include "caffinetrans.h"
#include "cworktable.h"
#include "m_math.h"
#include "claserpath.h"
#include "cmproduct.h"
#include "cmotor.h"
#include "cmotordriver.h"
#include "csystem_external.h"
#include "ccam.h"
#include "logic_io_define.h"
#include "logic_eap.h"
CMWorkTableDriver::CMWorkTableDriver(const QString &name, int32_t id)
    :CWorkTableDriver(name, id)
{

}

CMWorkTableDriver::~CMWorkTableDriver()
{

}

void CMWorkTableDriver::CaliCoord(int32_t iChid,
                                  const CMPointd &axis1, const CMPointd &axis2,
                                  int32_t iTableID)
{
    CWorkTable* pTable = (CWorkTable*)FindChannel(iTableID);
    Q_ASSERT(pTable);
    CCoordTrans* pCoord = pTable->FindCoord(iChid);
    Q_ASSERT(pCoord);
    switch (iChid) {
        case CAM2X:
        case CAM6X:
        case CAMKerf:
        {
            //////////////相机位于X轴上，龙门结构
            //    double X3=-axis1.tX;
            //    double Y3=axis1.tY;
            //    double X1=-axis2.tX;
            //    double Y1=axis2.tY;
            //    pCoord->m_dOffsetX=-(X1+X3-Y3+Y1)/2;
            //    pCoord->m_dOffsetY=(Y1+Y3-X1+X3)/2;

            //////////////相机位于平台上面，不随平台运动，xy平台结构
            double X3 = axis1.tX;
            double Y3 = axis1.tY;
            double X1 = axis2.tX;
            double Y1 = axis2.tY;
            //这是向量化简求解的结果，顺时针旋转
            //需要用到向量旋转公式 
            //逆时针  p': ( x*cos(d)-y*sin(d) , x*sin(d)+y*cos(d)) 
            //顺时针  p': ( x*cos(-d)-y*sin(-d) , x*sin(-d)+y*cos(-d) )
            //2OC=OP1+OP2+P1C+P2C （P1 为第1个点， P2为第2个点 PC 为中心到参考点的向量)
            //P2C=P1C顺时针旋转
            pCoord->m_dOffsetX = (X1 + X3 - Y3 + Y1) / 2;
            pCoord->m_dOffsetY = (Y1 + Y3 - X1 + X3) / 2;
            break;
        }
        case LASERPATH_D_WIDE:
        case LASERPATH_D_NARROW:
        case LASERPATH_S_NARROW:
        case LASERPATH_L_NARROW:
        case DistanceSensor:
        {
            CCoordTrans* pCoord1 = pTable->FindCoord(CAM6X);
            Q_ASSERT(pCoord1);

            double dOffset = 0;
            if (iChid != DistanceSensor)
            {
                //重置激光器的坐标系
                CLaserPath* pLaserPath = GET_CHDIRECT(LASERPATH_D_NARROW, CLaserPath);
                Q_ASSERT(pLaserPath);
                pLaserPath->GetOffset(dOffset);

                pLaserPath->SetOffset(0);
            }

            //axis1 激光划线的点
            //axis2 相机中心关联的点 6倍相机+向量Axis1-Axis2
            pCoord->m_dOffsetX = axis1.tX - axis2.tX + pCoord1->m_dOffsetX;
            pCoord->m_dOffsetY = axis1.tY - axis2.tY + pCoord1->m_dOffsetY - dOffset;
            break;
        }
        default:
            Q_ASSERT(0);
            break;
    }
}

//某个坐标系统转移到平台坐标
void CMWorkTableDriver::AxisToWt(int32_t iChid, const CMPointd &axis,
                                 CMPointd &wt,
                                 int32_t iTableID)
{
    CWorkTable* pTable = (CWorkTable*)FindChannel(iTableID);
    Q_ASSERT(pTable);
    CCoordTrans* pCoord = pTable->FindCoord(iChid);
    Q_ASSERT(pCoord);
    //  xy平台结构
    wt.tX = pCoord->m_dOffsetX - axis.tX;
    wt.tY = pCoord->m_dOffsetY - axis.tY;
    //  龙门结构
    //  wt.tX=-(pCoord->m_dOffsetX-axis.tX);
    //  wt.tY=pCoord->m_dOffsetY-axis.tY;

}
//当前轴坐标，使用iChild坐标系，转移到平台坐标
void CMWorkTableDriver::CurAxisToWt(int32_t iChid, CMPointd &wt, bool bfresh,
                                    int32_t iTableID)
{

    CMotor* pMotorX = GET_CHANNEL(MOTORX, MOTOR_DRIVER, CMotor);
    CMotor* pMotorY = GET_CHANNEL(MOTORY, MOTOR_DRIVER, CMotor);
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorX);
    Q_ASSERT(pMotorY);
    Q_ASSERT(pMotorDriver);
    if (bfresh) {
        pMotorDriver->freshpos(MOTORX);
        pMotorDriver->freshpos(MOTORY);
    }
    CMPointd  axis;
    axis.tX = pMotorX->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
    axis.tY = pMotorY->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
    AxisToWt(iChid, axis, wt, iTableID);

}

void CMWorkTableDriver::WtToAxis(int32_t iChid, const CMPointd &wt,
                                 CMPointd &axis,
                                 int32_t iTableID)
{
    CWorkTable* pTable = (CWorkTable*)FindChannel(iTableID);
    Q_ASSERT(pTable);
    CCoordTrans* pCoord = pTable->FindCoord(iChid);
    Q_ASSERT(pCoord);
    //  xy平台结构
    axis.tX = pCoord->m_dOffsetX - wt.tX;
    axis.tY = pCoord->m_dOffsetY - wt.tY;
    //  龙门结构
    // axis.tX=pCoord->m_dOffsetX+wt.tX;
    // axis.tY=pCoord->m_dOffsetY-wt.tY;
}

//相机坐标转移到平台坐标
void CMWorkTableDriver::CamToWt(int32_t iChid,
                                const CMPointd &axis,
                                const CMPointd &campixel,
                                CMPointd &wt,
                                int32_t iTableID)
{
    CCam* pCam = GET_CHDIRECT(iChid, CCam);
    Q_ASSERT(pCam);
    CMPointd ptLp;
    ptLp = pCam->DPTOLP(campixel);

    //TODO:相机进行X，Y坐标翻转
#if FILPCAM_XY
    CMPointd ptConveterd(ptLp.tY, ptLp.tX);
    ptConveterd.tY = -ptConveterd.tY;

    ptLp = ptConveterd;
#endif

    AxisToWt(iChid, axis, wt, iTableID);
    wt = ptLp + wt;
}

//相机的坐标转换到平台的坐标
void CMWorkTableDriver::CurCamToWt(int32_t iChid,
                                   const CMPointd &campixel,
                                   CMPointd &wt, bool bfresh,
                                   int32_t iTableID)
{
    CMotor* pMotorX = GET_CHANNEL(MOTORX, MOTOR_DRIVER, CMotor);
    CMotor* pMotorY = GET_CHANNEL(MOTORY, MOTOR_DRIVER, CMotor);
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorX);
    Q_ASSERT(pMotorY);
    Q_ASSERT(pMotorDriver);
    if (bfresh) {
        pMotorDriver->freshpos(MOTORX);
        pMotorDriver->freshpos(MOTORY);
    }
    CMPointd  axis;
    axis.tX = pMotorX->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
    axis.tY = pMotorY->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
    CamToWt(iChid, axis, campixel, wt, iTableID);
}

//平台坐标转移到相机坐标系统
void CMWorkTableDriver::WtToCam(int32_t iChid,
                                const CMPointd &axis,
                                const CMPointd &wt,
                                CMPointd &campixel,
                                int32_t iTableID)
{
    CMPointd wt1;
    AxisToWt(iChid, axis, wt1, iTableID);

    CMPointd LP;
    LP.tX = wt.tX - wt1.tX;
    LP.tY = wt.tY - wt1.tY;

    //TODO:相机进行X，Y坐标翻转,
#if FILPCAM_XY
    double tmp = -LP.tX;
    LP.tX = LP.tY;
    LP.tY = tmp;
#endif

    CCam* pCam = GET_CHDIRECT(iChid, CCam);
    Q_ASSERT(pCam);
    campixel = pCam->LPTODP(LP);
}

void CMWorkTableDriver::CurWtToCam(int32_t iChid,
                                   const CMPointd &wt,
                                   CMPointd &campixel, bool bfresh,
                                   int32_t iTableID)
{

    CMotor* pMotorX = GET_CHANNEL(MOTORX, MOTOR_DRIVER, CMotor);
    CMotor* pMotorY = GET_CHANNEL(MOTORY, MOTOR_DRIVER, CMotor);
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pMotorX);
    Q_ASSERT(pMotorY);
    Q_ASSERT(pMotorDriver);
    if (bfresh) {
        pMotorDriver->freshpos(MOTORX);
        pMotorDriver->freshpos(MOTORY);
    }
    CMPointd  axis;
    axis.tX = pMotorX->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
    axis.tY = pMotorY->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
    WtToCam(iChid, axis, wt, campixel, iTableID);
}

void CMWorkTableDriver::Axis1toAxis2(int32_t iChid1,
                                     const CMPointd &axis1,
                                     int32_t iChid2,
                                     CMPointd &axis2,
                                     int32_t iTableID)
{
    CMPointd wt;
    AxisToWt(iChid1, axis1, wt, iTableID);
    WtToAxis(iChid2, wt, axis2, iTableID);
}

void CMWorkTableDriver::CurAxis1toAxis2(int32_t iChid1,
                                        int32_t iChid2,
                                        CMPointd &axis2, bool bfresh,
                                        int32_t iTableID)
{

    CMPointd wt;
    CurAxisToWt(iChid1, wt, bfresh, iTableID);
    WtToAxis(iChid2, wt, axis2, iTableID);
}



int32_t CMWorkTableDriver::MeasTableFoucsPos(int32_t iChid,
                                             int32_t iTableID/*=WORKTABLE*/)
{
    //相机才支持自动校正
    if (iChid == CAM2X || iChid == CAM6X || iChid == CAMKerf) {
        if (_SysExt_Instance.CellExist(OUT_WT_WAFER_VACUUM, IN_WT_WAFER_VACUUM)) {
            MQT_THROW_CODESTR(ALARM_MAIN_SAFE_PROTECT, QObject::trUtf8("Meas Table Foucs Pos err."
                                                                       "cell on worktable"));
        }
        CWorkTable* pTable = (CWorkTable*)FindChannel(iTableID);
        Q_ASSERT(pTable);
        CWorkTable::FOUCSPOSINFO* pFoucs = pTable->FindFoucsPos(iChid);
        Q_ASSERT(pFoucs);

        pFoucs->dFoucsPos = FoucsChannel(iChid, 0, iTableID);

        CCam* pCam = GET_CHDIRECT(iChid, CCam);
        Q_ASSERT(pCam);
        pCam->m_imageContainer.clearString();
        pCam->m_imageContainer.addString(QObject::trUtf8("Table foucs Pos:%1 mm")
                                         .arg(pFoucs->dFoucsPos));
    }
    _SysExt_Instance.m_bNeedFreshUI = true;
    return 0;
}

double CMWorkTableDriver::MeasWaferThick(int32_t iChid, int32_t iTableID)
{
    double dDefoucsDis = 0;
    if (iChid == CAM2X || iChid == CAM6X || iChid == CAMKerf) {
        if (!_SysExt_Instance.IsDebug()) {
            if (!_SysExt_Instance.CellExist(OUT_WT_WAFER_VACUUM, IN_WT_WAFER_VACUUM)) {
                MQT_THROW_CODESTR(ALARM_MAIN_SAFE_PROTECT, QObject::trUtf8("Meas cell thick err."
                                                                           "no cell on worktable"));
            }
        }

        CMProduct* pProduct = GET_CHDIRECT(PRODUCT, CMProduct);
        dDefoucsDis = pProduct->WaferSurfaceToDefoucsDis();

        double dFoucsPos = FoucsChannel(iChid, dDefoucsDis, iTableID);
        dDefoucsDis = ZPosToDefoucsDis(iChid, dFoucsPos, iTableID);
        double dThickness = dDefoucsDis;

        CCam* pCam = GET_CHDIRECT(iChid, CCam);
        Q_ASSERT(pCam);
        pCam->m_imageContainer.clearString();
        QString str = QObject::trUtf8("Cell Thick:%1mm")
            .arg(dThickness);
        pCam->m_imageContainer.addString(str);
        _SysExt_Instance.EvtLog(QString::fromUtf8("FMENT"), str);
    }
    return dDefoucsDis;
}


int32_t CMWorkTableDriver::MeasWaferThickDiff(int32_t iChid, int32_t iTableID)
{
    double dDefoucsDis = 0;
    if (iChid == CAM2X || iChid == CAM6X || iChid == CAMKerf) {
        if (!_SysExt_Instance.CellExist(OUT_WT_WAFER_VACUUM, IN_WT_WAFER_VACUUM)) {
            MQT_THROW_CODESTR(ALARM_MAIN_SAFE_PROTECT, QObject::trUtf8("Meas cell thick err."
                                                                       "no cell on worktable"));
        }

        CMProduct* pProduct = GET_CHDIRECT(PRODUCT, CMProduct);
        dDefoucsDis = pProduct->WaferSurfaceToDefoucsDis();
        double dFoucsPos = FoucsChannel(iChid, dDefoucsDis, iTableID, 1);

        dDefoucsDis = ZPosToDefoucsDis(iChid, dFoucsPos, iTableID);

        pProduct->m_dThickDiff = 0;
        double dThick = pProduct->WaferSurfaceToDefoucsDis();

        //计算测量出的目标焦点与参数设定的焦点的差值
        //通过m_dThickDiff进行补偿
        pProduct->m_dThickDiff = dDefoucsDis - dThick;

        //TODO:如果片厚差异过大，需要报警
        CCam* pCam = GET_CHDIRECT(iChid, CCam);
        Q_ASSERT(pCam);
        pCam->m_imageContainer.clearString();
        QString str = QObject::trUtf8("Cell Thick different:%1mm")
            .arg(pProduct->m_dThickDiff);
        pCam->m_imageContainer.addString(str);
        _SysExt_Instance.EvtLog(QString::fromUtf8("DMENT"), str);
    }
    return 0;
}

int32_t CMWorkTableDriver::MeasTableFoucsPosOffset(int32_t iChid, int32_t iTableID)
{
    if (iChid == CAM2X || iChid == CAM6X || iChid == CAMKerf) {
        if (_SysExt_Instance.CellExist(OUT_WT_WAFER_VACUUM, IN_WT_WAFER_VACUUM)) {
            MQT_THROW_CODESTR(ALARM_MAIN_SAFE_PROTECT, QObject::trUtf8("Meas table foucs offset err."
                                                                       "cell on worktable"));
        }
        CWorkTable* pTable = (CWorkTable*)FindChannel(iTableID);
        Q_ASSERT(pTable);
        CWorkTable::FOUCSPOSINFO* pFoucs = pTable->FindFoucsPos(iChid);
        Q_ASSERT(pFoucs);
        double dPos = FoucsChannel(iChid, 0, iTableID);
        double dPosDiff = dPos - pFoucs->dFoucsPos;

        //TODO:是否要进行修正，修正数据需要经过验证，不能太大，太小就不修正
        //平台焦点检查后，同时使用数据修正其他坐标系的焦点位置
        CWorkTable::FOUCSPOSMAP::iterator ii = pTable->mFoucsPoss.begin();
        for (; ii != pTable->mFoucsPoss.end(); ii++) {
            CWorkTable::FOUCSPOSINFO& info = *ii;
            info.dFoucsPos += dPosDiff;
        }
        CCam* pCam = GET_CHDIRECT(iChid, CCam);
        Q_ASSERT(pCam);
        pCam->m_imageContainer.clearString();
        pCam->m_imageContainer.addString(QObject::trUtf8("Table foucs Pos.:%1mm")
                                         .arg(dPos));
        pCam->m_imageContainer.addString(QObject::trUtf8("Table foucs offset:%1mm. ")
                                         .arg(dPosDiff));
    }
    return 0;
}

double CMWorkTableDriver::FoucsChannel(int32_t iChid, double dDeFoucsDis, int32_t iTableID/*=WORKTABLE*/, int iStep/*=0*/)
{
    //Step1：以当前位置为基准，开始调整
    //Step2：先调大范围，再调小范围
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    CMotor* pMotorZ = GET_CHDIRECT(MOTORZ, CMotor);

    //计算运动到切割表面时的坐标，不应该包含切深
    double dFoucsPosZ = DefoucsDisToZPos(iChid, dDeFoucsDis, iTableID);

    CWorkTable* pTable = (CWorkTable*)FindChannel(iTableID);
    Q_ASSERT(pTable);
    CWorkTable::FOUCSPOSINFO* pFoucs = pTable->FindFoucsPos(iChid);
    Q_ASSERT(pFoucs);
    switch (iChid) {
        case CAM2X:
        case CAM6X:
        case CAMKerf:
        {
            CCam* pCam = GET_CHDIRECT(iChid, CCam);
            Q_ASSERT(pCam);
            CMPointd center;
            double dwidth = pCam->m_Variants.get_VData(Var_Cam_Width_ID)->toInt();
            double dheight = pCam->m_Variants.get_VData(Var_Cam_Height_ID)->toInt();
            center.tX = dwidth / 2;
            center.tY = dheight / 2;
            pCam->m_pAMorphology->m_rect_ROI = CRect((int)dwidth / 3, (int)dheight / 3, center);
            pCam->m_imageContainer.clearShape(CImageContainer::STATIC_SHAPE);
            pCam->m_imageContainer.addShape(new CRect(pCam->m_pAMorphology->m_rect_ROI),
                                            CImageContainer::STATIC_SHAPE);
            int count = 2;
            if (iStep < 0) {
                iStep = 0;
                count = 1;
            }
            for (int i = iStep; i < count; i++) {
                double dBottomPos = dFoucsPosZ;
                double dTopPos = dFoucsPosZ;
                pMotorDriver->setdefaultspeed(MOTORZ);
                if (i == 0) {
                    //高速调一个大范围
                    //从上往下移动
                    dTopPos = dFoucsPosZ + pFoucs->dCaliRange;
                    dBottomPos = dFoucsPosZ - pFoucs->dCaliRange;

                    pMotorDriver->absmove(MOTORZ, dTopPos, true);
                    pMotorDriver->setspeed(MOTORZ, pFoucs->dCaliSpd, 0, 0, 0, 0);
                    pMotorDriver->absmove(MOTORZ, dBottomPos, false);
					int32_t maxscore = 0;
					while (!pMotorDriver->is_move_finished(MOTORZ)) {
						//方差法
						//MqDebug() << "<<<<<";
						//MqDebug() << i << " " << QDateTime::currentDateTime().toString("hh:mm:ss zzz");
						pMotorDriver->freshpos(MOTORZ);
						//MqDebug() << i << " " << QDateTime::currentDateTime().toString("hh:mm:ss zzz");
						double zPos = pMotorZ->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
						int32_t score = pCam->getRoiContrastScore();

						//MqDebug() << i << " " << QDateTime::currentDateTime().toString("hh:mm:ss zzz") << "ZPos: " << zPos << " Score " << score;
						if (score > maxscore) {
							//记录当前最大得分的位置
							dFoucsPosZ = zPos;
							maxscore = score;
							//MqDebug() << "Measure Focus" << dFoucsPosZ;
                            QString info = QString("Focus Step1: Z:%1 Score:%2").arg(dFoucsPosZ).arg(maxscore);
                            _SysExt_Instance.SendLogMessage(info, 0);
						}
						//MqDebug() << ">>>>>>>>>>";

					}
                } else {
                    //低速步长更小调整
                    dTopPos = dFoucsPosZ + pFoucs->dCaliRangeMin;
                    dBottomPos = dFoucsPosZ - pFoucs->dCaliRangeMin;

                    pMotorDriver->absmove(MOTORZ, dFoucsPosZ, true);
                    pMotorDriver->setspeed(MOTORZ, pFoucs->dCaliSpdMin, 0, 0, 0, 0);
					double dCurPos = dTopPos;
					int32_t maxscore = 0;
					while (dBottomPos < dCurPos) {
						pMotorDriver->absmove(MOTORZ, dCurPos, false);
						while (!pMotorDriver->is_move_finished(MOTORZ)) {
						}
						MqDebug() << "Move " << dCurPos;

						pMotorDriver->freshpos(MOTORZ);
						//MqDebug() << i << " " << QDateTime::currentDateTime().toString("hh:mm:ss zzz");
						double zPos = pMotorZ->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
						int32_t score = pCam->getRoiContrastScore();

						//MqDebug() << i << " " << QDateTime::currentDateTime().toString("hh:mm:ss zzz") << "ZPos: " << zPos << " Score " << score;
						if (score > maxscore) {
							//记录当前最大得分的位置
							dFoucsPosZ = zPos;
							maxscore = score;
							MqDebug() << "---" << dFoucsPosZ;

                            QString info = QString("Focus Step2: Z:%1 Score:%2").arg(dFoucsPosZ).arg(maxscore);
                            _SysExt_Instance.SendLogMessage(info, 0);
						}

						dCurPos -= 0.005;
					}
                    //pMotorDriver->absmove(MOTORZ, dBottomPos, false);

                }
                
            }
            //移动到Focus位置
            pMotorDriver->setdefaultspeed(MOTORZ);
            pMotorDriver->absmove(MOTORZ, dFoucsPosZ, true);
            break;
        }
    }
    pMotorDriver->setdefaultspeed(MOTORZ);
    return dFoucsPosZ;
}


double CMWorkTableDriver::ZPosToDefoucsDis(int32_t iChid, double dPosZ,
                                           int32_t iTableID)
{
    CWorkTable* pTable = (CWorkTable*)FindChannel(iTableID);
    Q_ASSERT(pTable);
    const CWorkTable::FOUCSPOSINFO* pFoucs = pTable->FindFoucsPos(iChid);
    Q_ASSERT(pFoucs);
    double dDefoucs = dPosZ - pFoucs->dFoucsPos;
    return dDefoucs;
}

double CMWorkTableDriver::DefoucsDisToZPos(int32_t iChid, double dDefoucs,
                                           int32_t iTableID)
{
    CWorkTable* pTable = (CWorkTable*)FindChannel(iTableID);
    Q_ASSERT(pTable);
    const CWorkTable::FOUCSPOSINFO* pFoucs = pTable->FindFoucsPos(iChid);
    Q_ASSERT(pFoucs);
    double dPosZ = pFoucs->dFoucsPos + dDefoucs;
    return dPosZ;

}

int32_t CMWorkTableDriver::MoveToDeFoucsPos(int32_t iChid, double dDeFoucs,
                                            int32_t iTableID)
{
    double dZpos = DefoucsDisToZPos(iChid, dDeFoucs, iTableID);
    CMotorDriver* pMotorDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    pMotorDriver->absmove(MOTORZ, dZpos);
    return MSG_FINISHED;
}

int32_t CMWorkTableDriver::MoveToCutDepthPos(int32_t iChid, double ddepth, int32_t iTableID)
{

    CMProduct* pProduct = GET_CHDIRECT(PRODUCT, CMProduct);
    double ddefoucs = pProduct->CutDepthToDefoucsDis(ddepth);
    MoveToDeFoucsPos(iChid, ddefoucs, iTableID);
    return RTN_SUCCESS1;
}


int32_t CMWorkTableDriver::LoadPos(int32_t iTableID)
{
    CMotorDriver* pDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pDriver);
    CMWorkTable* pWorkTable = GET_CHDIRECT(iTableID, CMWorkTable);
    Q_ASSERT(pWorkTable);

    double dloadx = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Load_X_ID)->toDouble();
    double dloady = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Load_Y_ID)->toDouble();
    double dloadz = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Load_Z_ID)->toDouble();
    double dloadw = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Load_W_ID)->toDouble();

    double dpt[3] = { dloadx,dloady,dloadw };
    int32_t motorID[3] = { MOTORX,MOTORY,MOTORW };

    pDriver->absmove(MOTORZ, dloadz);
    pDriver->absmove_multi(motorID, dpt, 3);
    return RTN_SUCCESS1;

}
//移动到下料位
int32_t CMWorkTableDriver::UnLoadPos(int32_t iTableID)
{

    CMotorDriver* pDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pDriver);
    CMWorkTable* pWorkTable = GET_CHDIRECT(iTableID, CMWorkTable);
    Q_ASSERT(pWorkTable);

    double dunloadx = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Unload_X_ID)->toDouble();
    double dunloady = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Unload_Y_ID)->toDouble();
    double dunloadz = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Unload_Z_ID)->toDouble();
    double dunloadw = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Unload_W_ID)->toDouble();

    double dpt[3] = { dunloadx,dunloady,dunloadw };
    int32_t motorID[3] = { MOTORX,MOTORY,MOTORW };

    pDriver->absmove(MOTORZ, dunloadz);
    pDriver->absmove_multi(motorID, dpt, 3);

    return RTN_SUCCESS1;
}

//平台移动到手动加载位
int32_t CMWorkTableDriver::ManualLoadPos(int32_t iTableID)
{
    CMotorDriver* pDriver = GET_CARD(MOTOR_DRIVER, CMotorDriver);
    Q_ASSERT(pDriver);
    CMWorkTable* pWorkTable = GET_CHDIRECT(iTableID, CMWorkTable);
    Q_ASSERT(pWorkTable);

    double dloadx = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Manual_LoadX_ID)->toDouble();
    double dloady = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Manual_LoadY_ID)->toDouble();
    double dloadz = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Load_Z_ID)->toDouble();
    double dloadw = pWorkTable->m_Variants.get_VData(Var_MWorkTable_Load_W_ID)->toDouble();

    double dpt[3] = { dloadx,dloady,dloadw };
    int32_t motorID[3] = { MOTORX,MOTORY,MOTORW };

    pDriver->absmove(MOTORZ, dloadz);
    pDriver->absmove_multi(motorID, dpt, 3);
    return RTN_SUCCESS1;
}



void CMWorkTableDriver::do_msg(int iID, QVariantList &vlInput,
                               V_MsgOutput *pvlOutput)
{
    int32_t isize = vlInput.size();
    switch (iID) {
        case MEASTABLEFOUCSPOS:
            //自动校正焦距
            if (isize >= 1) {
                MeasTableFoucsPos(vlInput[0].toInt());
            }
            break;
        case MOVETODEFOUCSPOS:
            if (isize >= 2) {
                MoveToDeFoucsPos(vlInput[0].toInt(), vlInput[1].toDouble());
            }
            break;
        case MEASWAFERTHICK:
            if (isize >= 1) {
                CMProduct* pProduct = GET_CHDIRECT(PRODUCT, CMProduct);
                pProduct->m_dThickDiff = 0;
                MeasWaferThick(vlInput[0].toInt());
            }
            break;
        case MEASTABLEFOUCSPOSOFFSET:
            if (isize >= 1) {
                MeasTableFoucsPosOffset(vlInput[0].toInt());
            }
            break;
        case LOADPOS:
            if (isize >= 0) {
                LoadPos();
            }
            break;
        case UNLOADPOS:
            if (isize >= 0) {
                UnLoadPos();
            }
            break;
        case MANUALLOADPOS:
            if (isize >= 0) {
                ManualLoadPos();
            }
            break;
        default:break;
    }
    CDriverCard::do_msg(iID, vlInput, pvlOutput);
}
