#define  OMSController_CPP			// this will allow including libraries specific to this file only

#include "OMSController.h"
extern "C" {
#include "OMSUtil.h"				// "C" functions handling all axes
};

// ---- Constructors -----------------------------------
OMSController::
OMSController(char* boardName) {
	open(boardName);
}

OMSController::
OMSController() : boardHandle(NULL) {}; 

OMSController::
~OMSController() {
	close();
}

// ---- Member Functions -------------------------------

// ---------- Opening and Closing ----------------------

// Opens the board, initiating the session
bool OMSController::
open (char* boardName) {
	boardHandle = GetOmsHandle(boardName);
	return isOpened();
}


bool OMSController::
close()
{
	if (isOpened())	{					// if controller opened..
		CloseOmsHandle(boardHandle);	// .. close it
		boardHandle = NULL;
		return true;
	}
	return false;
}

inline bool OMSController::
isOpened() const 
{ 
	return boardHandle != NULL; 
}





// ---------- Moving -----------------------------------


void OMSController::
moveTo (const ActuatorPositions& p) 
			throw(RobotControllerException)
{
	int error = MoveOmsAllAxesAbs( boardHandle
						, p[0], p[1], p[2], p[3], p[4], p[5] );
	throwExceptionIfError( error, "moveTo");
}

class TestClass{};

void OMSController::
goHome() 
			throw(RobotControllerException)
{
	int error =					// error return should be implemented in the future
	HomeOmsAllAxes( boardHandle );

	throwExceptionIfError( error, "moveTo");

}







// ---------- Setting Parameters -----------------------
inline void OMSController::
setVelocity    (double velocity) 
{
	int error = SetOmsAllVelocities( boardHandle, (long int) velocity );

	throwExceptionIfError( error, "setVelocity");
}


inline void OMSController::
setAcceleration(double acceleration) 
{
	int error = SetOmsAllAccelerations( boardHandle, (long int) acceleration );

	throwExceptionIfError( error, "setAcceleration");

}


// ---------- Helper Functions -------------------------

inline void OMSController::
throwExceptionIfError(int errorCode, char* message) 
		throw (RobotControllerException)
{
	if( errorCode != SUCCESS ) {
		throw( RobotControllerException() );
	}
}