#include "stdafx.h"
#include "SO7_Proto_Aux.h"
#include "math.h"



#define MY_CONFIG 1
#define MAX_DEVPATH_LENGTH 256
#define ENDPOINT_TIMEOUT   500
#define MAX_IN_BUFF_SIZE        1024

//*****  Static Data  *****
static char *outBuff = NULL;
struct_so7_r_ep_buff  CSO7_Proto_Aux::ep_buff[lEPSIZE];
//================================================================
int              CSO7_Proto_Aux::g_hEP8x_Thread_State=THREAD_PAUSED;
HANDLE           CSO7_Proto_Aux::g_hEP8x_Thread_Id=NULL;

//================================================================
int              CSO7_Proto_Aux::g_hEP02_Thread_State=THREAD_PAUSED;
HANDLE           CSO7_Proto_Aux::g_hEP02_Thread_Id=NULL;
HANDLE           CSO7_Proto_Aux::g_hEP02_Serial_Mutex;

//================================================================
struct_so7_r_machine  CSO7_Proto_Aux::g_machine;
usb_dev_handle        *CSO7_Proto_Aux::g_dev=NULL;
CLogger               *CSO7_Proto_Aux::g_pLogger1;
HANDLE                CSO7_Proto_Aux::g_hHomedEvent = NULL;
CLogger* g_pLog1=NULL;

//******************************************************************************
CSO7_Proto_Aux::CSO7_Proto_Aux()
{
  ep_buff[EP_01_CMD_IDX]._ep  = EP_S07_01;
  ep_buff[EP_81_DATA_IDX]._ep = EP_S07_81;
  ep_buff[EP_02_CMD_IDX]._ep  = EP_S07_02;
  ep_buff[EP_82_DATA_IDX]._ep = EP_S07_82;

  for (int i=0;i<lEPSIZE;i++)
  {
    ep_buff[i]._size             = 0;
    ep_buff[i]._save_send_cmd    = 99;
    ep_buff[i]._async_context    = NULL;
    ep_buff[i]._buffer           = (char *)malloc(MAX_BUFF_SIZE);
    ep_buff[i]._hProtoPending    = false;
    ep_buff[i]._event            = NULL;
  };
  for (int i=0;i<20;i++)
  {
    for (int j=0;j<2;j++)
    {
      g_machine.GetInterruptMsg[i][j]=0;
    }
  }
  g_machine.IsSupportReadInterrputMsg=FALSE;
  g_machine.IsOffline=TRUE;
  g_machine.FPGAData=0;
  g_machine.FirmwareVer=FirmwareVer_3_X_R;
  g_machine.r._Move_Speed_Gear   =2;
  g_machine.r._MoveTo_Speed_Gear =0;
  g_machine.r._pos_fixed._long_  =0;
  g_machine.r._ZSignal_pos._long_=0;

  g_machine.r._d_cur_pos_  = 0;
  g_machine.ADC_Number=0;
  g_machine.ADC_Value =0;
  g_machine.Sys_Reset_Flag=0;
  g_machine.cVerNumber=3;   
  g_machine.dRotaryCirclDis=100;
  g_machine.SEQ_NUMBER=0;
  g_machine.s_machine_r_config.r_axis._scale_resolution=0.5;

  g_machine.s_machine_r_config.r_axis._neg_working_limit=0;

  g_machine.s_machine_r_config.r_axis._MoveToSpeed[0]=0.0;
  g_machine.s_machine_r_config.r_axis._MoveToSpeed[1]=0.0;
  g_machine.s_machine_r_config.r_axis._MotionSpeedScale=1.0;

  g_machine.s_machine_r_config.motion._EnCloseLoop=FALSE;
  g_machine.s_machine_r_config.motion._RetryTimes=5;
  g_machine.s_machine_r_config.motion._ShiftPositionR=0.0;

  g_machine.s_machine_r_config.motion.GetInterruptMsgMethod=E_GET_INTERRUPT_MSG_INTERRUPT;//E_GET_INTERRUPT_MSG_INQUIRY;
  g_machine.s_machine_r_config.motion.m_WriteDataSleepTime=0;
  g_machine.s_machine_r_config.motion.m_AccuraErrPulseR=1;
  g_machine.s_machine_r_config.motion.m_EQUIDIS_R=0;

  g_machine.s_machine_r_config.motion.m_SDK3000_CntThreadSleepVal=550000;
  g_machine.s_machine_r_config.motion.m_SV4000E_DenoisePara[0]=70;
  g_machine.s_machine_r_config.motion.m_SV4000E_DenoisePara[1]=70;
  g_machine.s_machine_r_config.motion.m_SV4000E_DenoisePara[2]=70;
  g_machine.s_machine_r_config.motion.m_SV4000E_DenoisePara[3]=70;
  
  g_machine.s_machine_r_config.motion.m_MachineType=MACHINE_SO7_R_CONTROLLER;
  g_machine.s_machine_r_config.motion.m_VideoCardType=0;

  g_machine.s_rstatus._bIsZMMotionFinished=0;
  g_machine.r._scale_pos._long_  =  0;
  g_machine.r._d_cur_pos_ = 0;
  g_machine.r._dSet_Zero_Pos  = 0.0;
  g_machine.r._lSet_Zero_Pos  = 0;
  g_machine.s_machine_r_config._dRSpeed[0] = 50;
  g_machine.s_machine_r_config._dRSpeed[1] = 50;
  g_machine.InterruptFlag[0]=0;
  g_machine.InterruptFlag[1]=0;
  g_machine.InPortStatus=0;
  m_bHomingActive = false;
  g_pLogger1 = new CLogger(_T("\\UtilityDebug.Log"));
  g_pLogger1->Send(_T("Construct Cso7_Proto.\r\n"));
};

