#include "cprocess_locate.h"
#include "logic.h"
#include "cafindeage.h"
#include "cmworktable.h"
#include "caffinetrans.h"
#define MOVE_STEPRATE 0.7
static void circleLeastFit(QMMutexVector<CMPointd> &points,
						   double &center_x,
						   double &center_y,
						   double &radius)
{
	center_x = 0.0f;
	center_y = 0.0f;
	radius = 0.0f;
	Q_ASSERT(points.size() >= 3);

	double sum_x = 0.0f, sum_y = 0.0f;
	double sum_x2 = 0.0f, sum_y2 = 0.0f;
	double sum_x3 = 0.0f, sum_y3 = 0.0f;
	double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;

	int N = points.size();
	for (int i = 0; i < N; i++)
	{
		double x = points[i].tX;
		double y = points[i].tY;
		double x2 = x * x;
		double y2 = y * y;
		sum_x += x;
		sum_y += y;
		sum_x2 += x2;
		sum_y2 += y2;
		sum_x3 += x2 * x;
		sum_y3 += y2 * y;
		sum_xy += x * y;
		sum_x1y2 += x * y2;
		sum_x2y1 += x2 * y;
	}

	double C, D, E, G, H;
	double a, b, c;

	C = N * sum_x2 - sum_x * sum_x;
	D = N * sum_xy - sum_x * sum_y;
	E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
	G = N * sum_y2 - sum_y * sum_y;
	H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
	a = (H * D - E * G) / (C * G - D * D);
	b = (H * C - E * D) / (D * D - G * C);
	c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

	center_x = a / (-2);
	center_y = b / (-2);
	radius = sqrt(a * a + b * b - 4 * c) / 2;

}

CProcess_Locate::CProcess_Locate(const QString &name, int32_t id)
    :CMProcess(name,id)
{
	m_iLocateCamID=CAM2X;
	m_iLocateLightDirectID=LIGHT_DIRECT2X;
	m_iLocateLightSideID=LIGHT_SIDE2X;
    m_iCurLocateIndex=0;
    m_Variants.add_Variant(Var_Process_Locate_LocateNum,
                           QObject::trUtf8("LocateNum"),int(4),false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Process_Locate_MaxDiameterError,
                           QObject::trUtf8("MaxDiameterError"),double(0.5),false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Process_Locate_TowardOut,
                           QObject::trUtf8("TowardOut"),false,false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Process_Locate_SearchStep,
                           QObject::trUtf8("SearchRange"),double(1.5),false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Process_Locate_SearchCount,
                           QObject::trUtf8("SearchCount"),int(6),false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Process_Locate_StopSleep,
                           QObject::trUtf8("StopSleep"),int(200),false,
                           Variant::TOXML);
    m_Variants.add_Variant(Var_Process_Locate_LightDirect,
                           QObject::trUtf8("LightDirect"),int(0),false,
                           Variant::TOBOTH);
    m_Variants.add_Variant(Var_Process_Locate_LightSide,
                           QObject::trUtf8("LightSide"),int(0),false,
                           Variant::TOBOTH);

	m_Variants.add_Variant(Var_Location_Exposure,
							QObject::trUtf8("LocationExposure"),int(1500),false,
							Variant::TOFILE);
	m_Variants.add_Variant(Var_Location_Gain,
							QObject::trUtf8("LocationGain"),int(0),false,
							Variant::TOFILE);

    m_Variants.add_Variant(Var_Process_Locate_CenterX,
                           QObject::trUtf8("LocationCenterX"), double(0), false,
                           Variant::TOFILE);
    m_Variants.add_Variant(Var_Process_Locate_CenterY,
                           QObject::trUtf8("LocationCenterY"), double(0), false,
                           Variant::TOFILE);

    m_Variants.add_Variant(Var_Process_Locate_TurnAngle,
                           QObject::trUtf8("LocationTurnAngle"), double(0), false,
                           Variant::TOXML);


    m_LocateVector.resize(4);
    for(int i=0;i<4;i++)
    {
        m_imageContainer[i].name=QObject::trUtf8("Edge%1").arg(i+1);
    }
}

CProcess_Locate::~CProcess_Locate()
{

}

void CProcess_Locate::ClearContainer()
{
    m_imageContainer[0].ClearImage();
    m_imageContainer[1].ClearImage();
    m_imageContainer[2].ClearImage();
    m_imageContainer[3].ClearImage();

}

