/* +---------------------------------------------------------------------------+
   |          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/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
//	NOTA MUY IMPORTANTE!!!!!
//		Para que el PC reconozca cualquier placa controladora de los Sonares, PRIMERO hay que conectar la placa al PC y
//		posteriormente conectar la alimentación de los sonares mediante el bus I2C, en otro caso el PC no reconocerá el dispositivo

#include <MRPT/UTILS/utils_defs.h>
#include <MRPT/UTILS/MRPT_OS.h>

#include <lib_hwdrivers/CBoardSonars.h>

using namespace UTILS;
using namespace HWDRIVERS;
using namespace std;

/*-------------------------------------------------------------
						CBoardSonars
-------------------------------------------------------------*/
CBoardSonars::CBoardSonars()
{
	MRPT_TRY_START;
	m_usbSerialNumber = "SONAR001";

	m_gain = 6;
	m_maxRange = 4.0f;

	MRPT_TRY_END;
}

/*-------------------------------------------------------------
						loadConfig
-------------------------------------------------------------*/
void  CBoardSonars::loadConfig(	const CConfigFileBase *configSource,
								const std::string	  &iniSection )
{
	MRPT_TRY_START;

	m_usbSerialNumber	= configSource->read_string(iniSection,"USB_serialNumber",m_usbSerialNumber,true);
	m_gain				= configSource->read_int(iniSection,"gain",m_gain,true);
	m_maxRange			= configSource->read_float(iniSection,"maxRange",m_maxRange,true);
	m_minTimeBetweenPings = configSource->read_float(iniSection,"minTimeBetweenPings",m_minTimeBetweenPings,true);
	configSource->read_vector( iniSection, "firingOrder",m_firingOrder,m_firingOrder, true );

	ASSERT_( m_maxRange>0 && m_maxRange<=11);
	ASSERT_( m_gain<=16);

	MRPT_TRY_END;
}

/*-------------------------------------------------------------
					queryFirmwareVersion
-------------------------------------------------------------*/
bool CBoardSonars::queryFirmwareVersion( 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 CBoardSonars::sendConfigCommands()
{
	try
	{
		if (!isOpen())	return false;
		UTILS::CMRPTMessage		msg,msgRx;
		size_t					i;

		// Send cmd for firing order:
		// ----------------------------
		msg.type = 0x12;
		msg.content.resize(16);
		for (i=0;i<16;i++)
		{
			if (i<m_firingOrder.size())
					msg.content[i] = m_firingOrder[i];
			else	msg.content[i] = 0xFF;
		}
		sendMessage(msg);
		if (!receiveMessage(msgRx) ) return false;	// Error

		// Send cmd for gain:
		// ----------------------------
		msg.type = 0x13;
		msg.content.resize(1);
		msg.content[0] = m_gain;
		sendMessage(msg);
		if (!receiveMessage(msgRx) ) return false;	// Error

		// Send cmd for max range:
		// ----------------------------
		msg.type = 0x14;
		msg.content.resize(1);
		msg.content[0] = (int)((m_maxRange/0.043f) - 1);
		sendMessage(msg);
		if (!receiveMessage(msgRx) ) return false;	// Error

		// Send cmd for max range:
		// ----------------------------
		msg.type = 0x15;
		msg.content.resize(2);
		uint16_t  T = (uint16_t)(m_minTimeBetweenPings * 1000.0f);
		msg.content[0] = T >> 8;
		msg.content[1] = T & 0x00FF;
		sendMessage(msg);
		if (!receiveMessage(msgRx) ) return false;	// Error

		return true;
	}
	catch(...)
	{
		// Error opening device:
		Close();
		return false;
	}
}

/*-------------------------------------------------------------
					getObservation
-------------------------------------------------------------*/
bool CBoardSonars::getObservation( MRML::CObservationRange &obs )
{
	try
	{
		obs.timestamp = SystemUtils::getCurrentTime();
		obs.minSensorDistance = 0.04f;
		obs.maxSensorDistance = m_maxRange;
		obs.sensorConeApperture = DEG2RAD(30.0f);
		obs.sensedData.clear();
		MRML::CObservationRange::TMeasurement obsRange;

		UTILS::CMRPTMessage		msg,msgRx;

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

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

		if (receiveMessage(msgRx) )
		{
			if ( !msgRx.content.size() )
				return false;

			// For each sensor:
			ASSERT_((msgRx.content.size() % 2) == 0);
			vector<uint16_t>	data(msgRx.content.size() / 2);
			memcpy( &data[0], &msgRx.content[0], msgRx.content.size() );

			for (size_t i=0;i<data.size() / 2;i++)
			{
				uint16_t	sonar_idx      = data[2*i + 0];
				uint16_t	sonar_range_cm = data[2*i + 1];
				if ( sonar_range_cm!=0xFFFF && sonar_idx<16 )
				{
					obsRange.sensorID = sonar_idx;
					obsRange.sensorPose = MRML::CPose3D(); // sonar_idx
					obsRange.sensedDistance = sonar_range_cm * 0.01f;
					obs.sensedData.push_back( obsRange );
				}
			}
			return true;
		}
		else
			return false;
	}
	catch(...)
	{
		Close();
		return false;
	}
}

/*-------------------------------------------------------------
					programI2CAddress
-------------------------------------------------------------*/
bool CBoardSonars::programI2CAddress( uint8_t currentAddress, uint8_t newAddress )
{
	try
	{
		UTILS::CMRPTMessage		msg,msgRx;

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

		msg.type = 0x20;
		msg.content.resize(2);
		msg.content[0] = currentAddress;
		msg.content[1] = newAddress;
		sendMessage(msg);

		MRPT_OS::sleep(10);

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

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

	try
	{
		OpenBySerialNumber( m_usbSerialNumber );
		MRPT_OS::sleep(10);
		Purge();
		MRPT_OS::sleep(10);
		SetLatencyTimer(1);
		SetTimeouts(300,100);

		return sendConfigCommands();
	}
	catch(...)
	{
		// Error opening device:
		Close();
		return false;
	}
}