//******************************************************************************
CSO7_Proto_Aux::~CSO7_Proto_Aux()
{
  for (int i=0;i<lEPSIZE;i++)
  {
    free(ep_buff[i]._buffer);
  };
  if (g_pLogger1)
    g_pLogger1->Send(_T("Destruct Cso7_Proto.\r\n"));
  delete g_pLogger1;
  g_pLogger1 = NULL;
}

#pragma warning(disable:4996)
//******************************************************************************
// Send is direct and async.
// The receive thread will receive data and interpret it. 
//******************************************************************************
SSI_STATUS_MOTION CSO7_Proto_Aux::Init_SO7Usb()
{
  //Set initial state of the machine
  g_machine.s_rstatus._machine_running = false;
  SSI_STATUS_MOTION rStatus=SSI_STATUS_MOTION_NORMAL;
  g_machine.IsOffline=FALSE;
  
  int usb_status = NULL;  
  usb_init();                       // initialize the library 
  usb_status = usb_find_busses();   // find all busses 
  usb_status = usb_find_devices();  // find all connected devices  
  g_dev = _open_usb_dev(USB_R_SEQ_NUMBER);
  if (!g_dev)
  {
    MessageBox(NULL, _T("Unable to open device"), _T("Message"), MB_OK|MB_ICONERROR);
    g_pLogger1->SendAndFlushPerMode(_T("Unable to open device %s \r\n"), usb_strerror());
    g_machine.IsOffline=TRUE;   
    rStatus= SSI_STATUS_MOTION_DATALINK_ERROR;
  }
  else if (usb_set_configuration(g_dev, MY_CONFIG) < 0)
  {
    MessageBox(NULL, _T("Unable to SET CONFIGURATION"), _T("Message"), MB_OK|MB_ICONERROR);
    g_machine.IsOffline=TRUE;   
    rStatus=SSI_STATUS_MOTION_DATALINK_ERROR;
  }
  else if (usb_claim_interface(g_dev, 0) < 0)
  {
    usb_close(g_dev);
    MessageBox(NULL, _T("Unable to CLAIM DEVICE"), _T("Message"), MB_OK|MB_ICONERROR);
    g_machine.IsOffline=TRUE;   
    rStatus=SSI_STATUS_MOTION_DATALINK_ERROR;
  }

  g_pLogger1->SendAndFlushPerMode(_T("Init:Open device succeed .\r\n"));

  // ********************************************************************
  // This event is used to kick the Serial Usb Command process.  This threading model
  // is important because the underlying library is not thread-safe.
  //
  ep_buff[EP_02_CMD_IDX]._event = CreateEvent(NULL,      // default security attributes
    FALSE,     // manual reset event object
    NULL,      // signaled
    NULL);     // unamed object

  g_hEP02_Thread_Id    = CreateThread( (LPSECURITY_ATTRIBUTES) NULL,
    0,
    (LPTHREAD_START_ROUTINE) g_EP02_Thread,
    (LPVOID) this,
    0,
    NULL);
  g_hEP02_Thread_State = THREAD_RUNNING_STATE1;

  // ********************************************************************
  // Prepare and start EP_S07_81 Thread - Use async commit.
  //
  ep_buff[EP_82_DATA_IDX]._event    = CreateEvent(NULL,   // default security attributes
    FALSE,  // manual reset event object
    NULL,   // signaled
    NULL);  // unamed object
  g_hEP8x_Thread_State  = THREAD_PAUSED;
  g_hEP8x_Thread_Id = CreateThread( (LPSECURITY_ATTRIBUTES) NULL,
    0,
    (LPTHREAD_START_ROUTINE) g_EP8x_Thread,
    (LPVOID) this,
    0,
    NULL);
  g_hEP02_Serial_Mutex = CreateMutex(NULL,     // default security attributes
    FALSE,     // initial owner
    NULL);     // name

  // *********************************************************************
  g_hHomedEvent    = CreateEvent(NULL,   // default security attributes
    TRUE,   // manual reset event object
    FALSE,   // initial state is signaled
    NULL);  // unamed object

  return rStatus;
}