void CProcess_Locate::do_msg(int iID, QVariantList &vlInput,
                             V_MsgOutput *pvlOutput)
{
    UNUSED(pvlOutput);
    int32_t isize=vlInput.size();
    switch(iID)
    {
    case CALCULOCATEVECTOR:
        if(isize>=0)
        {
            CalcuLocateVector();
        }
        break;
    case FITCELL:
        if(isize>=0)
        {
            FitCell();
        }
        break;
    case MOVEPOS:
        if(isize>=2)
        {
            MovePos(vlInput[0].toInt(),vlInput[1].toInt());
        }
        break;
    case SEARCHPOS:
        if(isize>=2)
        {
            SearchPos(vlInput[0].toInt(),vlInput[1].toBool());
        }
        break;
    case SETPOSMANUAL:
        if(isize>=1)
        {
            SetPosManual(vlInput[0].toInt());
        }
        break;
    case SETLOCATELIGHT:
        if(isize>=0)
        {
            SetLocateLight();
        }
        break;
	case SETLOCATECAM:
		if(isize>=0)
		{
			SetLocateCam();
		}
		break;
    }
}

//计算定位的四个点
int CProcess_Locate::CalcuLocateVector()
{
	CMProduct* pProduct=GET_CHDIRECT(PRODUCT,CMProduct);
	Q_ASSERT(pProduct);
	//回定4个点定位，所以这下面不需要了
//    int iLocateNum=m_Variants.get_VData(Var_Process_Locate_LocateNum)
//            ->toInt();

//	if (iLocateNum<3)
//	{
//        MQT_THROW_STR(QObject::trUtf8("cell Locate Process Err. locate num :%1 less then 3")
//					  .arg(iLocateNum));
//    }
//    m_LocateVector.clear();
//    m_LocateVector.resize(iLocateNum);
    int iLocateNum=4;
    /*double dRadius= pProduct->m_Variants.get_VData(Var_Product_Diameter_ID)
            ->toDouble()/2;*/
	//使用测量直径进行判断
	double dRadius= pProduct->m_Variants.get_VData(Var_MProduct_MeasureDiameter)
		->toDouble()/2;
    double dLength= pProduct->m_Variants.get_VData(Var_MProduct_FlatLength_ID)
            ->toDouble();

    double dCenterX = m_Variants.get_VData(Var_Process_Locate_CenterX)->toDouble();
    double dCenterY = m_Variants.get_VData(Var_Process_Locate_CenterY)->toDouble();
    if(DEQUAL0(dRadius))
    {
        MQT_THROW_CODESTR(ALARM_MAIN_LOGIC_PARAM,QObject::trUtf8("cell Locate Process Err.cell diameter is Zero") );
    }
    int32_t flatPos=pProduct->m_Variants.get_VData(Var_MProduct_FlatPos_ID)
            ->toInt();

    if (dLength>dRadius*2)
	{
		dLength=dRadius*2-1;
	}
	double angle=DEG(asin( dLength/2/dRadius));
	double dAngleStep=(360-angle*2)/(iLocateNum);
	double dStartAngle=0;
	double angleOffset = 0;
    if(!DEQUAL0(dLength))
    {
        if(flatPos==MYLEFT) dStartAngle=180+angle;  //左下角
        else if(flatPos==MYBOTTOM) dStartAngle=-90+angle;   //右下角
        else if(flatPos==MYRIGHT) dStartAngle=angle;    //右上角
        else if(flatPos==MYTOP) dStartAngle=90+angle;   //左上角
    }

	dStartAngle+=dAngleStep/2;
    bool_t towardout=m_Variants.get_VData(Var_Process_Locate_TowardOut)
            ->toBool();

	for (int i=0;i<iLocateNum;i++)
	{
		double dAnglePos=dStartAngle+(i)*dAngleStep + angleOffset;
		m_LocateVector[i].WPDP.tX=cos(RAD(dAnglePos))*dRadius + dCenterX;
		m_LocateVector[i].WPDP.tY=sin(RAD(dAnglePos))*dRadius + dCenterY;
        if(towardout)
        {
            m_LocateVector[i].dAngle=dAnglePos;
        }
        else
        {
            m_LocateVector[i].dAngle=dAnglePos+180;
        }
        m_LocateVector[i].bHasLocated=FALSE;
        m_LocateVector[i].img.resetImage();
        m_imageContainer[i].ClearImage();

        if (m_LocateVector[i].dAngle > 360)
        {
            m_LocateVector[i].dAngle -= 360;
        }
	}

    //交换位置到左上，右上，右下，左下
    //适应机台的逻辑坐标
        /*std::swap(m_LocateVector[3], m_LocateVector[2]);
    std::swap(m_LocateVector[1], m_LocateVector[2]);*/
    std::swap(m_LocateVector[0], m_LocateVector[1]);

    /*LOCATEPOS temp=m_LocateVector[0];
    m_LocateVector[0]=m_LocateVector[1];
    m_LocateVector[1]=temp;*/

    
	return MSG_FINISHED;
}


