/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */


#include <MRPT/UTILS/MRPT_OS.h>
#include <lib_hwdrivers/CBoardIMU.h>

using namespace UTILS;
using namespace HWDRIVERS;

/*-------------------------------------------------------------
					CBoardIMU
-------------------------------------------------------------*/
CBoardIMU::CBoardIMU( const std::string & serialNumberUSBdevice ) : m_usbSerialNumber(serialNumberUSBdevice)
{

}

/*-------------------------------------------------------------
					~CBoardIMU
-------------------------------------------------------------*/
CBoardIMU::~CBoardIMU()
{
}

/*-------------------------------------------------------------
					queryFirmwareVersion
-------------------------------------------------------------*/
bool CBoardIMU::queryFirmwareVersion( std::string &out_firmwareVersion )
{
	try
	{
		UTILS::CMRPTMessage		msg,msgRx;

		// Try to connect to the device:
		if (!checkConnectionAndConnect())	return false;

		msg.type = 0x10;
		sendMessage(msg);

		if (receiveMessage(msgRx) )
		{
			msgRx.getContentAsString( out_firmwareVersion );
			return true;
		}
		else
			return false;

	}
	catch(...)
	{
		Close();
		return false;
	}
}


/*-------------------------------------------------------------
					checkConnectionAndConnect
-------------------------------------------------------------*/
bool CBoardIMU::checkConnectionAndConnect()
{
	if (isOpen())
		return true;

	try
	{
		OpenBySerialNumber( m_usbSerialNumber );
		SetLatencyTimer(1);
		SetTimeouts(10,100);
		return true;
	}
	catch(...)
	{
		// Error opening device:
		Close();
		return false;
	}
}

/*-------------------------------------------------------------
				 queryAccelerometerReadings
-------------------------------------------------------------*/
bool CBoardIMU::queryAccelerometerReadings( vector_uint	&outReadings )
{
	try
	{
		UTILS::CMRPTMessage		msg,msgRx;

		// Try to connect to the device:
		if (!checkConnectionAndConnect())	return false;

		msg.type = 0x11;
		sendMessage(msg);

		if (receiveMessage(msgRx) )
		{
			ASSERT_(msgRx.content.size()==6*2);
			outReadings.resize(6);
			for (unsigned int i=0,j=0;i<6;i++,j+=2)
			{
				outReadings[i] = msgRx.content[j] | (msgRx.content[j+1]<<8);
			}
			return true;
		}
		else
			return false;

	}
	catch(...)
	{
		Close();
		return false;
	}
}