//******************************************************************************
SSI_STATUS_MOTION CSO7_Proto_Aux::Exit_SO7Usb()
{
  SSI_STATUS_MOTION Status=SSI_STATUS_MOTION_NORMAL;

  g_hEP8x_Thread_State  = THREAD_EXIT;
  g_hEP02_Thread_State  = THREAD_EXIT;

  SetEvent(ep_buff[EP_82_DATA_IDX]._event);
  if (g_hEP8x_Thread_Id)
  {
    DWORD dwCode = STILL_ACTIVE;
    while (dwCode == STILL_ACTIVE)
    {
      GetExitCodeThread(g_hEP8x_Thread_Id,&dwCode);
      Sleep(1);
    }
  }

  SetEvent(ep_buff[EP_02_CMD_IDX]._event);
  if (g_hEP02_Thread_Id)
  {
    DWORD dwCode = STILL_ACTIVE;
    while (dwCode == STILL_ACTIVE)
    {
      GetExitCodeThread(g_hEP02_Thread_Id,&dwCode);
      Sleep(1);
    }
  }

  if (g_dev)
  {
    usb_release_interface(g_dev,0);
    usb_close(g_dev);
    g_dev = NULL;
  }

  SetEvent(ep_buff[EP_82_DATA_IDX]._event);
  CloseHandle(ep_buff[EP_82_DATA_IDX]._event);
  SetEvent(ep_buff[EP_02_CMD_IDX]._event);
  CloseHandle(ep_buff[EP_02_CMD_IDX]._event);
  g_hEP02_Thread_State = THREAD_EXIT;
  ReleaseMutex(g_hEP02_Serial_Mutex);
  CloseHandle(g_hEP02_Serial_Mutex);

  g_pLogger1->SendAndFlushPerMode(_T("Exit: Exit_SO7Usb \r\n"));
  return Status;
}