int CProcess_Locate::FitCell()
{
	CMProduct* pProduct=GET_CHDIRECT(PRODUCT,CMProduct);
	Q_ASSERT(pProduct);
	CProductSide* pSideA=GET_CHDIRECT(SIDE_A,CProductSide);
	Q_ASSERT(pSideA);
	CProductSide* pSideB=GET_CHDIRECT(SIDE_B,CProductSide);
    Q_ASSERT(pSideB);
	CMWorkTable* pWorkTable=GET_CHDIRECT(WORKTABLE,CMWorkTable);
	Q_ASSERT(pWorkTable);
    double dProductDiameter=pProduct->m_Variants.get_VData(Var_Product_Diameter_ID)
            ->toDouble();
	//使用测量直径进行计算
	double dDiameter = pProduct->m_Variants.get_VData(Var_MProduct_MeasureDiameter)->toDouble();
	pProduct->m_dRealDiameter=dDiameter;
	pProduct->m_RealCenter.Reset();
	QMMutexVector<CMPointd>  PtVector;
	int iSizeLocate=m_LocateVector.size();
	for (int i=0;i<iSizeLocate;i++)
	{
		if (m_LocateVector[i].bHasLocated)
		{
			PtVector.push_back(m_LocateVector[i].WPDP);
            MqDebug() << "FitCell" << i << " " <<"x:"<< m_LocateVector[i].WPDP.tX <<" Y:"<< m_LocateVector[i].WPDP.tY;
		}
	}
	if (PtVector.size()<3)
	{
		MQT_THROW_CODESTR(ALARM_MAIN_LOCATE_FAILED_PTNotEnough, QObject::trUtf8("Cell Locate Fit Cell Err. "
									  "Useful Point not enough:%1")
					  .arg(PtVector.size()));
		return 0;
	}
	double dRadius;
	CMPointd ptCenter;
	//用找到的前3个定位点拟合圆心
	circleLeastFit(PtVector, ptCenter.tX, ptCenter.tY, dRadius);
    double dMaxDiameterOffSet=m_Variants.get_VData(Var_Process_Locate_MaxDiameterError)
            ->toDouble();
	if (fabs(dRadius*2-dDiameter)>dMaxDiameterOffSet)
    {
        LOG_ERR(fmt::format("Fit Diameter Error: Wafer:{} Diameter:{} Target:{}", m_gSystem.GetMainWaferId(), dRadius * 2, dDiameter));;
		MQT_THROW_CODESTR(ALARM_MAIN_LOCATE_FAILED_FitOutRange, QObject::trUtf8("Cell Locate Fit Cell Err."
									  "Diameter Out Of Range.\n"
									  "Real:%1,Ref:%2,Limit:%3")
									  .arg(dRadius*2)
					  .arg(dDiameter)
					  .arg(dMaxDiameterOffSet));
	}
	pProduct->m_RealCenter=ptCenter;
    pProduct->m_dRealDiameter=dRadius*2;

    CCam* pCam=GET_CHDIRECT(m_iLocateCamID,CCam);
    Q_ASSERT(pCam);
    pCam->m_imageContainer.clearString();
    pCam->m_imageContainer.addString(QObject::trUtf8("X:%1mm")
                                     .arg(ptCenter.tX,0,'f',3));
    pCam->m_imageContainer.addString(QObject::trUtf8("Y:%1mm")
                                     .arg(ptCenter.tY,0,'f',3));
    pCam->m_imageContainer.addString(QObject::trUtf8("Dia:%1mm")
                                     .arg(dRadius*2,0,'f',3));


    double dInitAngleA=pSideA->m_Variants.get_VData(Var_ProductSide_InitAngle)
            ->toDouble();
    double dInitAngleB=pSideB->m_Variants.get_VData(Var_ProductSide_InitAngle)
            ->toDouble();
    double dLoadw=pWorkTable->m_Variants.get_VData(Var_MWorkTable_Load_W_ID)
            ->toDouble();
	//HACK:这个仿射变换跟方向有关系
	CAffineTrans Trans;
    Trans.CalcuParam(CMPointd(0,0), -(dInitAngleA-dLoadw), CMPointd(1.0,1.0));
	pSideA->m_realCenter= Trans.AffineSrctoDest(pProduct->m_RealCenter);
    Trans.CalcuParam(CMPointd(0,0), -(dInitAngleB-dLoadw), CMPointd(1.0,1.0));
	pSideB->m_realCenter= Trans.AffineSrctoDest(pProduct->m_RealCenter);
    qDebug()<<"side angle A:"<<dInitAngleA<<"side center A:"
           <<pSideA->m_realCenter.tX<<pSideA->m_realCenter.tY
		   <<"Diameter:" <<dRadius*2;
    qDebug()<<"side angle B:"<<dInitAngleB<<"side center B:"
           <<pSideB->m_realCenter.tX<<pSideB->m_realCenter.tY;

	
	AddProductData("Locate Data:");
	AddProductData(QString("\tMeasureDiameter:%1mm ProductDiameter:%2mm RealDiameter:%3mm").arg(dDiameter).arg(dProductDiameter).arg(pProduct->m_dRealDiameter));
	AddProductData(QString("\tRealCenter:(%1,%2)").arg(pProduct->m_RealCenter.tX).arg(pProduct->m_RealCenter.tY));
	AddProductData(QString("\tCH1 Angle:%1 Center:(%2,%3)").arg(pSideA->m_dLevelAngle).arg(pSideA->m_realCenter.tX).arg(pSideA->m_realCenter.tY));
	AddProductData(QString("\tCH2 Angle:%1 Center:(%2,%3)").arg(pSideA->m_dLevelAngle).arg(pSideA->m_realCenter.tX).arg(pSideA->m_realCenter.tY));
	m_gSystem.EvtLog(QObject::tr("DIAMETER"), QString("X:%1,Y:%2,Dia:%3").arg(pProduct->m_RealCenter.tX).arg(pProduct->m_RealCenter.tY).arg(pProduct->m_dRealDiameter));
    return 0;
}

