#include "StdAfx.h"
#include "CAutoZoom.h"

#define PI 3.1415926535897932384626433    
#define MAX_BUFF_SIZE 0x200

CPSerial* m_pSO7_Serial=NULL;
CAutoZoom::CAutoZoom(void)
{
  PreZoomRatio=0;
  ZoomOrgPos=360;
  ZoomStartPos=-10;
  ZoomEndPos=210;
  ZoomCurPos=0;
  ZoomMode=0;
  ZoomDeadband=0.1;
  ZoomScale=25.13473606496862;
  iRetryCntFrom=0;
  iRetryCntReadV=0;
  SelectedMultiple=0;
  iReadingInterval=400;
  MotorSpeedFast=4000;
  MotorSpeedSlow=1000;
  SerialComPort=1;
  bInterrupt=FALSE;
  _bReading=FALSE;
  _bZoomHoming=FALSE;
  _bZoomMoving=FALSE;
  IsMotionFinished=0;

};
CAutoZoom::~CAutoZoom(void)
{
  delete m_pSO7_Serial;
  m_pSO7_Serial=NULL;
}

BOOL CAutoZoom::Initialization(bool _bInitConfig)
{
  BOOL status(FALSE);
  if (!m_pSO7_Serial)
  {
    m_pSO7_Serial = new CPSerial();
  }
  if (_bInitConfig)
  {
    LoadConfig();
  }
  if (m_pSO7_Serial)
  {
    m_pSO7_Serial->SetPort(SerialComPort);
    if(m_pSO7_Serial->Open())
    {
      bEnComm=TRUE;
      MaxReadTimes=static_cast<int>(20000/MotorSpeedFast);//每一段倍率最多读取10次2000HZ时
      double dGrade=(ZoomStartPos-ZoomEndPos)/11;
      for (INT i=0;i<12;i++)
      {
        dZoomMultiple[i]=dGrade*i+ZoomEndPos;
      }
      status=TRUE;
      if (_bInitConfig)
      {
        status=CalibrateEncoder();
      }
    }
    else
    {
      status=FALSE;
      bEnComm=FALSE;
      delete m_pSO7_Serial;
      m_pSO7_Serial=NULL;
    }
  }
  return status;
}
//===========================					  
void CAutoZoom::LoadConfig()
{
  FILE   *hConfigFile = NULL;
  char    szLine[MAX_BUFF_SIZE];
  char   *token = NULL;
  char    seps[]  = " =,\t\n";
  char   cTemp[20]={0};
  CString csSO7ConfigFile =GetAppPath()+("\\so7_config.ini");
  _wfopen_s(&hConfigFile,csSO7ConfigFile,_T("rt"));//Unicode
  //hConfigFile=fopen(csSO7ConfigFile,_T("rt"));//MultiBytes
  if(hConfigFile)
  {
    while (!feof(hConfigFile))
    {
      fgets(szLine,MAX_BUFF_SIZE,hConfigFile);//read a line 
      if((szLine[0]!=';')&&(strlen(szLine)>2))
      {
        token = strtok(szLine,seps);
        if(!_stricmp(token,"ZOOM_COM_PORT"))
        {
          token = strtok( NULL, seps);
          if (token)
          {
            strcpy(cTemp,token);
            SerialComPort=atoi(cTemp);
          }
        }
        else if(!_stricmp(token,"ZOOM_START_DEG"))
        {
          token = strtok( NULL, seps);
          if (token)
          {
            strcpy(cTemp,token);
            ZoomStartPos=atof(cTemp);
          }
        }
        else if (!_stricmp(token,"ZOOM_END_DEG"))
        {
          token = strtok( NULL, seps);
          if (token)
          {
            strcpy(cTemp,token);
            ZoomEndPos=atof(cTemp);
          }
        }
        else if (!_stricmp(token,"ZOOM_ORG_DEG"))
        {
          token = strtok( NULL, seps);
          if (token)
          {
            strcpy(cTemp,token);
            ZoomOrgPos=atof(cTemp);
          }
        }
        else if (!_stricmp(token,"ZOOM_DEADBAND_DEG"))
        {
          token = strtok( NULL, seps);
          if (token)
          {
            strcpy(cTemp,token);
            ZoomDeadband=atof(cTemp);
          }
        }
        else if (!_stricmp(token,"ZOOM_PULSE_PER_DEG"))
        {
          token = strtok( NULL, seps);
          if (token)
          {
            strcpy(cTemp,token);
            ZoomScale=atof(cTemp);
          }
        }
        else if (!_stricmp(token,"ZOOM_READING_INTERVAL_TIME"))
        {
          token = strtok( NULL, seps);
          if (token)
          {
            strcpy(cTemp,token);
            iReadingInterval=atoi(cTemp);
          }
        }
        else if (!_stricmp(token,"ZOOM_MOTOR_SPEED_FAST"))
        {
          token = strtok( NULL, seps);
          if (token)
          {
            strcpy(cTemp,token);
            MotorSpeedFast=static_cast<short>(atoi(cTemp));
          }
        }
        else if (!_stricmp(token,"ZOOM_MOTOR_SPEED_SLOW"))
        {
          token = strtok( NULL, seps);
          if (token)
          {
            strcpy(cTemp,token);
            MotorSpeedSlow=static_cast<short>(atoi(cTemp));
          }
        }

      }
    }
    fclose(hConfigFile);
  }
}
double CAutoZoom::ReadZoomAngle()
{
  double dZoomAngle(-1);
  if(bEnComm)
  {
    if (!_bReading)
    {
      _bReading=TRUE;
      char sendData[6]={'A','D',':','0',13,10};
      DWORD iWriteByte=m_pSO7_Serial->Send(sendData,6);

      INT iRetrys(0);
      while(!m_pSO7_Serial->m_iRecvState && iRetrys<20 && iWriteByte==6)
      {
        iRetrys++;
        Sleep(10);
      }
      if (m_pSO7_Serial->m_iRecvState)
      {
        if(m_pSO7_Serial->m_RecvData[0]==0)
          Sleep(40);//200

        char *token = NULL;
        char   cTemp[20]={0};

        char  seps[]  = "$AD:";

        token = strtok(m_pSO7_Serial->m_RecvData,seps);
        if (token)
        {
          memcpy(cTemp,token,8);
          dZoomAngle=atof(cTemp);
          dZoomAngle=dZoomAngle*180/PI;
          if (dZoomAngle>ZoomOrgPos)
          {
            dZoomAngle=dZoomAngle-360;
          }
        };
        token=NULL;
        m_pSO7_Serial->m_iRecvState=FALSE;
        _bReading=FALSE;
      }
      else
      {
        _bReading=FALSE;
        dZoomAngle=-2;
      }    
    }
    else
    {
      dZoomAngle=-2;
    }
  } 
  return dZoomAngle;
}