//******************************************************************************
usb_dev_handle* CSO7_Proto_Aux::_open_usb_dev(unsigned short sSeqNumber)
{
  struct usb_bus *bus = NULL;
  struct usb_device *dev = NULL;
  usb_dev_handle *udev = NULL;

  for (bus = usb_get_busses(); bus; bus = bus->next)
  {
    for (dev = bus->devices; dev; dev = dev->next)
    {
      if (dev->descriptor.idVendor == SEVENOCEAN_VID && dev->descriptor.idProduct == SEVENOCEAN_PID)
      {
        udev = usb_open(dev);
        usb_claim_interface(udev, 0);
                
        if(Get_R_SeqNumber(udev) != sSeqNumber)
        {
          usb_close(udev);
        }
        else
        {
          return udev;
        }
      }
    }
  }
  return NULL;
}

//******************************************************************************
int CSO7_Proto_Aux::Get_R_SeqNumber(usb_dev_handle *udev)
{
  memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);

  *(ep_buff[EP_02_CMD_IDX]._buffer) = CT_DATA;
  *(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_READ_SEQ_NUMBER;

  ep_buff[EP_02_CMD_IDX]._size = 0x02;
  ep_buff[EP_82_DATA_IDX]._size = 0x01;

  int _ret;
  _ret = usb_bulk_write(udev, ep_buff[2]._ep, (char *)ep_buff[2]._buffer,(int)ep_buff[2]._size, 50);
  if (_ret < 0)
  {
    return -1;
  }

  _ret = usb_bulk_read(udev, ep_buff[3]._ep, (char *)ep_buff[3]._buffer,(int)ep_buff[3]._size, 5000);
  if (_ret < 0)
  {
    return -1;
  }

  g_machine.SEQ_NUMBER = -1;
  g_machine.SEQ_NUMBER = *(ep_buff[EP_82_DATA_IDX]._buffer);
  return g_machine.SEQ_NUMBER;
}

//===========================================================================
//  Worker Thread to serialize EP_S07_02 commands.
//===========================================================================
unsigned __stdcall CSO7_Proto_Aux::g_EP02_Thread(LPVOID pThis)
{
  CSO7_Proto_Aux* _This = (CSO7_Proto_Aux*)pThis;
  for (;;)
  {
    if (g_hEP02_Thread_State == THREAD_EXIT) 
      ExitThread(0);
    WaitForSingleObject(ep_buff[EP_02_CMD_IDX]._event,INFINITE);
    switch (g_hEP02_Thread_State)
    {
    case THREAD_EXIT:
      {
        ExitThread(0);
      }
    case THREAD_PAUSED:
      break;
    case THREAD_RUNNING_STATE1:
      { 
        _This->_send_usb_cmd(EP_02_CMD_IDX);
        break;
      }
    case THREAD_RUNNING_STATE2:
      { 
        _This->_write_usb_data_only(EP_02_CMD_IDX);
        break;
      }
    default:
      ExitThread(0);
    }
  };
  g_hEP02_Thread_State = THREAD_EXIT;
  ExitThread(0);
};

//******************************************************************************
// This is the worker thread to process USBD_TRANSFER_DIRECTION_IN
//******************************************************************************
unsigned __stdcall CSO7_Proto_Aux::g_EP8x_Thread(LPVOID pThis)
{
  CSO7_Proto_Aux* _This = (CSO7_Proto_Aux*)pThis;
  for (;;)
  {
    if (g_hEP8x_Thread_State == THREAD_EXIT) 
      ExitThread(0);
    WaitForSingleObject(ep_buff[EP_82_DATA_IDX]._event, INFINITE);
    switch (g_hEP8x_Thread_State)
    {
    case THREAD_EXIT:
      {
        ExitThread(0);
      }
    case THREAD_PAUSED:
      break;
    case THREAD_RUNNING_STATE1:
      {
        _This->_read_data_8x(EP_81_DATA_IDX);
        break;
      }
    case THREAD_RUNNING_STATE2:
      {
        _This->_read_data_8x(EP_82_DATA_IDX);
        break;
      }
    default:
      ExitThread(0);
    }
  }
}