int CProcess_Locate::FitCellSimulator()
{
    CMProduct* pProduct = GET_CHDIRECT(PRODUCT, CMProduct);
    Q_ASSERT(pProduct);
    CProductSide* pSideA = GET_CHDIRECT(SIDE_A, CProductSide);
    Q_ASSERT(pSideA);
    CProductSide* pSideB = GET_CHDIRECT(SIDE_B, CProductSide);
    Q_ASSERT(pSideB);
    CMWorkTable* pWorkTable = GET_CHDIRECT(WORKTABLE, CMWorkTable);
    Q_ASSERT(pWorkTable);
    double dProductDiameter = pProduct->m_Variants.get_VData(Var_Product_Diameter_ID)
        ->toDouble();
    //使用测量直径进行计算
    double dDiameter = pProduct->m_Variants.get_VData(Var_MProduct_MeasureDiameter)->toDouble();
    pProduct->m_dRealDiameter = dDiameter;
    pProduct->m_RealCenter.Reset();
    return RTN_SUCCESS1;
}

void CProcess_Locate::WriteRecipe(ConfigStreamWriter& streamWriter, int iVersion)
{
    CMProcess::WriteRecipe(streamWriter, iVersion);
}

void CProcess_Locate::ReadRecipe(ConfigStreamReader& streamReader, int iVersion)
{
    CMProcess::ReadRecipe(streamReader, iVersion);
    CalcuLocateVector();
}

