#include "StdAfx.h"
#include "Product.h"
#include "GlobalFunction.h"
#include "LogMgr.h"
#include "ObjComponentMgr.h"
#include "ExceptionMsg.h"
#include "ProductMgr.h"
#include "CommonFlowMgr.h"
#include "DrawSimpleShape.h"


CProduct::CProduct(void)
{
      m_bUsed = true;//是否使用
      m_MaxRotateAng = 0.3;//最大旋转角度
      m_RotateAng = 0;
      m_bLastOne = true;//是否为最后一个
      m_bHasMarkPt3 = false;//是否有mark3
}
CProduct::~CProduct(void)
{
}
void CProduct::Draw(CDC* pDC)
{
     CPen Pen;
     Pen.CreatePen(PS_INSIDEFRAME,0.5,RGB_GREEN1);
     DbRect Rect(m_BasePt,3);
     DrawCrossX(pDC,Pen,Rect);
}
#if 1
//设置定位点理论坐标
void CProduct::SetTheoryMarkPt(Dbxy pt1,Dbxy pt2)
{
    if(pt1.x<pt2.x)//保证左边的点为mark1
    {
         m_TheoryMarkPt1 = pt1;
         m_TheoryMarkPt2 = pt2;
    }
    else
    {
         m_TheoryMarkPt1 = pt2;
         m_TheoryMarkPt2 = pt1;
    }
}

//获取定位点的大致实际坐标
void CProduct::GetMarkCoord(Dbxy &mark1,Dbxy &mark2)
{
      mark1.x = m_TheoryMarkPt1.x;
      mark1.y = m_TheoryMarkPt1.y;
      mark2.x = m_TheoryMarkPt2.x;
      mark2.y = m_TheoryMarkPt2.y;
}
#endif
#if 1
void CProduct::WriteWorkFile(vector<CLab> &LabVec)
{
      LabVec.push_back(CLab(LAB_NULL,m_BasePt.x));
      LabVec.push_back(CLab(LAB_NULL,m_BasePt.y));
      LabVec.push_back(CLab(LAB_NULL,m_bUsed));
      LabVec.push_back(CLab(LAB_NULL,m_TheoryMarkPt1.x));
      LabVec.push_back(CLab(LAB_NULL,m_TheoryMarkPt1.y));
      LabVec.push_back(CLab(LAB_NULL,m_TheoryMarkPt2.x));
      LabVec.push_back(CLab(LAB_NULL,m_TheoryMarkPt2.y));
}
void CProduct::ReadWorkFile(CLabVecRang &LabVecRang)
{
      int idx = LabVecRang.GetStart();
      m_BasePt.x = LabVecRang.GetDouble(idx++);
      m_BasePt.y = LabVecRang.GetDouble(idx++);
      m_bUsed = LabVecRang.GetBool(idx++);
      m_TheoryMarkPt1.x = LabVecRang.GetDouble(idx++);
      m_TheoryMarkPt1.y = LabVecRang.GetDouble(idx++);
      m_TheoryMarkPt2.x = LabVecRang.GetDouble(idx++);
      m_TheoryMarkPt2.y = LabVecRang.GetDouble(idx++);
}
#endif
#if 1
//重设实际定位点
void CProduct::ResetRealMarkPt()
{
    m_RealMarkPt1.x = 0;
    m_RealMarkPt1.y = 0;
    m_RealMarkPt2.x = 0;
    m_RealMarkPt2.y = 0;
    //m_RealMarkPt3.x = 0;
    //m_RealMarkPt3.y = 0;
    //偏移和旋转都要重设
    m_Offset.x = 0;//理论数据映射为平台坐标的偏移X
    m_Offset.y = 0;//理论数据映射为平台坐标的偏移Y
    m_RotateAng = 0;//理论数据映射为平台坐标的旋转角度
}
bool CProduct::IsSetRealMarkPt1()
{
     return (!IsDbxyZero(m_RealMarkPt1));
}
bool CProduct::IsSetRealMarkPt2()
{
     return (!IsDbxyZero(m_RealMarkPt2));
}
//定位数据是否准备好了
bool CProduct::IsMarkReady()
{
	return true;
      Dbxy MarkPt1;
      Dbxy MarkPt2;
      if(gObjComponentMgr->GetTwoMarkPt(MarkPt1,MarkPt2))//有定位点
      {
            return IsSetRealMarkPt1() && IsSetRealMarkPt2();
      }
      else//无定位点
      {
            return gCommonFlowMgr->IsNoMarkCanWork();
      }
}
//设置实际定位点坐标(CCD 的抓取结果)
void CProduct::SetRealMarkPt(Dbxy pt)
{
    if(IsSetRealMarkPt1() && IsSetRealMarkPt2())
    {
         //重设实际定位点
         ResetRealMarkPt();
    }
    //如果没有设置定位点1的情况先设置定位点1
    if(!IsSetRealMarkPt1())
    {
         gLogMgr->WriteDebugLog("SetRealMarkPt1");
         m_RealMarkPt1 = pt;
    }
    //设置了定位点1，没设置定位点2 时设置定位点2
    else if(!IsSetRealMarkPt2())
    {
         gLogMgr->WriteDebugLog("SetRealMarkPt2");
         m_RealMarkPt2 = pt;
         double RotatoAdjustX = gCommonFlowMgr->GetRotatoAdjust();//mark点手动调整量
         m_RealMarkPt2.x += RotatoAdjustX;
    }

    //需要同时设置了理论数据和真实数据的定位点坐标才能计算
    if(!IsSetRealMarkPt1() || !IsSetRealMarkPt2())
         return;

	CalAffinePars();
	return;

    //计算偏移旋转
    CalTheoryToRealPar();

    //根据抓取两个mark 来计算拉伸数据
    if(gCommonFlowMgr->IsbStretchDataToRealSize())
    {
         //计算拉伸实际尺寸拉伸参数
         CalRealStretchPar();
         //把理论定位点坐标拉伸
         StretchPt(m_TheoryMarkPt1,m_StretchParX);
         StretchPt(m_TheoryMarkPt1,m_StretchParY);
         StretchPt(m_TheoryMarkPt2,m_StretchParX);
         StretchPt(m_TheoryMarkPt2,m_StretchParY);
         //重新计算旋转和偏移
         CalTheoryToRealPar();
    }
}