//===============================================================================
// _send_usb_cmd(iEP) sends data to the corresponding iEP channel.
//     iEP = EP_S07_01 / EP_S07_02        EP_S07_01 = 0x01; EP_S07_02 = 0x02;
//===============================================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_usb_cmd(int iEP_Base)
{
  if (g_machine.IsOffline)
  {
    SetEvent(ep_buff[iEP_Base+1]._event);
    ep_buff[iEP_Base]._hProtoPending = false;
    return SSI_STATUS_MOTION_NORMAL;
  }
  int _ret;
  ep_buff[iEP_Base]._save_send_cmd  = ep_buff[iEP_Base]._buffer[0];
  ep_buff[iEP_Base]._save_send_cmd0 = ep_buff[iEP_Base]._buffer[1];
  ep_buff[iEP_Base]._save_send_cmd1 = ep_buff[iEP_Base]._buffer[2];

  _ret = usb_bulk_write(g_dev, ep_buff[iEP_Base]._ep, (char *)ep_buff[iEP_Base]._buffer,(int) ep_buff[iEP_Base]._size, 50);
  if (_ret < 0)
  {
    TRACE("Write Timeout \n");
    ASSERT(0);
    return SSI_STATUS_MOTION_TIMEOUT;
  }

  SetEvent(ep_buff[iEP_Base+1]._event);
  ep_buff[iEP_Base]._hProtoPending = false;
  return SSI_STATUS_MOTION_NORMAL;
}

//===============================================================================
// _write_usb_data_only(iEP) sends data to the corresponding iEP channel.No need to 
//process the reap async
//     iEP = EP_S07_01 / EP_S07_02        EP_S07_01 = 0x01; EP_S07_02 = 0x02;
//===============================================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_write_usb_data_only(int iEP_Base)
{
  if (g_machine.IsOffline)
  {
    ep_buff[iEP_Base]._hProtoPending = false;
    ep_buff[iEP_Base+1]._hProtoPending = false;
    return SSI_STATUS_MOTION_NORMAL;
  }
  int _ret;
  ep_buff[iEP_Base]._save_send_cmd  = ep_buff[iEP_Base]._buffer[0];
  ep_buff[iEP_Base]._save_send_cmd0 = ep_buff[iEP_Base]._buffer[1];
  ep_buff[iEP_Base]._save_send_cmd1 = ep_buff[iEP_Base]._buffer[2];

  //TRACE3("_send_usb_cmd()  iEP : %X - ep_buff[iEP]._save_send_cmd : %X ._buffer[0] : %X\r\n", iEP_Base, ep_buff[iEP_Base]._save_send_cmd, ep_buff[iEP_Base]._buffer[0]);

  _ret = usb_bulk_write(g_dev, ep_buff[iEP_Base]._ep, (char *)ep_buff[iEP_Base]._buffer,(int) ep_buff[iEP_Base]._size, 50);
  if (_ret < 0)
  {
    TRACE("Write Timeout \n");
    ASSERT(0);
    return SSI_STATUS_MOTION_TIMEOUT;
  }
  ep_buff[iEP_Base]._hProtoPending = false;
  ep_buff[iEP_Base+1]._hProtoPending = false;
  return SSI_STATUS_MOTION_NORMAL;
}