//根据index需要进行调整移动到的位置
//iAdjustIndex 0 ~ Var_Process_Locate_SearchCount
void CProcess_Locate::MovePos(int iLocateIndex, int iAdjustIndex)
{
    m_gSystem.changeChannel(m_iLocateCamID,false);
	SetLocateLight();
	SetLocateCam();
	int iSize=m_LocateVector.size();
    if(OUTOFRANGE(iLocateIndex,0,iSize))
    {
		MQT_THROW_CODESTR(ALARM_MAIN_LEVEING_PARAM, QObject::trUtf8("cell locate process err."
                                      "locate id out of range"));
    }

	CMotorDriver* pDriver=GET_CARD(MOTOR_DRIVER,CMotorDriver);
	CMWorkTableDriver* pWorkTableDriver=
			GET_CARD(WORKTABLE_DRIVER,CMWorkTableDriver);

	CMWorkTable* pWorkTable=GET_CHDIRECT(WORKTABLE,CMWorkTable);
	double dAngle=m_LocateVector[iLocateIndex].dAngle;
    double dStep=fabs(m_Variants.get_VData(Var_Process_Locate_SearchStep)
            ->toDouble());

	//如果偶数次往负方向运动
    if(iAdjustIndex%2){dStep=-dStep;}
    double dRelDis=dStep*(iAdjustIndex/2);

    CMPointd Axis,WPDP;
    WPDP=m_LocateVector[iLocateIndex].WPDP;
    WPDP.tX+=cos(RAD(dAngle))*dRelDis;
    WPDP.tY+=sin(RAD(dAngle))*dRelDis;

	pWorkTableDriver->WtToAxis(m_iLocateCamID,WPDP,Axis);

    double dLoadW=pWorkTable->m_Variants.get_VData(Var_MWorkTable_Load_W_ID)
            ->toDouble();
	double dpt[3]={Axis.tX,Axis.tY,dLoadW};
	int axis[3]={MOTORX,MOTORY,MOTORW};
	pDriver->absmove_multi(axis,dpt,3);

}

bool CProcess_Locate::SearchPos(int iLocateIndex, bool breport)
{

    int iSize=m_LocateVector.size();
    if(OUTOFRANGE(iLocateIndex,0,iSize))
    {
        MQT_THROW_CODESTR(ALARM_MAIN_LEVEING_PARAM, QObject::trUtf8("cell locate process err."
                                      "locate id out of range"));
    }
    SetLocateLight();
	SetLocateCam();
    MSleep(m_Variants.get_VData(Var_Process_Locate_StopSleep)
           ->toInt());
    AddStaticToolToCam(  iLocateIndex);
    CImageContainer* pImageContainer=&m_imageContainer[iLocateIndex];

    m_iCurLocateIndex=iLocateIndex;
    int iCamID=m_iLocateCamID;
    CaptureImage( iCamID,   QObject::trUtf8("Locate Before"),iLocateIndex,false);
    CCam* pCam=GET_CHDIRECT(m_iLocateCamID,CCam);
    Q_ASSERT(pCam);
    CAFindEage* pFind=(CAFindEage*)pCam->Find_Algorithm(M_FINDEAGE);
    Q_ASSERT(pFind);
    double dCenterX=pCam->m_Variants.get_VData(Var_Cam_Width_ID)
            ->toInt()/2.0;
    double dCenterY=pCam->m_Variants.get_VData(Var_Cam_Height_ID)
            ->toInt()/2.0;
	CMWorkTableDriver* pWorkTableDriver=
			GET_CARD(WORKTABLE_DRIVER,CMWorkTableDriver);
    Q_ASSERT(pWorkTableDriver);

    AddStaticToolToCam(  iLocateIndex);
    CCamDriver* pCamDriver=GET_CARD(CAM_DRIVER,CCamDriver);
    Q_ASSERT(pCamDriver);
    MSleep(m_Variants.get_VData(Var_Process_Locate_StopSleep)
           ->toInt());
    CMImage Img;
    pCamDriver->grab_image(m_iLocateCamID, &Img);

    pFind->exe(&Img);
	if (!pFind->lcenters.size())
	{
		//CaptureImage( iCamID,   QObject::trUtf8("Locate Failed"),iLocateIndex,false);
        if(breport)
        {
			MQT_THROW_CODESTR(ALARM_MAIN_LOCATE_FAILED_CantFindEage, QObject::trUtf8("Search cell eage(%1) failed")
                          .arg(iLocateIndex));
        }

		return false;
    }
    pFind->showresult(&pCam->m_imageContainer);
    pFind->showragion_static(&pCam->m_imageContainer);
    CMPointd ptRef;
    int32_t iindex;
    ptRef.tX=dCenterX;
    ptRef.tY=dCenterY;

	if(iLocateIndex == 0){
		ptRef.tX = dCenterY;
		ptRef.tY = -dCenterX;
	}
	else if(iLocateIndex == 1){
		ptRef.tX = dCenterY;
		ptRef.tY = dCenterX;
	}else if(iLocateIndex == 2){
		ptRef.tX = -dCenterX;
		ptRef.tY = -dCenterY;
	}else if(iLocateIndex == 3){
		ptRef.tX = dCenterX;
		ptRef.tY = -dCenterY;
	}

	MqDebug()<<(QString::fromUtf8("Cell Locate SearchPos:"
								 "dRefenceX:%1,dRefenceY:%2")
			   .arg(ptRef.tX).arg(ptRef.tY));
    pFind->get_mostnear_point(m_LocateVector[iLocateIndex].PixelResultPos,
                              iindex,
                              ptRef);

	get_nearnest_point(m_LocateVector[iLocateIndex].dAngle,dCenterX, dCenterY,pFind->lcenters, m_LocateVector[iLocateIndex].PixelResultPos);

    MqDebug() << (QString::fromUtf8("Cell Locate PixelResultPos:"
                                    "%3:X:%1,Y:%2")
                  .arg(m_LocateVector[iLocateIndex].PixelResultPos.tX).arg(m_LocateVector[iLocateIndex].PixelResultPos.tY)).arg(iLocateIndex);
	pWorkTableDriver->CurCamToWt(m_iLocateCamID,
		m_LocateVector[iLocateIndex].PixelResultPos,
        m_LocateVector[iLocateIndex].WPDP,true);
    m_LocateVector[iLocateIndex].iCamID=m_iLocateCamID;
    m_LocateVector[iLocateIndex].bHasLocated=TRUE;
     m_LocateVector[iLocateIndex].img=Img;
    CaptureImage( iCamID,   QObject::trUtf8("Locate Success"),iLocateIndex,false);
    pFind->showresult_shape(pImageContainer);
    pImageContainer->addShape(new CCross(200,200,
                                         m_LocateVector[iLocateIndex].PixelResultPos),
                         CImageContainer::STATIC_SHAPE);
    m_gSystem.m_bNeedFreshUI=true;
    return true;
}

