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 "rawlog-edit-declarations.h"
  
  #include <mrpt/img/color_maps.h>
  #include <mrpt/math/interp_fit.h>
  #include <mrpt/obs/CObservationGPS.h>
  #include <mrpt/obs/CObservationGasSensors.h>
  #include <mrpt/topography.h>
  
  using namespace mrpt;
  using namespace mrpt::obs;
  using namespace mrpt::system;
  using namespace mrpt::rawlogtools;
  using namespace mrpt::topography;
  using namespace std;
  using namespace mrpt::io;
  
  // STL data must have global scope:
  struct TGPSGASDataPoint
  {
  	double lon, lat, alt;  // degrees, degrees, meters
  	uint8_t fix;  // 1: standalone, 2: DGPS, 4: RTK fix, 5: RTK float, ...
  	float color;
  };
  
  struct TDataPerGPSGAS
  {
  	map<TTimeStamp, TGPSGASDataPoint> path;
  };
  
  // ======================================================================
  //		op_export_gps_gas_kml
  // ======================================================================
  DECLARE_OP_FUNCTION(op_export_gps_gas_kml)
  {
  	// A class to do this operation:
  	class CRawlogProcessor_ExportGPSGAS_KML
  		: public CRawlogProcessorOnEachObservation
  	{
  	   protected:
  		string m_inFile;
  
  		map<string, TDataPerGPSGAS> m_gps_paths;  // sensorLabel -> data
  
  		mrpt::obs::CObservationGPS* obs;
  		mrpt::obs::CObservationGasSensors* obsGas;
  		float gasConcentration;
  		float maxGasValue, minGasValue;
  		bool hasGAS, hasGPS;
  
  	   public:
  		CRawlogProcessor_ExportGPSGAS_KML(
  			CFileGZInputStream& in_rawlog, TCLAP::CmdLine& cmdline,
  			bool _verbose)
  			: CRawlogProcessorOnEachObservation(in_rawlog, cmdline, _verbose)
  		{
  			getArgValue<string>(cmdline, "input", m_inFile);
  
  			maxGasValue = -1.0;
  			minGasValue = 10000;
  			obs = nullptr;
  			obsGas = nullptr;
  			hasGAS = false;
  			hasGPS = false;
  		}
  
  		// return false on any error.
  		bool processOneObservation(CObservation::Ptr& o) override
  		{
  			if (IS_CLASS(*o, CObservationGPS))
  			{
  				obs = dynamic_cast<CObservationGPS*>(o.get());
  				if (!obs->has_GGA_datum())
  				{
  					obs = nullptr;
  					return true;  // Nothing to do...
  				}
  				hasGPS = true;
  			}
  			else if (IS_CLASS(*o, CObservationGasSensors))
  			{
  				obsGas = dynamic_cast<CObservationGasSensors*>(o.get());
  				if (obsGas->m_readings.size() < 1)
  				{
  					cout << "Empty Gas Sensor" << endl;
  					obsGas = nullptr;
  					return true;  // Nothing to do...
  				}
  
  				if (obsGas->m_readings[0].readingsVoltage.size() < 1)
  				{
  					cout << "Empty Gas Obs" << endl;
  					obsGas = nullptr;
  					return true;  // Nothing to do...
  				}
  				// cout << "new observations has: "<<
  				// obsGas->m_readings[0].readingsVoltage.size() << " elements."
  				// << endl;
  				gasConcentration = obsGas->m_readings[0].readingsVoltage[0];
  				hasGAS = true;
  			}
  			else
  				return true;
  
  			// Insert the new entries:
  			if (hasGAS && hasGPS)
  			{
  				TDataPerGPSGAS& D = m_gps_paths[obs->sensorLabel];
  				TGPSGASDataPoint& d = D.path[o->timestamp];
  				const mrpt::obs::gnss::Message_NMEA_GGA& gga =
  					obs->getMsgByClass<mrpt::obs::gnss::Message_NMEA_GGA>();
  				d.lon = gga.fields.longitude_degrees;
  				d.lat = gga.fields.latitude_degrees;
  				d.alt = gga.fields.altitude_meters;
  				d.fix = gga.fields.fix_quality;
  				d.color = gasConcentration;
  
  				if (d.color > maxGasValue) maxGasValue = d.color;
  				if (d.color < minGasValue) minGasValue = d.color;
  
  				hasGAS = false;
  				hasGPS = false;
  			}
  
  			return true;  // All ok
  		}
  
  		void generate_KML()
  		{
  			const bool save_altitude = false;
  
  			const string outfilname =
  				mrpt::system::fileNameChangeExtension(m_inFile, "kml");
  			VERBOSE_COUT << "Writing KML file: " << outfilname << endl;
  
  			CFileOutputStream f(outfilname);
  
  			// Header:
  			f.printf(
  				"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
  				"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
  				"<!-- File automatically generated by rawlog-edit \n"
  				"      Part of the MRPT initiative - https://www.mrpt.org/ \n"
  				"      Generated on %s from file '%s'  -->\n"
  				"  <Document>\n"
  				"    <name>GPS-GAS Paths</name>\n"
  				"    <description>GPS-GAS paths from dataset "
  				"'%s'</description>\n",
  				mrpt::system::dateTimeLocalToString(mrpt::system::now())
  					.c_str(),
  				m_inFile.c_str(), m_inFile.c_str());
  
  			// For each GPS sensor label:
  			for (auto it = m_gps_paths.begin(); it != m_gps_paths.end(); ++it)
  			{
  				const string& label = it->first;  // GPS Label
  				const TDataPerGPSGAS& D = it->second;
  
  				f.printf(
  					"    <Folder>\n"
  					"      <name>%s all points</name>\n"
  					"      <description>%s: All received points (for all "
  					"quality levels)</description>\n",
  					label.c_str(), label.c_str());
  
  				// For each GPS point
  				int n = 0;
  				float scale = 1;  // scale to dispay in KML
  				int alpha = 120;  //[0 255]
  				for (auto itP = D.path.begin(); itP != D.path.end(); ++itP, n++)
  				{
  					const TGPSGASDataPoint& d = itP->second;
  
  					// Decode gas concentration [0,1] to color RGBA
  					float r, g, b;
  					mrpt::img::jet2rgb(
  						(d.color - minGasValue) / (maxGasValue - minGasValue),
  						r, g, b);
  
  					f.printf(
  						"        <Placemark>\n"
  						"          <description> %s point: %i </description>\n"
  						"          <Style>\n"
  						"            <IconStyle>\n"
  						"              <color> %02X%02X%02X%02X </color>\n"
  						"              <scale> %.2f </scale>\n"
  						"              "
  						"<Icon><href>http://maps.google.com/mapfiles/kml/"
  						"shapes/shaded_dot.png</href></Icon>\n"
  						"            </IconStyle>\n"
  						"          </Style>\n"
  						"          <Point>\n"
  						"            <coordinates>",
  						label.c_str(), n, alpha, (int)floor(b * 255),
  						(int)floor(g * 255), (int)floor(r * 255), scale);
  
  					if (save_altitude)
  						f.printf(" %.15f,%.15f,%.3f ", d.lon, d.lat, d.alt);
  					else
  						f.printf(" %.15f,%.15f ", d.lon, d.lat);
  
  					f.printf(
  						"</coordinates>\n"
  						"          </Point>\n"
  						"        </Placemark>\n");
  				}
  
  				// end part:
  				f.printf("    </Folder>\n");
  
  			}  // end for each sensor label
  
  			f.printf(
  				"  </Document>\n"
  				"</kml>\n");
  		}  // end generate_KML
  
  	};  // end CRawlogProcessor_ExportGPS_KML
  
  	// Process
  	// ---------------------------------
  	CRawlogProcessor_ExportGPSGAS_KML proc(in_rawlog, cmdline, verbose);
  	proc.doProcessRawlog();
  
  	// Now that the entire rawlog is parsed, do the actual output:
  	proc.generate_KML();
  
  	// Dump statistics:
  	// ---------------------------------
  	VERBOSE_COUT << "Time to process file (sec)        : " << proc.m_timToParse
  				 << "\n";
  }
  