//===============================================================================
// iEP_Base : EP_81_DATA_IDX/EP_82_DATA_IDX
//===============================================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_read_data_8x(int iEP_Base)
{	
  if (g_machine.IsOffline)
  {
    ep_buff[EP_82_DATA_IDX]._hProtoPending = false;
    ep_buff[EP_01_CMD_IDX]._hProtoPending = false;
    ep_buff[EP_81_DATA_IDX]._hProtoPending = false;
    return SSI_STATUS_MOTION_NORMAL;
  }
  if (iEP_Base == EP_82_DATA_IDX)
  {
    int _ret;
    _ret = usb_bulk_read(g_dev, ep_buff[iEP_Base]._ep, (char *)ep_buff[iEP_Base]._buffer,(int)  ep_buff[iEP_Base]._size, 5000);
    if (_ret > 0)
    {
      _process_rcv_transfer_data(iEP_Base);
      ep_buff[EP_82_DATA_IDX]._hProtoPending = false;
    }
    else
    {
      TRACE1("Read Timeout %xx\n", *((char *)ep_buff[iEP_Base]._buffer));
      ASSERT(0);
      return SSI_STATUS_MOTION_TIMEOUT;
    }
    return SSI_STATUS_MOTION_NORMAL;
  }
  else
  {
    int _ret;
    _ret = usb_interrupt_read(g_dev, ep_buff[iEP_Base]._ep, (char *)ep_buff[iEP_Base]._buffer,(int)  ep_buff[iEP_Base]._size, 20);
    if (_ret > 0)
    {
      _process_rcv_transfer_data(iEP_Base);
    }
    else
    {
      g_machine.InterruptFlag[0] = 0;
      TRACE1("There is no data interrupt read from controller.  %xx\n", *((char *)ep_buff[iEP_Base]._buffer));
    }

    ep_buff[EP_01_CMD_IDX]._hProtoPending = false;
    ep_buff[EP_81_DATA_IDX]._hProtoPending = false;
    return SSI_STATUS_MOTION_NORMAL;
  }
}

//******************************************************************************
// Parse the data received based on the index (which EP are we processing).
// The CCmdObj should only have one CMD_STATUS_PROCESSING.
// Update CCmdObj _ep_01_status and _ep_81_status.  
// Update _status to CMD_STATUS_COMPLETE when both _ep_01_status and _ep_81_status 
// are CMD_STATUS_COMPLETE.
//
//******************************************************************************
void CSO7_Proto_Aux::_process_rcv_transfer_data(int iEP)
{
  switch (iEP)
  {
  case EP_01_CMD_IDX :
    TRACE0("_process_rcv_transfer_data() : Update EP_01_CMD_IDX.\r\n");
    break;
  case EP_82_DATA_IDX :   // 
    if(ep_buff[EP_02_CMD_IDX]._save_send_cmd == CT_DATA)
    {
      switch (ep_buff[EP_02_CMD_IDX]._save_send_cmd0)
      {
      case CT_READ_AXISXYZ:
        _process_SO7_CMD_READ_AXIS_R();
        break;
      case CT_READ_SEC_REALY:
        _process_SO7_CMD_READ_ZSIGNAL_POS_R();
        break;

      default:
        TRACE1("_process_rcv_transfer_data() : Unknown ep_buff[EP_02_CMD_IDX]._save_send_cmd : %X \r\n", ep_buff[EP_02_CMD_IDX]._save_send_cmd);
        TRACE1("_process_rcv_transfer_data() : Unknown ep_buff[EP_81_DATA_IDX]._buffer[0] : %X \r\n", ep_buff[EP_81_DATA_IDX]._buffer[0]);
        break;
      };
    }
    break;
  case EP_02_CMD_IDX :
    TRACE0("_process_rcv_transfer_data() : Update EP_02_CMD_IDX.\r\n");
    break;
  case EP_81_DATA_IDX :

    TRACE0("_process_rcv_transfer_data() : Update EP_81_DATA_IDX.\r\n");
    break;
  default:
    break;
  };
};

//******************************************************************************
// Kick the g_hEP01_Thread_Event to get the g_EP01_Thread going.
// iEP = EP_S07_01 or EP_S07_02    = 0x01 or 0x02
//******************************************************************************
SSI_STATUS_MOTION CSO7_Proto_Aux::_do_single_threaded_usb_comm(int iEP_Base)
{
  while ((ep_buff[iEP_Base]._hProtoPending == TRUE) || (ep_buff[iEP_Base+1]._hProtoPending == TRUE))
  {
    ASSERT(0);
    Sleep(3);
  }
  ep_buff[iEP_Base]._hProtoPending = ep_buff[iEP_Base+1]._hProtoPending = TRUE;

  if (iEP_Base == EP_01_CMD_IDX)
    SetEvent(ep_buff[EP_82_DATA_IDX]._event);
  else
    SetEvent(ep_buff[iEP_Base]._event);
  while ((ep_buff[iEP_Base]._hProtoPending == TRUE) || (ep_buff[iEP_Base+1]._hProtoPending == TRUE))
  {
    Sleep(3);
  }

  return SSI_STATUS_MOTION_NORMAL;
}