double CProcess_Locate::get_nearnest_point(double angle, double dCenterX, double dCenterY, QList<CMPointd> lcenters, CMPointd& nearnestPt)
{
	double dLong=(dCenterY)*2;
	angle = angle - 90;
	double k = tan(RAD(angle));
	//外边线的方程为
	//y = kX + C
	//kX -y + C = 0;
	//y = tan(RAD(angle))*x + dLong*cos(RAD(angle));
	//d = |ax+by+c|/sqrt(power(a) + power(b))
	double dA = tan(RAD(angle));
	double dC = dLong*2*sin(RAD(angle-90));
	double sqrtRate = sqrt(dA*dA + 1);
	double mindis = dLong*2;
	int idx = -1;
	double dis = fabs(dC) / sqrtRate;
	for(int i=0;i< lcenters.size();i++){

		double x0 = lcenters[i].tX-dCenterX;
		double y0 = dCenterY - lcenters[i].tY;
		double dis = fabs(dA*x0 - y0 + dC) / sqrtRate;
		if(dis<mindis){
			idx = i;
			mindis = dis;
		}
		//qDebug() << "lcenters[i].tX" << lcenters[i].tX << "lcenters[i].tY" << lcenters[i].tY << "X0" << x0 << "y0" << y0 << "dis" << dis << endl;

	}
	
	nearnestPt = lcenters[idx];
	//qDebug() << "-----Min dis" << mindis;
	return mindis;
}