double CAutoZoom::GetZoomCurPos()
{
  if(bEnComm)
  {
    //====================AutoZoom==============================
    double dZoomPos(0),dZoomAng(0),dZoomPreAng(0);
    INT iCnt2(0);
    INT iRetryCount(0);
    dZoomAng=ReadZoomAngle();

    while(iCnt2<2 && iRetryCount<10)
    {
      Sleep(iReadingInterval);//400
      dZoomPreAng=dZoomAng;
      dZoomAng=ReadZoomAngle();
      INT iRetrys(0);
      while ((dZoomAng==-1 || dZoomAng==-2 || dZoomAng==0 )&&iRetrys<2)
      {
        iRetrys++;
        Sleep(iReadingInterval);//400
        dZoomAng=ReadZoomAngle();
      } 
      if(dZoomAng!=-1 && dZoomAng!=-2 && dZoomAng!=0)
      {
        if ((dZoomAng-dZoomPreAng)<1 && (dZoomPreAng-dZoomAng)<1)
        {
          dZoomPos+=dZoomAng;
          iCnt2++; 
        }
      }
      iRetryCount++;

    }
    if (iCnt2==0)
    {
      dZoomPos=-2;
    } 
    else
    {
      dZoomPos=dZoomPos/(iCnt2);
    }


    return dZoomPos;
  }
  else
  {
    return 0;
  }

}
BOOL CAutoZoom::CalibrateEncoder()
{
  if (bEnComm)
  {
    char sendDataI[12]={'U','P',':','1','1','1','1','1','1','1',13,10};
    char sendDataII[6]={'S','C',':','1',13,10};
    char sendDataIII[6]={'R','F',':','1',13,10};
    CString csRightRecv;
    csRightRecv.Format(_T("@OK\r\n"));
    CStringA m_csRecv("");
    INT iRetrys(0);
    DWORD iWriteByte(0);
    m_pSO7_Serial->m_iRecvState=FALSE;

    //Phase I
    iWriteByte=m_pSO7_Serial->Send(sendDataI,12);
    while(!m_pSO7_Serial->m_iRecvState && iRetrys<20 && iWriteByte==12)
    {
      iRetrys++;
      Sleep(50);
    }

    if (m_pSO7_Serial->m_iRecvState)
    {
      if(m_pSO7_Serial->m_RecvData[0]==0)
      {    
        m_pSO7_Serial->m_iRecvState=FALSE;
        iRetrys=0;
        while(!m_pSO7_Serial->m_iRecvState && iRetrys<30)
        {
          iRetrys++;
          Sleep(50);
        }
      }

      m_csRecv.Format(("%s"),m_pSO7_Serial->m_RecvData);
      if((CString)m_csRecv==csRightRecv)
      {
        m_pSO7_Serial->m_iRecvState=FALSE;
        iRetrys=0;
        iWriteByte=0;
        //Phase II

        iWriteByte=m_pSO7_Serial->Send(sendDataII,6);
        while(!m_pSO7_Serial->m_iRecvState && iRetrys<20 && iWriteByte==6)
        {
          iRetrys++;
          Sleep(50);
        }
        if(m_pSO7_Serial->m_iRecvState)
        {
          if(m_pSO7_Serial->m_RecvData[0]==0)
          {    
            m_pSO7_Serial->m_iRecvState=FALSE;
            iRetrys=0;
            while(!m_pSO7_Serial->m_iRecvState && iRetrys<30)
            {
              iRetrys++;
              Sleep(50);
            }
          }

          m_csRecv.Format(("%s"),m_pSO7_Serial->m_RecvData);
          if((CString)m_csRecv==csRightRecv)
          {
            m_pSO7_Serial->m_iRecvState=FALSE;
            iRetrys=0;
            iWriteByte=0;
            //Phase III

            iWriteByte=m_pSO7_Serial->Send(sendDataIII,6);
            while(!m_pSO7_Serial->m_iRecvState && iRetrys<20 && iWriteByte==6)
            {
              iRetrys++;
              Sleep(50);
            }
            if(m_pSO7_Serial->m_iRecvState)
            {
              if(m_pSO7_Serial->m_RecvData[0]==0)
              {    
                m_pSO7_Serial->m_iRecvState=FALSE;
                iRetrys=0;
                while(!m_pSO7_Serial->m_iRecvState && iRetrys<30)
                {
                  iRetrys++;
                  Sleep(50);
                }
              }

              m_csRecv.Format(("%s"),m_pSO7_Serial->m_RecvData);
              if((CString)m_csRecv==csRightRecv)
              {
                m_pSO7_Serial->m_iRecvState=FALSE;
                iRetrys=0;
                iWriteByte=0;
                return TRUE;
              }
              else
              {
                m_pSO7_Serial->m_iRecvState=FALSE;
                return FALSE;
              }
            }
            else
            {
              return FALSE;
            }
          }
          else
          {
            m_pSO7_Serial->m_iRecvState=FALSE;
            return FALSE;
          }
        }
        else
        {
          return FALSE;
        }

      }
      else
      {
        m_pSO7_Serial->m_iRecvState=FALSE;
        return FALSE;
      }

    }
    else
    {
      return FALSE;
    }
  }
  else
  {
    return FALSE;
  }
}