/* +---------------------------------------------------------------------------+
   |          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/CBoardENoses.h>

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

/*-------------------------------------------------------------
					CBoardENoses
-------------------------------------------------------------*/
CBoardENoses::CBoardENoses( )
{
	m_usbSerialNumber = "ENOSE001";
}

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

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

	m_usbSerialNumber = configSource->read_string(iniSection,"USB_serialname","",true);

	configSource->read_vector( iniSection, "enose_poses_x", vector<float>(0), enose_poses_x, true);
	configSource->read_vector( iniSection, "enose_poses_y", vector<float>(0), enose_poses_y, true);
	configSource->read_vector( iniSection, "enose_poses_z", vector<float>(0), enose_poses_z, true);

	configSource->read_vector( iniSection, "enose_poses_yaw", vector<float>(0), enose_poses_yaw, true);
	configSource->read_vector( iniSection, "enose_poses_pitch", vector<float>(0), enose_poses_pitch, true);
	configSource->read_vector( iniSection, "enose_poses_roll", vector<float>(0), enose_poses_roll, true);

	ASSERT_( enose_poses_x.size() == enose_poses_y.size() );
	ASSERT_( enose_poses_x.size() == enose_poses_z.size() );
	ASSERT_( enose_poses_x.size() == enose_poses_yaw.size() );
	ASSERT_( enose_poses_x.size() == enose_poses_pitch.size() );
	ASSERT_( enose_poses_x.size() == enose_poses_roll.size() );

	// Pass angles to radians:
	enose_poses_yaw *= M_PIf / 180.0f;
	enose_poses_pitch *= M_PIf / 180.0f;
	enose_poses_roll *= M_PIf / 180.0f;

	MRPT_TRY_END;
}

/*-------------------------------------------------------------
					queryFirmwareVersion
-------------------------------------------------------------*/
bool CBoardENoses::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 CBoardENoses::checkConnectionAndConnect()
{
	if (isOpen())
		return true;

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


/*-------------------------------------------------------------
					getObservation
-------------------------------------------------------------*/
bool CBoardENoses::getObservation( MRML::CObservationGasSensors &obs )
{
	try
	{
		// Connected?
		if (!checkConnectionAndConnect())	return false;

		UTILS::CMRPTMessage		msg;
		CObservationGasSensors::TObservationENose	newRead;

		obs.m_readings.clear();

		// Send request:
		msg.type = 0x11;
		msg.content.clear();
		sendMessage( msg );

		// Wait for response:
		receiveMessage( msg );

		// Copy to "uint16_t":
		ASSERT_((msg.content.size() % 2)==0);

		if (msg.content.size()>0)
		{
			// Copy to a vector of 16bit integers:
			vector<uint16_t>	readings( msg.content.size() / 2 );
			memcpy( &readings[0],&msg.content[0],msg.content.size() * sizeof(msg.content[0]) );

			// Get the number of eNoses data packs:
			size_t wordsPereNose = 5 /*5 sensors/enose*/ * 2 /*descriptor+data*/;
			ASSERT_((readings.size() % wordsPereNose)==0);
			size_t		M = readings.size() / wordsPereNose;

			// Process them (some may be empty):
			for (size_t i=0;i<M;i++)
			{
				// ------------------------------------------------------------
				// Each "i" comprises the 8 values:
				//  readings[ i*8+0 ] ... readings[ i*8+7 ]
				// Which correspond to the 4 sensors on the i'th eNose:
				// ------------------------------------------------------------

				// Do we have the sensor position?
				if (i<enose_poses_x.size())
				{
					newRead.eNosePoseOnTheRobot = CPose3D(
						enose_poses_x[i],
						enose_poses_y[i],
						enose_poses_z[i],
						enose_poses_yaw[i],
						enose_poses_pitch[i],
						enose_poses_roll[i]);
				}
				else
					newRead.eNosePoseOnTheRobot = CPose3D(0,0,0);

				// Process the sensor codes:
				newRead.sensorTypes.clear();
				newRead.readingsVoltage.clear();
				newRead.hasTemperature = false;
				for (size_t idx=0;idx<wordsPereNose/2;idx++)
				{
					if ( readings[i*wordsPereNose + 2*idx + 0] != 0x0000 )
					{
						// Is temperature?
						if (readings[i*wordsPereNose + 2*idx + 0]==0xFFFF)
						{
							newRead.hasTemperature = true;
							newRead.temperature = ((int16_t)readings[i*wordsPereNose + 2*idx + 1]) / 32.0f;
						}
						else
						{
							// It is not a null code: There is a valid measure:
							newRead.sensorTypes.push_back( readings[i*wordsPereNose + 2*idx + 0] );

							// Pass from ADC value to [0-5] volt range:
							newRead.readingsVoltage.push_back( ( readings[i*wordsPereNose + 2*idx + 1] * 5.0f) / 1024.0f  );
						}
					}
				} // end for each sensor on this eNose

				// Add to observations:
				if ( !newRead.sensorTypes.empty() )
					obs.m_readings.push_back( newRead );

			} // end for each i'th eNose

		} // end if message has data

		// Set timestamp:
		obs.timestamp = SystemUtils::getCurrentTime();

		return !obs.m_readings.empty();	// Done OK!
	}
	catch(exception &)
	{
		//cerr << "[CBoardENoses::getObservation] Returning false due to exception: " << endl;
		//cerr << e.what() << endl;
		return false;
	}
	catch(...)
	{
		return false;
	}
}
