#include "StdAfx.h"
#include "GratingRuler.h"
#include "Propertie.h"
#include "PropertieMgr.h"
#include "CommPortMgr.h"
#include "LogMgr.h"
#include "BitOperation.h"
#include "AuthorityMgr.h"


#define WE6800_RETURN_BTYE_CNT 17//WE6800 数显表串口协议返回的字节数

CGratingRuler *gGratingRuler = new CGratingRuler;
CGratingRuler::CGratingRuler(void)
{
    m_bIni = false;//光栅尺是否初始化
    m_bUseGratingRuler = false;//是否使用光栅尺
    m_CommPort = -1;//串口编号
    m_OriginCalibrationError = 0.02;//原点校准误差(mm)
    m_MaxCalibrationTimes = 10;//最大校准次数，避免死循环
    m_MeasureErrorGap = 1;//校准平台误差的间隔(mm)(越小分的越细)
    m_MaxOriginError = 10;//回原点时平台原点和光栅尺原点最大允许的误差(mm)
}
CGratingRuler::~CGratingRuler(void)
{
}
#if 1
CMFCPropertyGridProperty *CGratingRuler::CreatGridProperty()
{
      CString PropertyName;//属性名称
      CString Description;//描述
      CString Path = _T("GratingRuler");;//存储路径
      CString Name;
//-------------------------------------------------------------------------------//
      PropertyName = _T("光栅尺");
      CMFCPropertyGridProperty* pGroup = new CMFCPropertyGridProperty(PropertyName);
//-------------------------------------------------------------------------------//
      if(gAuthorityMgr->CheckAuthority(_FACTORY))
      {
          //添加属性变量映射
          Name = _T("m_CommPort");//变量名字
          CPropertie *pPropertie = new CPropertie;
          pPropertie->SetpVal((void*)&m_CommPort);
          pPropertie->SetType(_PROP_TYPE_INT);
          pPropertie->SetpModule(this);
          pPropertie->SetPath(Path);
          pPropertie->SetName(Name);      
          pPropertie->WriteRead(true);//读取保存的属性
          
          //添加属性显示
          PropertyName = _T("串口号");
          Description = _T("光栅尺和PC 通讯的串口号(0~16)");
          CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_CommPort, Description);
          pGroup->AddSubItem(p1);
          gDevicePropertieMgr.Insert(p1, pPropertie);
      }
      if(gAuthorityMgr->CheckAuthority(_FACTORY))
      {
          //添加属性变量映射
          Name = _T("m_bUseGratingRuler");//变量名字
          CPropertie *pPropertie = new CPropertie;
          pPropertie->SetpVal((void*)&m_bUseGratingRuler);
          pPropertie->SetType(_PROP_TYPE_BOOL);
          pPropertie->SetpModule(this);
          pPropertie->SetPath(Path);
          pPropertie->SetName(Name);      
          pPropertie->WriteRead(true);//读取保存的属性
          
          //添加属性显示
          PropertyName = _T("光栅尺");
          Description = _T("光栅尺校准开关");
          CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_bUseGratingRuler, Description);
          pGroup->AddSubItem(p1);
          gDevicePropertieMgr.Insert(p1, pPropertie);
      }
      if(gAuthorityMgr->CheckAuthority(_FACTORY))
      {
          //添加属性变量映射
          Name = _T("m_OriginCalibrationError");//变量名字
          CPropertie *pPropertie = new CPropertie;
          pPropertie->SetpVal((void*)&m_OriginCalibrationError);
          pPropertie->SetType(_PROP_TYPE_DOUBLE);
          pPropertie->SetpModule(this);
          pPropertie->SetPath(Path);
          pPropertie->SetName(Name);      
          pPropertie->WriteRead(true);//读取保存的属性
          
          //添加属性显示
          PropertyName = _T("原点校准误差");
          Description = _T("平台回原点时误差大于这个值需要补偿脉冲(单位: mm)");
          CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_OriginCalibrationError, Description);
          pGroup->AddSubItem(p1);
          gDevicePropertieMgr.Insert(p1, pPropertie);
      }
      if(gAuthorityMgr->CheckAuthority(_FACTORY))
      {
          //添加属性变量映射
          Name = _T("m_MaxCalibrationTimes");//变量名字
          CPropertie *pPropertie = new CPropertie;
          pPropertie->SetpVal((void*)&m_MaxCalibrationTimes);
          pPropertie->SetType(_PROP_TYPE_INT);
          pPropertie->SetpModule(this);
          pPropertie->SetPath(Path);
          pPropertie->SetName(Name);      
          pPropertie->WriteRead(true);//读取保存的属性
          
          //添加属性显示
          PropertyName = _T("最大校准次数");
          Description = _T("平台回原点时误差最大校准次数,避免死循环");
          CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_MaxCalibrationTimes, Description);
          pGroup->AddSubItem(p1);
          gDevicePropertieMgr.Insert(p1, pPropertie);
      }
      if(gAuthorityMgr->CheckAuthority(_FACTORY))
      {
          //添加属性变量映射
          Name = _T("m_MeasureErrorGap");//变量名字
          CPropertie *pPropertie = new CPropertie;
          pPropertie->SetpVal((void*)&m_MeasureErrorGap);
          pPropertie->SetType(_PROP_TYPE_DOUBLE);
          pPropertie->SetpModule(this);
          pPropertie->SetPath(Path);
          pPropertie->SetName(Name);      
          pPropertie->WriteRead(true);//读取保存的属性
          
          //添加属性显示
          PropertyName = _T("测量间隔");
          Description = _T("校准平台误差的间隔(单位: mm)");
          CMFCPropertyGridProperty* p1 = new CMFCPropertyGridProperty(PropertyName, (_variant_t)m_MeasureErrorGap, Description);
          pGroup->AddSubItem(p1);
          gDevicePropertieMgr.Insert(p1, pPropertie);
      }