//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_process_SO7_CMD_READ_AXIS_R()
{
  g_machine.r._scale_pos._long_=0;
  g_machine.r._scale_pos._char_[2] = *(ep_buff[EP_82_DATA_IDX]._buffer);
  g_machine.r._scale_pos._char_[1] = *(ep_buff[EP_82_DATA_IDX]._buffer+1);
  g_machine.r._scale_pos._char_[0] = *(ep_buff[EP_82_DATA_IDX]._buffer+2);
  g_machine.r._scale_pos._char_[3] = 0;
  if (g_machine.r._scale_pos._long_ > 8388608)
    g_machine.r._scale_pos._long_ = g_machine.r._scale_pos._long_ - 16777216;

  return SSI_STATUS_MOTION_NORMAL;
};

//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_process_SO7_CMD_READ_ZSIGNAL_POS_R()
{
  g_machine.r._ZSignal_pos._char_[2] = *(ep_buff[EP_82_DATA_IDX]._buffer);
  g_machine.r._ZSignal_pos._char_[1] = *(ep_buff[EP_82_DATA_IDX]._buffer+1);
  g_machine.r._ZSignal_pos._char_[0] = *(ep_buff[EP_82_DATA_IDX]._buffer+2);
  g_machine.r._ZSignal_pos._char_[3] = 0;

  if (g_machine.r._ZSignal_pos._long_ > 8388608)
    g_machine.r._ZSignal_pos._long_=g_machine.r._ZSignal_pos._long_-16777216;

  return SSI_STATUS_MOTION_NORMAL;
}

//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_RESET_R(int iType)
{
  iType++;
  WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
  memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);

  *(ep_buff[EP_02_CMD_IDX]._buffer)   = CT_SCALE;
  *(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_SET_RESET_FLAG;
  *(ep_buff[EP_02_CMD_IDX]._buffer+2) = 0x01;

  ep_buff[EP_02_CMD_IDX]._size = 0x03;
  ep_buff[EP_82_DATA_IDX]._size = 0x02;
  
  g_hEP02_Thread_State=THREAD_RUNNING_STATE2;
  _do_single_threaded_usb_comm(EP_02_CMD_IDX);
  ReleaseMutex(g_hEP02_Serial_Mutex);
  return SSI_STATUS_MOTION_NORMAL;
};

