//////////////////////////////////////////////////////////////////////
//
// HAL_Motion.cpp : interface for the Motion related functions
//
//////////////////////////////////////////////////////////////////////

#include "StdAfx.h"

#include "HAL.h"
#include "HSI.h"

extern CHAL           *g_pHSI;
extern CLogger        *g_pLogger;
extern CHALMicroVuExt  g_HSIExt;
extern CMv_Proto      *g_pMv_Proto;
//extern HANDLE          hEventEStop;

extern double TimeInMsecs(void);

//extern HANDLE  hMotionInitDoneEvent;
static double PositionSampleTime = 3.0;

//===========================================================================================================
void HAL_Motion_HomeStatus(bool &bHomed)
{
  bHomed = g_pMv_Proto->mv_motion_is_homed();
  return;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_IsSupported(UINT &Type)
{
  static int isDCC = -1;
  Type = HAL_MOTION_SUPPORTS_MANUAL_CONTROL|HAL_MOTION_SUPPORTS_HOMING | HAL_MOTION_SUPPORTS_DCC_CONTROL;
  return HAL_STATUS_NORMAL;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_Startup( bool /*bHome*/ )
{
  HAL_STATUS Status = HAL_STATUS_NORMAL;
  //  g_HSIExt.m_ActiveMachineType = HAL_MACHINE_TYPE_EXCEL;
  // If the machine type hasn't been selected then exit the initialisation
#pragma message("what is the correct return status if no machine type selected")
  if(g_HSIExt.m_ActiveMachineType == -1)
    return Status;
  return Status;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetAxisCount( enum HAL_MOTION_AXIS_TYPE AxisType, int &nAxis )
{
  switch(AxisType)
  {
  case HAL_MOTION_AXIS_X:
  case HAL_MOTION_AXIS_Y:
  case HAL_MOTION_AXIS_Z:
    nAxis = 1;
    break;
  case HAL_MOTION_AXIS_R:
  default:
      nAxis = 0;
    break;
  }
  HAL_SendDebug(_T("HAL_Motion_GetAxisCount %d\n"), nAxis);
  return HAL_STATUS_NORMAL;
}


//===========================================================================================================
HAL_STATUS HAL_Motion_SetSpeedXYZ( double dSpeed )
{
  g_pMv_Proto->mv_motion_set_speed_xyz(dSpeed);
  HAL_STATUS Status = HAL_STATUS_NORMAL;
  return Status;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetSpeedXYZ( double &dSpeed )
{
  HAL_STATUS Status = HAL_STATUS_NORMAL;
  g_pMv_Proto->mv_motion_get_speed_xyz(dSpeed);
  return Status;
}

////===========================================================================================================
//HAL_STATUS HAL_Motion_SetSpeedXYZ1( double SpeedX, double SpeedY, double SpeedZ )
//{
//  HAL_STATUS Status = HAL_STATUS_NORMAL;
//  long lSpeedX = (long) SpeedX;
//  long lSpeedY = (long) SpeedY;
//  long lSpeedZ = (long) SpeedZ;
//  g_pMv_Proto->mv_motion_set_speed_xyz(lSpeedX, lSpeedY, lSpeedZ);
//  return Status;
//}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetSpeedR( double & /*Speed*/ )
{
  return HAL_STATUS_NOT_SUPPORTED;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_SetSpeedR( double /*Speed*/ )
{
  HAL_STATUS Status = HAL_STATUS_NOT_SUPPORTED;
  return Status;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetAccelerationXYZ( double &AccelX, double &AccelY, double &AccelZ )
{
  HAL_STATUS Status = HAL_STATUS_NORMAL;
  return Status;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_SetAccelerationXYZ( double AccelX, double AccelY, double AccelZ )
{
  HAL_STATUS Status = HAL_STATUS_NORMAL;
  return Status;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetAccelerationR( double & /*AccelR*/ )
{
  return HAL_STATUS_NOT_SUPPORTED;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_SetAccelerationR( double /*AccelQ*/ )
{
  return HAL_STATUS_NOT_SUPPORTED;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetPositionXYZ( UINT /*GetAxisTypes*/, double &PositionX, double &PositionY, double &PositionZ, double &Time)
{
  SSI_STATUS Status = g_pMv_Proto->mv_motion_get_position_xyz(PositionX, PositionY, PositionZ);   
  Time = TimeInMsecs();
  return (Status == SSI_STATUS_NORMAL)?HAL_STATUS_NORMAL:HAL_STATUS_FAILED;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_SetPositionXYZ( UINT /*SetAxisType*/, double PositionX, double PositionY, double PositionZ, enum HAL_MOTION_MOVE_TYPE eType, double /*flyRadius*/)
{
  bool bWait = (eType == HAL_MOTION_MOVE_WAIT)?true:false;
  SSI_STATUS Status = g_pMv_Proto->mv_motion_set_position_xyz(PositionX, PositionY, PositionZ, bWait);   
  return (Status == SSI_STATUS_NORMAL)?HAL_STATUS_NORMAL:HAL_STATUS_FAILED;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetPositionR( UINT /*GetAxisTypes*/, double & /*PositionR*/, double & /*Time*/ )
{
  g_pMv_Proto->mv_motion_get_position_r();   
  return HAL_STATUS_NOT_SUPPORTED;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetSettleTime( double &SettleTime )
{
  SettleTime=g_HSIExt.m_StageSettleTime;
  return HAL_STATUS_NORMAL;
}

////===========================================================================================================
HAL_STATUS HAL_Motion_GetDeadband( double &DeadbandX, double &DeadbandY, double &DeadbandZ, double &DeadbandR )
{ 
  HAL_STATUS retVal=HAL_STATUS_NOT_SUPPORTED;
  return retVal;
}

////===========================================================================================================
HAL_STATUS HAL_Motion_GetRefreshDeadband( double &Deadband )
{  // not used because the controller will set this automatically
  HAL_STATUS retVal=HAL_STATUS_NOT_SUPPORTED;
  return retVal;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_IsHomed( bool &bHomed )
{
  bHomed = g_pMv_Proto->mv_motion_is_homed();
  return HAL_STATUS_NORMAL;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetStageLimits(double *StageLimitsMax, double *StageLimitsMin)
{
//  StageLimitsMax[HAL_AXIS_X] =  
//  StageLimitsMin[HAL_AXIS_X] =  
//  StageLimitsMax[HAL_AXIS_Y] =  g_pMv_Proto
//  StageLimitsMin[HAL_AXIS_Y] =  
//  StageLimitsMax[HAL_AXIS_Z] =  
//  StageLimitsMin[HAL_AXIS_Z] =  
  return HAL_STATUS_NORMAL;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_GetMaxSpeed(double *MaxSpeed)
{
  SSI_STATUS Status = SSI_STATUS_ERROR;
  if (MaxSpeed) 
    Status = g_pMv_Proto->mv_motion_get_3D_max_speed(*MaxSpeed); 
  return (Status == SSI_STATUS_NORMAL) ? HAL_STATUS_NORMAL : HAL_STATUS_FAILED;
}

//===========================================================================================================
HAL_STATUS HAL_Motion_Shutdown( )
{
  HAL_STATUS Status = HAL_STATUS_NORMAL;
  return Status;
}