/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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 "apps-precomp.h"  // Precompiled headers
//
#include <mrpt/obs/CObservationRawDAQ.h>

#include "rawlog-edit-declarations.h"

using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::system;
using namespace mrpt::apps;
using namespace std;
using namespace mrpt::io;

#define WRITE_AIN_SAMPLES(_VECTOR_NAME, _FRMT, _TYPECAST)                      \
	if (!obs->_VECTOR_NAME.empty())                                            \
	{                                                                          \
		if (obs->AIN_interleaved)                                              \
		{                                                                      \
			for (i = 0, tim = tim0;                                            \
				 i < obs->_VECTOR_NAME.size() / obs->AIN_channel_count;        \
				 i++, tim += At)                                               \
			{                                                                  \
				::fprintf(f_this, "%14.4f ", tim);                             \
				for (size_t j = 0; j < obs->AIN_channel_count; j++)            \
				{                                                              \
					::fprintf(                                                 \
						f_this, _FRMT " ",                                     \
						(_TYPECAST)obs                                         \
							->_VECTOR_NAME[i * obs->AIN_channel_count + j]);   \
					m_entriesSaved++;                                          \
				}                                                              \
				::fprintf(f_this, "\n");                                       \
			}                                                                  \
		}                                                                      \
		else                                                                   \
		{                                                                      \
			const size_t nSmpls =                                              \
				obs->_VECTOR_NAME.size() / obs->AIN_channel_count;             \
			for (i = 0, tim = tim0; i < nSmpls; i++, tim += At)                \
			{                                                                  \
				::fprintf(f_this, "%14.4f ", tim);                             \
				for (size_t j = 0; j < obs->AIN_channel_count; j++)            \
				{                                                              \
					::fprintf(                                                 \
						f_this, _FRMT " ",                                     \
						(_TYPECAST)obs->_VECTOR_NAME[i + j * nSmpls]);         \
					m_entriesSaved++;                                          \
				}                                                              \
				::fprintf(f_this, "\n");                                       \
			}                                                                  \
		}                                                                      \
	}

#define WRITE_OTHER_SAMPLES(_VECTOR_NAME, _FRMT, _TYPECAST)                    \
	if (!obs->_VECTOR_NAME.empty())                                            \
	{                                                                          \
		for (i = 0, tim = tim0; i < obs->_VECTOR_NAME.size(); i++, tim += At)  \
		{                                                                      \
			::fprintf(f_this, "%14.4f ", tim);                                 \
			::fprintf(f_this, _FRMT " ", (_TYPECAST)obs->_VECTOR_NAME[i]);     \
			m_entriesSaved++;                                                  \
		}                                                                      \
		::fprintf(f_this, "\n");                                               \
	}

// ======================================================================
//		op_export_rawdaq_txt
// ======================================================================
DECLARE_OP_FUNCTION(op_export_rawdaq_txt)
{
	// A class to do this operation:
	class CRawlogProcessor_ExportRAWDAQ_TXT
		: public CRawlogProcessorOnEachObservation
	{
	   protected:
		string m_inFile;

		map<string, FILE*> lstFiles;
		string m_filPrefix;

	   public:
		size_t m_entriesSaved;

		CRawlogProcessor_ExportRAWDAQ_TXT(
			CFileGZInputStream& in_rawlog, TCLAP::CmdLine& cmdline,
			bool Verbose)
			: CRawlogProcessorOnEachObservation(in_rawlog, cmdline, Verbose),
			  m_entriesSaved(0)
		{
			getArgValue<string>(cmdline, "input", m_inFile);

			m_filPrefix =
				extractFileDirectory(m_inFile) + extractFileName(m_inFile);
		}

		// return false on any error.
		bool processOneObservation(CObservation::Ptr& o) override
		{
			if (!IS_CLASS(*o, CObservationRawDAQ)) return true;

			const CObservationRawDAQ* obs =
				dynamic_cast<CObservationRawDAQ*>(o.get());

			auto it = lstFiles.find(obs->sensorLabel);

			FILE* f_this;

			if (it == lstFiles.end())  // A new file for this sensorlabel??
			{
				const std::string fileName = m_filPrefix + string("_") +
					fileNameStripInvalidChars(obs->sensorLabel) +
					string(".txt");

				VERBOSE_COUT << "Writing RAW DAQ TXT file: " << fileName
							 << endl;

				f_this = lstFiles[obs->sensorLabel] =
					os::fopen(fileName.c_str(), "wt");
				if (!f_this)
					THROW_EXCEPTION_FMT(
						"Cannot open output file for write: %s",
						fileName.c_str());

				// The first line is a description of the columns:
				::fprintf(
					f_this,
					"%% "
					"%14s "	 // Time
					"%23s"
					"\n",
					"Time", "Measured values (meaning is channel-dependant)");
			}
			else
				f_this = it->second;

			const double tim0 = mrpt::system::timestampTotime_t(obs->timestamp);
			const double At =
				obs->sample_rate != 0.0 ? 1.0 / obs->sample_rate : 0.0;
			double tim;
			size_t i;

			// Analog IN channels:
			if (obs->AIN_channel_count > 0)
			{
				// Save file:
				WRITE_AIN_SAMPLES(AIN_8bits, "%4u", unsigned int)
				WRITE_AIN_SAMPLES(AIN_16bits, "%6u", unsigned int)
				WRITE_AIN_SAMPLES(AIN_32bits, "%8u", unsigned int)
				WRITE_AIN_SAMPLES(AIN_float, "%9.5f", double)
				WRITE_AIN_SAMPLES(AIN_double, "%9.5f", double)
			}

			// Encoder channels:
			WRITE_OTHER_SAMPLES(CNTRIN_32bits, "%f", double)
			WRITE_OTHER_SAMPLES(CNTRIN_double, "%f", double)

			return true;  // All ok
		}

		// Destructor: close files and generate summary files:
		~CRawlogProcessor_ExportRAWDAQ_TXT()
		{
			for (auto it = lstFiles.begin(); it != lstFiles.end(); ++it)
			{
				os::fclose(it->second);
			}
			lstFiles.clear();
		}  // end of destructor
	};

	// Process
	// ---------------------------------
	CRawlogProcessor_ExportRAWDAQ_TXT proc(in_rawlog, cmdline, verbose);
	proc.doProcessRawlog();

	// Dump statistics:
	// ---------------------------------
	VERBOSE_COUT << "Time to process file (sec)        : " << proc.m_timToParse
				 << "\n";
	VERBOSE_COUT << "Number of records saved           : "
				 << proc.m_entriesSaved << "\n";
}