#include<opencv2/opencv.hpp>
using namespace cv;

void CProduct::CalAffinePars()
{
	CString logstr;

	o_TheoryMarkPt1 = m_TheoryMarkPt1 - m_BasePt;
	logstr.Format("TheoryMarkPt1 Coord(%f,%f)", o_TheoryMarkPt1.x, o_TheoryMarkPt1.y);
	gLogMgr->WriteDebugLog(logstr);

	o_TheoryMarkPt2 = m_TheoryMarkPt2 - m_BasePt;
	logstr.Format("TheoryMarkPt2 Coord(%f,%f)", o_TheoryMarkPt2.x, o_TheoryMarkPt2.y);
	gLogMgr->WriteDebugLog(logstr);

	o_TheoryMarkPt3 = m_TheoryMarkPt3 - m_BasePt;
	logstr.Format("TheoryMarkPt3 Coord(%f,%f)", o_TheoryMarkPt3.x, o_TheoryMarkPt3.y);
	gLogMgr->WriteDebugLog(logstr);

	o_RealMarkPt1 = m_RealMarkPt1 - m_BasePt;
	logstr.Format("RealMarkPt1 Coord(%f,%f)", o_RealMarkPt1.x, o_RealMarkPt1.y);
	gLogMgr->WriteDebugLog(logstr);

	o_RealMarkPt2 = m_RealMarkPt2 - m_BasePt;
	logstr.Format("RealMarkPt2 Coord(%f,%f)", o_RealMarkPt2.x, o_RealMarkPt2.y);
	gLogMgr->WriteDebugLog(logstr);

	o_RealMarkPt3 = m_RealMarkPt3 - m_BasePt;
	logstr.Format("RealMarkPt3 Coord(%f,%f)", o_RealMarkPt3.x, o_RealMarkPt3.y);
	gLogMgr->WriteDebugLog(logstr);

	double TheoryDis = CalDistance(o_TheoryMarkPt1, o_TheoryMarkPt2);
	logstr.Format("[Mark1&2理论间距]: [%f]", TheoryDis);
	gLogMgr->WriteDebugLog(logstr);

	double RealDis = CalDistance(o_RealMarkPt1, o_RealMarkPt2);
	logstr.Format("[Mark1&2真实间距]: [%f]", RealDis);
	gLogMgr->WriteDebugLog(logstr);

	double DisDiff = abs(TheoryDis - RealDis);
	logstr.Format(_T("[Mark 间距误差] = [%f]"), DisDiff);
	gLogMgr->WriteDebugLog(logstr);

	if (DisDiff > abs(gProductMgr->GetMaxMarkDisDiff()))
	{
		gTrackWorkFlow1.RadAlamOnOff(true);//报警提示
		ResetRealMarkPt();
		CString LogStr("Mark 间距误差超出范围,定位可能误判!");
		AfxMessageBox(logstr);	
		gTrackWorkFlow1.RadAlamOnOff(false);//报警提示		
		CExceptionMsg Msg;
		Msg.SetMsg(CString(""));
		throw Msg;//抛出异常
	/*	CExceptionMsg Msg;
		Msg.SetMsg(LogStr);
		throw Msg;*/
	}

	Point2f  ThroryCoords[3];
	Point2f  RealCoords[3];

	Mat warp_mat(2, 3, CV_32FC1);

	ThroryCoords[1] = Point2f(o_TheoryMarkPt1.x, o_TheoryMarkPt1.y);
	ThroryCoords[2] = Point2f(o_TheoryMarkPt2.x, o_TheoryMarkPt2.y);
	ThroryCoords[0] = Point2f(o_TheoryMarkPt3.x, o_TheoryMarkPt3.y);

	RealCoords[1] = Point2f(o_RealMarkPt1.x, o_RealMarkPt1.y);
	RealCoords[2] = Point2f(o_RealMarkPt2.x, o_RealMarkPt2.y);
	RealCoords[0] = Point2f(o_RealMarkPt3.x, o_RealMarkPt3.y);

	//得放射变换参数矩阵
	warp_mat = getAffineTransform(ThroryCoords, RealCoords);
	warp_mat.convertTo(warp_mat, CV_32FC1);//不转化时,默认CV_64FC1  后续计算会错误

	//取出6个变换参数
	m_p00 = warp_mat.at<float>(0, 0);
	m_p01 = warp_mat.at<float>(0, 1);
	m_p02 = warp_mat.at<float>(0, 2);

	m_p10 = warp_mat.at<float>(1, 0);
	m_p11 = warp_mat.at<float>(1, 1);
	m_p12 = warp_mat.at<float>(1, 2);


}
//计算真实数据的偏移和旋转值
void CProduct::CalTheoryToRealPar()
{
     gLogMgr->WriteDebugLog("func : CalTheoryToRealPar");
    
     //以第一个点的来计算相对的偏移
     m_Offset.x = m_RealMarkPt1.x - m_TheoryMarkPt1.x;
     m_Offset.y = m_RealMarkPt1.y - m_TheoryMarkPt1.y;
     //得到偏移后的第二个点(理论值)
     Dbxy OffsetPt2 = m_TheoryMarkPt2;
     OffsetPt2.x += m_Offset.x;
     OffsetPt2.y += m_Offset.y;
     
     //特殊处理(避免两个mark 在一条直线上时的角度计算错误)
     if(IsTwoDbEqual(m_RealMarkPt1.x,OffsetPt2.x))
     {
          OffsetPt2.x += 0.001;
     }
     if(IsTwoDbEqual(m_RealMarkPt1.y,OffsetPt2.y))
     {
          OffsetPt2.y += 0.001;
     }
     //以第二个点的旋转来计算相对的角度
     m_RotateAng = CalAngle(m_RealMarkPt1,OffsetPt2,m_RealMarkPt2);
     m_RotateAng *= -1;
     double RotateAng = AngleTo360(m_RotateAng);

     //gLogMgr->SetbWriteDebugLog(true);
     CString LogStr;
     LogStr.Format(_T("[m_Offset.x] = [%f] ,[m_Offset.y] = [%f]"),m_Offset.x,m_Offset.y);
     gLogMgr->WriteDebugLog(LogStr);
     LogStr.Format(_T("[RotateAng] = [%f]"),m_RotateAng);//弧度角
     gLogMgr->WriteDebugLog(LogStr);
     LogStr.Format(_T("[RotateAng360] = [%f]"),RotateAng);
     gLogMgr->WriteDebugLog(LogStr);
     //gLogMgr->SetbWriteDebugLog(false);

     //通过判断实际测量mark1 和mark2 的距离来避免抓取误差
     double TheoryDis = CalDistance(m_TheoryMarkPt1,m_TheoryMarkPt2);//理论距离
     double RealDis = CalDistance(m_RealMarkPt1,m_RealMarkPt2);//实际距离
     double DisDiff = abs(TheoryDis - RealDis);
     LogStr.Format(_T("[Mark 间距误差] = [%f]"),DisDiff);
     gLogMgr->WriteDebugLog(LogStr);
     if(DisDiff > abs(gProductMgr->GetMaxMarkDisDiff()))
     {
		
           gTrackWorkFlow1.RadAlamOnOff(true);//报警提示
           ResetRealMarkPt();
           CString LogStr("Mark 间距误差超出范围,定位可能误判!");
           CExceptionMsg Msg;
           Msg.SetMsg(LogStr);
           throw Msg;
     }
}
//使用默认偏移量
void CProduct::UseDefualtOffset()
{
      m_Offset.x = m_Offset.y = 0;
      m_RotateAng = 0;
}
#endif
#if 1
//获取旋转角度(360 度角)
double CProduct::GetRotateAng()
{
     return AngleTo360(m_RotateAng);
}
//计算拉伸实际尺寸拉伸参数
void CProduct::CalRealStretchPar()
{
     Dbxy ProductScale = CalRealProductScale();//伸缩比例
     DbRect AllObjRect = gObjComponentMgr->GetAllObjRect2();
     SObjOperatePar &StretchParX = m_StretchParX;
     SObjOperatePar &StretchParY = m_StretchParY;
     //计算X 方向拉伸参数
     {
         StretchParX.OpType = _OP_STRETCH;
         StretchParX.BasePt = AllObjRect.GetCenterPt();
         StretchParX.OldSize = AllObjRect.Width();
         StretchParX.Diff = (ProductScale.x-1)*StretchParX.OldSize;
         StretchParX.NewSize = StretchParX.OldSize + StretchParX.Diff;
         StretchParX.xy = _X;
     }
     //计算Y 方向拉伸参数
     {
         StretchParY.OpType = _OP_STRETCH;
         StretchParY.BasePt = AllObjRect.GetCenterPt();
         StretchParY.OldSize = AllObjRect.Height();
         StretchParY.Diff = (ProductScale.y-1)*StretchParY.OldSize;
         StretchParY.NewSize = StretchParY.OldSize + StretchParY.Diff;
         StretchParY.xy = _Y;
     }
}