//-------------------------------------------------------------------------------//
    return pGroup;
}
void CGratingRuler::ExportPar(ofstream  *pFile)
{
    (*pFile)<<"[模块] [CGratingRuler]------------------------------------------------"<<endl;
    (*pFile)<<"[光栅尺][m_bUseGratingRuler] = "<<m_bUseGratingRuler<<endl;
    (*pFile)<<"[串口号][m_CommPort] = "<<m_CommPort<<endl;
    (*pFile)<<"[原点校准误差][m_OriginCalibrationError] = "<<m_OriginCalibrationError<<endl;
    (*pFile)<<"[最大校准次数][m_MaxCalibrationTimes] = "<<m_MaxCalibrationTimes<<endl;
    (*pFile)<<"[测量间隔][m_MeasureErrorGap] = "<<m_MeasureErrorGap<<endl;
    (*pFile)<<"[原点最大允许误差][m_MaxOriginError] = "<<m_MaxOriginError<<endl;
}
void CGratingRuler::ExportErrorTable(ofstream  *pFile,vector<TypePosError> &ErrorTable)
{
    
}
#endif

#if 1//读取光栅尺
//读取光栅尺
bool CGratingRuler::ReadGratingRuler(Dbxy &pt)
{
    CCommPortMgr com;
    //打开串口
    if(com.Open(m_CommPort)==false)
    {
         return false;
    }
    int DelayTime = 100;
    
    CString str1 = "R";
    CString str = str1+char(13)+char(10);
    com.Write(str);
    Sleep(DelayTime);
    
    //读取串口反馈
    char Buffer[WE6800_RETURN_BTYE_CNT];
    int len = com.Read(Buffer,WE6800_RETURN_BTYE_CNT);
    if(len<WE6800_RETURN_BTYE_CNT)
    {
         com.Close();
         return false;
    }
    for(int i=0;i<WE6800_RETURN_BTYE_CNT;i++)
    {
    }
    //解析坐标值
    if(AnalysisRulerRet(Buffer,pt)==false)
    {
         com.Close();
         return false;
    }
        
    com.Close();
    return true;
}
//解析光栅尺反馈结果
bool CGratingRuler::AnalysisRulerRet(char *Buffer,Dbxy &pt)
{
    bool bIsNegativeX = false;
    bool bIsNegativeY = false;
    
    //解析符号位-------------------------------------------
    if(AnalysisSymbol(Buffer[1],bIsNegativeX,bIsNegativeY)==false)
    {
          return false;
    }
    //解析XY 轴的状态
    if(AnalysisXYState(Buffer[2])==false)
    {
          return false;
    }
    //解析X 轴坐标值
    pt.x = AnalysisCoord(Buffer[3],Buffer[4],Buffer[5],Buffer[6]);
    //解析Y 轴坐标值
    pt.y = AnalysisCoord(Buffer[7],Buffer[8],Buffer[9],Buffer[10]);

    //处理符号
    if(bIsNegativeX)
    {
         pt.x *= -1;
    }
    if(bIsNegativeY)
    {
         pt.y *= -1;
    }
    
    return true;
}
//解析符号位
bool CGratingRuler::AnalysisSymbol(BYTE byte,bool &bIsNegativeX,bool &bIsNegativeY)
{
     if(IsBitOn(byte,4))//判断单位0 是毫米1 是英尺
     {
          CString str = "[error] [数显表单位设置为英尺]";
          gLogMgr->WriteDebugLog(str);
          return false;
     }
     if(IsBitOn(byte,0))//判断x 轴的符号
     {
          bIsNegativeX = true;
     }
     if(IsBitOn(byte,1))//判断Y 轴的符号
     {
          bIsNegativeY = true;
     }
     return true;
}
//解析XY 轴的状态
bool CGratingRuler::AnalysisXYState(BYTE byte)
{
     if(IsBitOn(byte,0))
     {
          CString str = "[error] [X 轴光栅尺异常]";
          gLogMgr->WriteDebugLog(str);
          return false;
     }
     if(IsBitOn(byte,1))
     {
          CString str = "[error] [Y 轴光栅尺异常]";
          gLogMgr->WriteDebugLog(str);
          return false;
     }
     return true;
}
//解析坐标值(压缩BCD 码格式)
double CGratingRuler::AnalysisCoord(BYTE byte1,BYTE byte2,BYTE byte3,BYTE byte4)
{
     double coord = 0;
     double coefficient = 0.0001;//这个系数是根据光栅尺的精度来取的
     double Increment = 10;//每次增加的倍数
     //千分位,百分位
     {
          double high = 0;
          double low = 0;
          GetTwoNum(byte1,high,low);
          coord += low*coefficient;
          coefficient *= Increment;
          coord += high*coefficient;
          coefficient *= Increment;
     }
     //十分位,个位
     {
          double high = 0;
          double low = 0;
          GetTwoNum(byte2,high,low);
          coord += low*coefficient;
          coefficient *= Increment;
          coord += high*coefficient;
          coefficient *= Increment;
     }
     //十位，百位
     {
          double high = 0;
          double low = 0;
          GetTwoNum(byte3,high,low);
          coord += low*coefficient;
          coefficient *= Increment;
          coord += high*coefficient;
          coefficient *= Increment;
     }
     //千位，万位
     {
          double high = 0;
          double low = 0;
          GetTwoNum(byte4,high,low);
          coord += low*coefficient;
          coefficient *= Increment;
          coord += high*coefficient;
     }
     return coord;
}
//获取byte 低四位和高四位的数值
void CGratingRuler::GetTwoNum(BYTE byte,double &high,double &low)
{
    //低四位
    {
         BYTE byte1 = byte<<4;//左边清0
         int tmp = (int)(byte1>>4);//先转化为整型
         low = tmp;
    }
    //高四位
    {
         BYTE byte1 = byte>>4;//右边清0
         int tmp = (int)(byte1);//先转化为整型
         high = tmp;
    }
}
#endif
#if 1//坐标相关
//设置当前位置为光栅尺的原点
bool CGratingRuler::SetOriginCoord()
{
     Dbxy pt;
     if(ReadGratingRuler(pt)==false)//读取当前值
     {
          return false;
     }
     m_bIni = true;//初始化成功
     m_OriginCoord = pt;//记录当前的读数
     return true;
}
//获取光栅尺当前的坐标
bool CGratingRuler::GetCurCoord(Dbxy &pt)
{
    if(!m_bIni)
    {
         return false;
    }
    Dbxy ReadPt;
    if(ReadGratingRuler(ReadPt)==false)
    {
         return false;
    }
    //转换为相对坐标
    pt.x = ReadPt.x - m_OriginCoord.x;
    pt.y = ReadPt.y - m_OriginCoord.y;
    
    return true;
}
#endif
#if 1//平台操作
//平台原点校准(平台此时理论坐标已经回原点，再通过光栅尺来校准其位置)
bool CGratingRuler::PlatformOriginCalibration(CPlatformXY &PlatformXY)
{
     if(IsUsed()==false)//没有使用光栅尺，则认为已经校准
     {
         return true;
     }
     if(!m_bIni)
     {
         return false;
     }
     int i=0;
     for(;i<m_MaxCalibrationTimes;i++)
     {
           //获取光栅尺当前的坐标
           Dbxy pt;
           if(GetCurCoord(pt)==false)
           {
                 return false;
           }
           if(pt.x>m_MaxOriginError || pt.y>m_MaxOriginError)
           {
                //异常情况，误差大于正常值
                return false;
           }
           //XY 轴的误差都小于m_OriginCalibrationError 时，结束校准
           if(abs(pt.x)<m_OriginCalibrationError && abs(pt.y)<m_OriginCalibrationError)
           {
                 break;
           }
           //否则补偿(脉冲变化不计入理论坐标值,这个时候理论坐标是零)
           if(abs(pt.x)>m_OriginCalibrationError)
           {
                 if(pt.x>0)
                 {
                       PlatformXY.MoveDistance(pt.x,_DIR_L,false);
                 }
                 else
                 {
                       PlatformXY.MoveDistance(pt.x,_DIR_R,false);
                 }
           }
           if(abs(pt.y)>m_OriginCalibrationError)
           {
                 if(pt.y>0)
                 {
                       PlatformXY.MoveDistance(pt.x,_DIR_D,false);
                 }
                 else
                 {
                       PlatformXY.MoveDistance(pt.x,_DIR_U,false);
                 }
           }
     }
     if(i==m_MaxCalibrationTimes)
     {
           return false;
     }
     return true;
}
//测量平台误差
//rect 是需要校准的范围(相对于平台理论坐标)
//PlatformXY 是要校准的平台
bool CGratingRuler::MeasurePlatfromError(DbRect rect,CPlatformXY &PlatformXY)
{
     if(IsUsed()==false)
         return true;
     if(!m_bIni)
     {
         return false;
     }

     //x 轴
     if(MeasureMotorError(PlatformXY,_X,rect.L,rect.R,m_ErrorTableX)==false)
     {
         return false;
     }
     //y 轴
     if(MeasureMotorError(PlatformXY,_Y,rect.B,rect.T,m_ErrorTableY)==false)
     {
         return false;
     }
     return true;
}
//测量马达的误差(Start,End 是测量范围)
bool CGratingRuler::MeasureMotorError(CPlatformXY &PlatformXY,X_OR_Y xy,double Start,double End,vector<TypePosError> &ErrorTable)
{
     ErrorTable.clear();
     
     Dbxy CurPt = PlatformXY.GetCoord();//当前平台坐标,应该是0,0
     Dbxy OriginPt;//原点
     
     double CurPos = Start;
     while(CurPos <= End)
     {
           Dbxy MovePt;//当前测量点(平台理论坐标)
           if(xy == _X)
           {
                MovePt.x = CurPos;
                MovePt.y = CurPt.y;
           }
           else
           {
                MovePt.x = CurPt.x;
                MovePt.y = CurPos;
           }
           PlatformXY.Move(false,MovePt,OriginPt);//移动
           //读取光栅尺当前坐标
           Dbxy RulerCoord;//光栅尺实际坐标
           if(GetCurCoord(RulerCoord)==false)
           {
                return false;
           }
           //记录当前误差(实际走位的是否理论值减掉这个误差即可)
           double Error;
           if(xy == _X)
           {
               Error = RulerCoord.x - CurPos;
           }
           else
           {
               Error = RulerCoord.y - CurPos;
           }
           
           TypePosError  PosError;
           PosError.first = CurPos;
           PosError.second = Error;
           ErrorTable.push_back(PosError);
     }
	 return true;
}
#endif