void CProcess_Locate::SetPosManual(int iLocateIndex)
{
    int iSize=m_LocateVector.size();
    if(OUTOFRANGE(iLocateIndex,0,iSize))
    {
		MQT_THROW_CODESTR(ALARM_MAIN_LEVEING_PARAM, QObject::trUtf8("cell locate process err."
                                      "locate id out of range"));
    }
    m_iCurLocateIndex=iLocateIndex;
	CMWorkTableDriver* pWorkTableDriver=
			GET_CARD(WORKTABLE_DRIVER,CMWorkTableDriver);
	CCam* pCam=GET_CHDIRECT(m_iLocateCamID,CCam);
	Q_ASSERT(pCam);

    double dCenterX=pCam->m_Variants.get_VData(Var_Cam_Width_ID)
            ->toInt()/2.0;
    double dCenterY=pCam->m_Variants.get_VData(Var_Cam_Height_ID)
            ->toInt()/2.0;
	m_LocateVector[iLocateIndex].iCamID=m_iLocateCamID;
	m_LocateVector[iLocateIndex].PixelResultPos.tX=dCenterX;
	m_LocateVector[iLocateIndex].PixelResultPos.tY=dCenterY;
	pWorkTableDriver->CurCamToWt(m_iLocateCamID,
		m_LocateVector[iLocateIndex].PixelResultPos,
        m_LocateVector[iLocateIndex].WPDP,true);
    m_LocateVector[iLocateIndex].bHasLocated=true;

    CImageContainer* pImageContainer=&m_imageContainer[0];
    if(iLocateIndex<4)
    {
        pImageContainer=&m_imageContainer[iLocateIndex];
    }
    int iCamID=m_iLocateCamID;
    CaptureImage( iCamID,  QObject::trUtf8("Locate Manual"),iLocateIndex,true);
    m_gSystem.m_bNeedFreshUI=true;
}

//设置定位光源
void CProcess_Locate::SetLocateLight()
{
    int idirect= m_Variants.get_VData(Var_Process_Locate_LightDirect)
            ->toInt();
    int iside= m_Variants.get_VData(Var_Process_Locate_LightSide)
            ->toInt();
    m_gSystem.SetCamLight(m_iLocateCamID,idirect,true,false);
    m_gSystem.SetCamLight(m_iLocateCamID,iside,false,false);
}

void CProcess_Locate::SetLocateCam()
{
	int exposure = m_Variants.get_VData(Var_Location_Exposure)
		->toInt();
	int gain = m_Variants.get_VData(Var_Location_Gain)
		->toInt();
	m_gSystem.SetCamParam(m_iLocateCamID, exposure, gain);
}

void CProcess_Locate::CaptureImage(int iCamID, QString str, int iIndex, bool bSetMark)
{
    CCamDriver* pCamDriver=GET_CARD(CAM_DRIVER,CCamDriver);
    Q_ASSERT(pCamDriver);
    /*CMImage& img=m_LocateVector[iIndex].img;
    CImageContainer* pImageContainer=&m_imageContainer[iIndex];
	{
		QMutexLocker l(&pImageContainer->mutex);
		pCamDriver->grab_image(iCamID, &img);
	}*/
	CMImage img;
	pCamDriver->grab_image(iCamID, &img);

	CImageContainer* pImageContainer=&m_imageContainer[iIndex];
    pImageContainer->SetImage(img,bSetMark);
    pImageContainer->clearShape(-1);
    pImageContainer->clearString();
    pImageContainer->addString(str);
}

void CProcess_Locate::AddStaticToolToCam(int iLocateIndex)
{
    CCam* pCam=GET_CHDIRECT(m_iLocateCamID,CCam);
    Q_ASSERT(pCam);
    CAFindEage* pFind=(CAFindEage*)pCam->Find_Algorithm(M_FINDEAGE);
    Q_ASSERT(pFind);

    double dAngle=m_LocateVector[iLocateIndex].dAngle;
    double dWidth=pCam->m_Variants.get_VData(Var_Cam_Width_ID)
            ->toInt();
    double dHeight=pCam->m_Variants.get_VData(Var_Cam_Height_ID)
            ->toInt();
    double dCenterX=dWidth/2.0;
    double dCenterY=dHeight/2.0;
    double dLong=(dCenterY*2)*MOVE_STEPRATE;
    pFind->ragion_area_rectr=CRectRot(dLong,dLong/2,CMPointd(dCenterX,dCenterY),
                                      dAngle);
    pFind->showragion(&pCam->m_imageContainer);
    pFind->showragion(&m_imageContainer[iLocateIndex]);
}

void CProcess_Locate::Serialize(QDataStream &stream, bool bwrite, int iVersion)
{
    CMProcess::Serialize(stream,bwrite, iVersion);

    if(!bwrite)
    {
        CalcuLocateVector();
    }
}