//理论数据转换为实际数据
void CProduct::TheoryDataToRealData(vector<Dbxy> &vec,Dbxy &Offset)
{
     vector<Dbxy>::iterator iter = vec.begin();
     vector<Dbxy>::iterator iter_end = vec.end();
     for(;iter!=iter_end;iter++)
     {
          (*iter) = TheoryPtToRealPt((*iter));
          (*iter).x += Offset.x;
          (*iter).y += Offset.y;
     }
}

//理论数据转换为实际数据
void CProduct::TheoryDataToRealData(vector<Dbxy> &vec, Dbxy &BaseOffset, Dbxy CutAdjust)
{
	vector<Dbxy>::iterator iter = vec.begin();
	vector<Dbxy>::iterator iter_end = vec.end();
	for (;iter != iter_end;iter++)
	{
		//(*iter) = TheoryPtToRealPt((*iter));
		//(*iter) = (*iter) - m_BasePt;	
		auto x = (*iter).x;
		auto y = (*iter).y;
		double retx = m_p00*x + m_p01*y + m_p02+ CutAdjust.x;
		double rety = m_p10*x + m_p11*y + m_p12+ CutAdjust.y;
		(*iter).x = retx;
		(*iter).y = rety;

		auto temp = BaseOffset;//-m_BasePt;

		(*iter) = (*iter) - temp;
		
		/*(*iter).x += CutAdjust.x;
		(*iter).y += CutAdjust.y;*/
	}
}
void CProduct::ResetAffinePars()
{
	m_p00 = 1;
	m_p01 = 0;
	m_p02 = 0;

	m_p10 = 0;
	m_p11 = 1;
	m_p12 = 0;
}
//获取实际产品的尺寸比例
Dbxy CProduct::CalRealProductScale()
{
     Dbxy ProductScale;
     //三个mark 的情况xy 方向拉伸比例分开计算
     if(m_bHasMarkPt3)//3       
     {
          ProductScale.x = CalRealProductScaleExt(m_TheoryMarkPt1,m_TheoryMarkPt2,m_RealMarkPt1,m_RealMarkPt2);
          if(IsTwoDbEqual(m_TheoryMarkPt1.x,m_TheoryMarkPt3.x))//mark1 和mark3 
          {
                ProductScale.y = CalRealProductScaleExt(m_TheoryMarkPt1,m_TheoryMarkPt3,m_RealMarkPt1,m_RealMarkPt3);
          }
          else
          {
                ProductScale.y = CalRealProductScaleExt(m_TheoryMarkPt2,m_TheoryMarkPt3,m_RealMarkPt2,m_RealMarkPt3);
          }
     }
     else//两个mark 的情况
     {
          double Scale = CalRealProductScaleExt(m_TheoryMarkPt1,m_TheoryMarkPt2,m_RealMarkPt1,m_RealMarkPt2);
          ProductScale.x = ProductScale.y =  Scale;
     }
     CString LogStr;
     LogStr.Format(_T("[拉伸比例x] = [%f]"),ProductScale.x);
     gLogMgr->WriteDebugLog(LogStr);
     LogStr.Format(_T("[拉伸比例y] = [%f]"),ProductScale.y);
     gLogMgr->WriteDebugLog(LogStr);
     
     return ProductScale;
}
double CProduct::CalRealProductScaleExt(Dbxy TheoryMarkPt1,Dbxy TheoryMarkPt2,Dbxy RealMarkPt1,Dbxy RealMarkPt2)
{
     double TheoryMarkDis = CalDistance(TheoryMarkPt1,TheoryMarkPt2);
     double RealMarkDis = CalDistance(RealMarkPt1,RealMarkPt2);
     double Diff = (RealMarkDis-TheoryMarkDis);
     double ProductScale = RealMarkDis/TheoryMarkDis;
     CString LogStr;
     LogStr.Format(_T("[间距理论值] = [%f]"),TheoryMarkDis);
     gLogMgr->WriteDebugLog(LogStr);
     LogStr.Format(_T("[间距实际值] = [%f]"),RealMarkDis);
     gLogMgr->WriteDebugLog(LogStr);
     LogStr.Format(_T("[尺寸误差] = [%f]"),Diff);
     gLogMgr->WriteDebugLog(LogStr);
     return ProductScale;
}
//拉伸
//BasePt 拉伸基准点
//Size 拉伸前的尺寸
//Diff 是旧尺寸和新尺寸的差
void CProduct::StretchPt(Dbxy &Pt,SObjOperatePar &Par)
{
    if(Par.Diff == 0)
        return;
    if(Par.xy == _X)
    {
        Pt.x = Pt.x+Par.Diff*((Pt.x-Par.BasePt.x)/Par.OldSize);
    }
    else
    {
        Pt.y = Pt.y+Par.Diff*((Pt.y-Par.BasePt.y)/Par.OldSize);
    }
}

