text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          https://www.mrpt.org/                         |
     |                                                                        |
     | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
     | See: https://www.mrpt.org/Authors - All rights reserved.               |
     | Released under BSD License. See: https://www.mrpt.org/License          |
     +------------------------------------------------------------------------+ */
  
  #include "obs-precomp.h"  // Precompiled headers
  
  #include <mrpt/math/CVectorDynamic.h>
  #include <mrpt/obs/CObservationGasSensors.h>
  #include <mrpt/serialization/CArchive.h>
  #include <mrpt/system/os.h>
  #include <fstream>
  #include <iostream>
  
  using namespace mrpt::obs;
  using namespace mrpt::poses;
  using namespace mrpt::math;
  using namespace std;
  
  // This must be added to any CSerializable class implementation file.
  IMPLEMENTS_SERIALIZABLE(CObservationGasSensors, CObservation, mrpt::obs)
  
  /** Constructor
   */
  CObservationGasSensors::CObservationGasSensors() : m_readings() {}
  uint8_t CObservationGasSensors::serializeGetVersion() const { return 5; }
  void CObservationGasSensors::serializeTo(
  	mrpt::serialization::CArchive& out) const
  {
  	uint32_t i, n = m_readings.size();
  	out << n;
  
  	for (i = 0; i < n; i++)
  	{
  		out << CPose3D(m_readings[i].eNosePoseOnTheRobot);
  		out << m_readings[i].readingsVoltage;
  		out << m_readings[i].sensorTypes;
  		out << m_readings[i].hasTemperature;
  		if (m_readings[i].hasTemperature) out << m_readings[i].temperature;
  	}
  
  	out << sensorLabel << timestamp;
  }
  
  void CObservationGasSensors::serializeFrom(
  	mrpt::serialization::CArchive& in, uint8_t version)
  {
  	switch (version)
  	{
  		case 2:
  		case 3:
  		case 4:
  		case 5:
  		{
  			// A general set of e-nose descriptors:
  			uint32_t i, n;
  
  			in >> n;
  			m_readings.resize(n);
  
  			CPose3D aux;
  
  			for (i = 0; i < n; i++)
  			{
  				in >> aux;
  				m_readings[i].eNosePoseOnTheRobot = aux.asTPose();
  				in >> m_readings[i].readingsVoltage;
  				in >> m_readings[i].sensorTypes;
  				if (version >= 3)
  				{
  					in >> m_readings[i].hasTemperature;
  					if (m_readings[i].hasTemperature)
  						in >> m_readings[i].temperature;
  				}
  				else
  				{
  					m_readings[i].hasTemperature = false;
  					m_readings[i].temperature = 0;
  				}
  			}
  
  			if (version >= 4)
  				in >> sensorLabel;
  			else
  				sensorLabel = "";
  
  			if (version >= 5)
  				in >> timestamp;
  			else
  				timestamp = INVALID_TIMESTAMP;
  		}
  		break;
  		case 0:
  		case 1:
  		{
  			TObservationENose eNose;
  
  			m_readings.clear();
  
  			// There was a single set of 16 values from "Sancho" (DEC-2006)
  			CVectorFloat readings;
  			in >> readings;
  
  			ASSERT_(readings.size() == 16);
  
  			// There was TWO e-noses:
  			// (1)
  			eNose.eNosePoseOnTheRobot =
  				TPose3D(0.20, -0.15, 0.10, 0, 0, 0);  // (x,y,z) only
  			eNose.readingsVoltage.resize(4);
  			eNose.readingsVoltage[0] = readings[2];
  			eNose.readingsVoltage[1] = readings[4];
  			eNose.readingsVoltage[2] = readings[5];
  			eNose.readingsVoltage[3] = readings[6];
  
  			eNose.sensorTypes.clear();
  			eNose.sensorTypes.resize(4, 0);
  			m_readings.push_back(eNose);
  
  			// (2)
  			eNose.eNosePoseOnTheRobot =
  				TPose3D(0.20, 0.15, 0.10, .0, .0, .0);  // (x,y,z) only
  			eNose.readingsVoltage.resize(4);
  			eNose.readingsVoltage[0] = readings[8];
  			eNose.readingsVoltage[1] = readings[10];
  			eNose.readingsVoltage[2] = readings[12];
  			eNose.readingsVoltage[3] = readings[14];
  
  			eNose.sensorTypes.clear();
  			eNose.sensorTypes.resize(4, 0);
  			m_readings.push_back(eNose);
  		}
  		break;
  		default:
  			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  	};
  }
  
  /*---------------------------------------------------------------
  					 getSensorPose
   ---------------------------------------------------------------*/
  void CObservationGasSensors::getSensorPose(CPose3D& out_sensorPose) const
  {
  	if (!m_readings.empty())
  		out_sensorPose = CPose3D(m_readings[0].eNosePoseOnTheRobot);
  	else
  		out_sensorPose = CPose3D(0, 0, 0);
  }
  
  void CObservationGasSensors::setSensorPose(const CPose3D& newSensorPose)
  {
  	for (auto& r : m_readings) r.eNosePoseOnTheRobot = newSensorPose.asTPose();
  }
  
  bool CObservationGasSensors::CMOSmodel::get_GasDistribution_estimation(
  	float& reading, mrpt::system::TTimeStamp& timestamp)
  {
  	try
  	{
  		// Noise filtering
  		noise_filtering(reading, timestamp);
  
  		// Decimate
  		if (decimate_count != decimate_value)
  		{
  			decimate_count++;
  			return false;
  		}
  
  		// Gas concentration estimation based on FIRST ORDER + NONLINEAR
  		// COMPENSATIONS DYNAMICS
  		inverse_MOSmodeling(
  			m_antiNoise_window[winNoise_size / 2].reading_filtered,
  			m_antiNoise_window[winNoise_size / 2].timestamp);
  		decimate_count = 1;
  
  		// update (output)
  		reading = last_Obs.estimation;
  		timestamp = last_Obs.timestamp;
  
  		// Save data map in log file for Matlab visualization
  		if (save_maplog)
  			save_log_map(
  				last_Obs.timestamp, last_Obs.reading, last_Obs.estimation,
  				last_Obs.tau);
  
  		return true;
  	}
  	catch (...)
  	{
  		cout << "Error when decimating \n";
  		mrpt::system::pause();
  		return false;
  	}
  }
  
  /*---------------------------------------------------------------
  					 noise_filtering (smooth)
   ---------------------------------------------------------------*/
  void CObservationGasSensors::CMOSmodel::noise_filtering(
  	float reading, const mrpt::system::TTimeStamp& timestamp)
  {
  	try
  	{
  		// Store values in the temporal Observation
  		temporal_Obs.reading = reading;
  		temporal_Obs.timestamp = timestamp;
  
  		// If first reading from E-nose
  		if (m_antiNoise_window.empty())
  		{
  			// use default values
  			temporal_Obs.reading_filtered = reading;
  
  			// Populate the noise window
  			m_antiNoise_window.assign(winNoise_size, temporal_Obs);
  		}
  		else
  		{
  			// Erase the first element (the oldest), and add the new one
  			m_antiNoise_window.erase(m_antiNoise_window.begin());
  			m_antiNoise_window.push_back(temporal_Obs);
  		}
  
  		// Average data to reduce noise (Noise Filtering)
  		float partial_sum = 0;
  		for (auto& i : m_antiNoise_window) partial_sum += i.reading;
  
  		m_antiNoise_window.at(winNoise_size / 2).reading_filtered =
  			partial_sum / winNoise_size;
  	}
  	catch (...)
  	{
  		cout << "Error when filtering noise from readings \n";
  		mrpt::system::pause();
  	}
  }
  
  /*---------------------------------------------------------------
  				inverse_MOSmodeling
   ---------------------------------------------------------------*/
  void CObservationGasSensors::CMOSmodel::inverse_MOSmodeling(
  	float reading, const mrpt::system::TTimeStamp& timestamp)
  {
  	try
  	{
  		// Keep the minimum reading value as an approximation to the basline
  		// level
  		if (reading < min_reading) min_reading = reading;
  
  		// Check if estimation posible (not possible in the first iteration)
  		if (!first_iteration)
  		{
  			// Assure the samples are provided at constant rate (important for
  			// the correct gas distribution estimation)
  			double incT =
  				mrpt::system::timeDifference(last_Obs.timestamp, timestamp);
  
  			if ((incT > 0) & (!first_incT))
  			{  // not the same sample due to initialization of buffers
  				if (fixed_incT == 0)
  					fixed_incT = incT;
  				else
  					// ASSERT_(fabs(incT - fixed_incT) < (double)(0.05));
  					if (fabs(incT - fixed_incT) > (double)(0.05))
  					cout << "IncT is not constant by HW." << endl;
  			}
  			else
  			{
  				if (incT > 0) first_incT = false;
  			}
  
  			// slope<0 -->Decay
  			if (reading < last_Obs.reading)
  			{
  				last_Obs.tau = a_decay * abs(reading - min_reading) + b_decay;
  			}
  			else  // slope>=0 -->rise
  			{
  				last_Obs.tau = a_rise * abs(reading - min_reading) + b_rise;
  			}  // end-if
  
  			// New estimation values -- Ziegler-Nichols model --
  			if (incT > 0)
  				// Initially there may come repetetive values till
  				// m_antiNoise_window is full populated.
  				last_Obs.estimation =
  					((reading - last_Obs.reading) * last_Obs.tau / incT) +
  					reading;
  			else
  				last_Obs.estimation = reading;
  
  			// Prepare the New observation
  			last_Obs.timestamp = timestamp;
  			last_Obs.reading = reading;
  		}
  		else
  		{
  			// First filtered reading (use default values)
  			last_Obs.tau = b_rise;
  			last_Obs.reading = reading;
  			last_Obs.timestamp = timestamp;
  			last_Obs.estimation =
  				reading;  // No estimation possible at this step
  			first_iteration = false;
  		}  // end-if estimation values
  	}
  	catch (const exception& e)
  	{
  		cerr << "**Exception in "
  				"CObservationGasSensors::CMOSmodel::inverse_MOSmodeling** "
  			 << e.what() << endl;
  	}
  }
  
  /*---------------------------------------------------------------
  						save_log_map
    ---------------------------------------------------------------*/
  void CObservationGasSensors::CMOSmodel::save_log_map(
  	const mrpt::system::TTimeStamp& timestamp, float reading, float estimation,
  	float tau)
  {
  	// function to save in a log file the information of the generated gas
  	// distribution estimation
  
  	double time = mrpt::system::timestampTotime_t(timestamp);
  	char buffer[50];
  	sprintf(buffer, "./log_MOSmodel_GasDistribution.txt");
  
  	if (!m_debug_dump) m_debug_dump = new ofstream(buffer);
  
  	if (m_debug_dump->is_open())
  	{
  		*m_debug_dump << format("%f \t", time);
  		*m_debug_dump << format("%f \t", reading);
  		*m_debug_dump << format("%f \t", estimation);
  		*m_debug_dump << format("%f \t", tau);
  		*m_debug_dump << "\n";
  	}
  	else
  		cout << "Unable to open file";
  }
  
  void CObservationGasSensors::getDescriptionAsText(std::ostream& o) const
  {
  	using namespace std;
  	CObservation::getDescriptionAsText(o);
  
  	for (size_t j = 0; j < m_readings.size(); j++)
  	{
  		o << format("e-nose #%u:\n", (unsigned)j);
  
  		vector<float>::const_iterator it;
  		std::vector<int>::const_iterator itKind;
  
  		ASSERT_(
  			m_readings[j].readingsVoltage.size() ==
  			m_readings[j].sensorTypes.size());
  
  		for (it = m_readings[j].readingsVoltage.begin(),
  			itKind = m_readings[j].sensorTypes.begin();
  			 it != m_readings[j].readingsVoltage.end(); it++, itKind++)
  			o << format("%04X: %.03f ", *itKind, *it);
  
  		o << endl;
  
  		o << format(
  			"  Sensor pose on robot: (x,y,z)=(%.02f,%.02f,%.02f)\n",
  			m_readings[j].eNosePoseOnTheRobot.x,
  			m_readings[j].eNosePoseOnTheRobot.y,
  			m_readings[j].eNosePoseOnTheRobot.z);
  
  		o << "Measured temperature: ";
  		if (m_readings[j].hasTemperature)
  			o << format("%.03f degC\n", m_readings[j].temperature);
  		else
  			o << "NOT AVAILABLE\n";
  	}
  }
  