//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_MOVE_R(int iType, int iMoveAngle)
{  
  WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
  memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);

  g_machine.r._pos_fixed._long_ = (ONE_R_PULSE * iMoveAngle / 360);
  *(ep_buff[EP_02_CMD_IDX]._buffer) = CT_MOTOR;
  if(iType)
  {
    *(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_MOVETOY;
  }
  else
  {
    *(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_MOVETOX;
  }
  *(ep_buff[EP_02_CMD_IDX]._buffer+2)=(((g_machine.r._pos_fixed._char_[3])<0x80)?(g_machine.r._pos_fixed._char_[2]):((g_machine.r._pos_fixed._char_[2])|0x80));//×î¸ßλ
  *(ep_buff[EP_02_CMD_IDX]._buffer+3)=(g_machine.r._pos_fixed._char_[1]);
  *(ep_buff[EP_02_CMD_IDX]._buffer+4)=(g_machine.r._pos_fixed._char_[0]);
    
  ep_buff[EP_02_CMD_IDX]._size = 0x07;
  ep_buff[EP_81_DATA_IDX]._size = 0x07;  

  g_hEP02_Thread_State = THREAD_RUNNING_STATE2;
  _do_single_threaded_usb_comm(EP_02_CMD_IDX);
  ReleaseMutex(g_hEP02_Serial_Mutex);
  return SSI_STATUS_MOTION_NORMAL;
};

//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_READ_AXIS_R()
{
  WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
  memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);

  *(ep_buff[EP_02_CMD_IDX]._buffer) = CT_DATA;
  *(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_READ_AXISXYZ;
  *(ep_buff[EP_02_CMD_IDX]._buffer+2)=0x00;

  ep_buff[EP_02_CMD_IDX]._size = 0x03;
  ep_buff[EP_82_DATA_IDX]._size = 0x09;

  g_hEP02_Thread_State=THREAD_RUNNING_STATE1;
  g_hEP8x_Thread_State=THREAD_RUNNING_STATE2;
  _do_single_threaded_usb_comm(EP_02_CMD_IDX);
  ReleaseMutex(g_hEP02_Serial_Mutex);
  return SSI_STATUS_MOTION_NORMAL;
};

//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_READ_ZSIGNAL_POS_R(int iType)
{
  WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
  memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);

  *(ep_buff[EP_02_CMD_IDX]._buffer) = CT_DATA;
  if(iType)
  {
    *(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_READ_SEC_REALY;
  }
  else
  {
    *(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_READ_SEC_REALX;
  }
  *(ep_buff[EP_02_CMD_IDX]._buffer+2)=0;

  ep_buff[EP_02_CMD_IDX]._size = 0x03;
  ep_buff[EP_82_DATA_IDX]._size = 0x03;

  g_hEP02_Thread_State=THREAD_RUNNING_STATE1;
  g_hEP8x_Thread_State=THREAD_RUNNING_STATE2;
  _do_single_threaded_usb_comm(EP_02_CMD_IDX);
  ReleaseMutex(g_hEP02_Serial_Mutex);
  return SSI_STATUS_MOTION_NORMAL;
}

//******************************************************************************
long CSO7_Proto_Aux::GetRCurrentPosition(int iType)
{
  long ztos = -1;
  _send_cmd_SO7_CMD_READ_AXIS_R();
  _send_cmd_SO7_CMD_READ_ZSIGNAL_POS_R(iType);
  ztos = abs(g_machine.r._scale_pos._long_ - g_machine.r._ZSignal_pos._long_);
  if(ztos > 8388608)
  {
    ztos = ztos - 9413433;
  }

  return (ztos * 360 / 4720000);
}


//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_STOP_R(int iType)
{
  iType++;
  WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
  memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);

  *(ep_buff[EP_02_CMD_IDX]._buffer) = CT_MOTOR;
  *(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_STOPA;
  *(ep_buff[EP_02_CMD_IDX]._buffer+2)=0x00;

  TRACE("[STOP_R]\n");

  ep_buff[EP_02_CMD_IDX]._size = 0x04;
  ep_buff[EP_81_DATA_IDX]._size = 0x04;

  g_hEP02_Thread_State=THREAD_RUNNING_STATE2;
  _do_single_threaded_usb_comm(EP_02_CMD_IDX);
  ReleaseMutex(g_hEP02_Serial_Mutex);
  return SSI_STATUS_MOTION_NORMAL;
};

//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_SET_SEQ_NUMBER()
{
  WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
  memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);

  *(ep_buff[EP_02_CMD_IDX]._buffer) = CT_DATA;
  *(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_WRITE_SEQ_NUMBER;
  *(ep_buff[EP_02_CMD_IDX]._buffer+2)=g_machine.SEQ_NUMBER;

  ep_buff[EP_02_CMD_IDX]._size = 0x03;
  ep_buff[EP_81_DATA_IDX]._size = 0x45;

  g_hEP02_Thread_State=THREAD_RUNNING_STATE2;
  _do_single_threaded_usb_comm(EP_02_CMD_IDX);
  ReleaseMutex(g_hEP02_Serial_Mutex);
  return SSI_STATUS_MOTION_NORMAL;
}