//根据抓取两个mark 来计算拉伸数据(CenterPt计算的中心点)
void CProduct::StretchDataToRealSize(vector<vector<Dbxy>> &vec)
{
     vector<vector<Dbxy>>::iterator iter = vec.begin();
     vector<vector<Dbxy>>::iterator iter_end = vec.end();
     for(;iter!=iter_end;iter++)
     {
          int size = (*iter).size();
          for(int k=0;k<size;k++)
          {
               Dbxy &pt = (*iter)[k];
               StretchPt(pt,m_StretchParX);
               StretchPt(pt,m_StretchParY);
          }
     }
}
//理论数据转换为实际数据(SrcVec 和DecVec 的大小必须一致)
void CProduct::TheoryDataToRealData(vector<vector<Dbxy>> &SrcVec,vector<vector<Dbxy>> &DecVec,Dbxy &Offset)
{
     gLogMgr->WriteDebugLog("CProduct::TheoryDataToRealData");
     int size1 = SrcVec.size();
     int size2 = DecVec.size();
     if(size1!=size2)
     {
           gLogMgr->WriteDebugLog("SrcVec.size() != DecVec.size()");
           return;
     }
     for(int k=0;k<size1;k++)
     {
          TheoryDataToRealData(SrcVec[k],DecVec[k],Offset);
     }
}
//理论数据转换为实际数据(SrcVec 和DecVec 的大小必须一致)
void CProduct::TheoryDataToRealData(vector<Dbxy> &SrcVec,vector<Dbxy> &DecVec,Dbxy &Offset)
{
     int size1 = SrcVec.size();
     int size2 = DecVec.size();
     if(size1!=size2)
     {
           gLogMgr->WriteDebugLog("SrcVec.size() != DecVec.size()");
           return;
     }
     for(int k=0;k<size1;k++)
     {
          DecVec[k] = TheoryPtToRealPt(SrcVec[k]);
          DecVec[k].x += Offset.x;
          DecVec[k].y += Offset.y;
     }
}
//(TheoryPt 先旋转,再相对偏移)
Dbxy CProduct::TheoryPtToRealPt(Dbxy TheoryPt)
{
    double RotateAng = m_RotateAng;
    Dbxy pt;
    //先旋转----------------------------------
    pt =  RotatoPt(TheoryPt,RotateAng,m_TheoryMarkPt1);//旋转点是理论定位点
    //再加上相对偏移-----------------------
    Dbxy Offset;
    Offset.x = m_Offset.x;
    Offset.y = m_Offset.y;

    pt.x = pt.x + Offset.x;
    pt.y = pt.y + Offset.y;
    return pt;
}
//理论数据转换为实际数据
void CProduct::TheoryDataToRealData(vector<vector<Dbxy>> &vec,Dbxy &Offset)
{
     gLogMgr->WriteDebugLog("CProduct::TheoryDataToRealData");
     vector<vector<Dbxy>>::iterator iter = vec.begin();
     vector<vector<Dbxy>>::iterator iter_end = vec.end();
     for(;iter!=iter_end;iter++)
     {
          TheoryDataToRealData(*iter,Offset);
     }
}


//理论数据转换为实际数据
void CProduct::TheoryDataToRealData(vector<vector<Dbxy>> &vec, Dbxy &BaseOffset, Dbxy CutAdjust)
{
	gLogMgr->WriteDebugLog("CProduct::TheoryDataToRealData");
	vector<vector<Dbxy>>::iterator iter = vec.begin();
	vector<vector<Dbxy>>::iterator iter_end = vec.end();
	for (;iter != iter_end;iter++)
	{
		TheoryDataToRealData(*iter, BaseOffset,CutAdjust);
	}
}
#endif