/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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 "maps-precomp.h"  // Precomp header
//
#include <mrpt/core/round.h>
#include <mrpt/img/color_maps.h>
#include <mrpt/maps/CWirelessPowerGridMap2D.h>
#include <mrpt/obs/CObservationWirelessPower.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace std;

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
	"mrpt::maps::CWirelessPowerGridMap2D,wifiGrid",
	mrpt::maps::CWirelessPowerGridMap2D)

CWirelessPowerGridMap2D::TMapDefinition::TMapDefinition() = default;

void CWirelessPowerGridMap2D::TMapDefinition::loadFromConfigFile_map_specific(
	const mrpt::config::CConfigFileBase& source,
	const std::string& sectionNamePrefix)
{
	// [<sectionNamePrefix>+"_creationOpts"]
	const std::string sSectCreation =
		sectionNamePrefix + string("_creationOpts");
	MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
	MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
	MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
	MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
	MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
	mapType = source.read_enum<CWirelessPowerGridMap2D::TMapRepresentation>(
		sSectCreation, "mapType", mapType);

	insertionOpts.loadFromConfigFile(
		source, sectionNamePrefix + string("_insertOpts"));
}

void CWirelessPowerGridMap2D::TMapDefinition::dumpToTextStream_map_specific(
	std::ostream& out) const
{
	out << mrpt::format(
		"MAP TYPE                                  = %s\n",
		mrpt::typemeta::TEnumType<
			CWirelessPowerGridMap2D::TMapRepresentation>::value2name(mapType)
			.c_str());
	LOADABLEOPTS_DUMP_VAR(min_x, double);
	LOADABLEOPTS_DUMP_VAR(max_x, double);
	LOADABLEOPTS_DUMP_VAR(min_y, double);
	LOADABLEOPTS_DUMP_VAR(max_y, double);
	LOADABLEOPTS_DUMP_VAR(resolution, double);

	this->insertionOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap*
	CWirelessPowerGridMap2D::internal_CreateFromMapDefinition(
		const mrpt::maps::TMetricMapInitializer& _def)
{
	const CWirelessPowerGridMap2D::TMapDefinition& def =
		*dynamic_cast<const CWirelessPowerGridMap2D::TMapDefinition*>(&_def);
	auto* obj = new CWirelessPowerGridMap2D(
		def.mapType, def.min_x, def.max_x, def.min_y, def.max_y,
		def.resolution);
	obj->insertionOptions = def.insertionOpts;
	return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(
	CWirelessPowerGridMap2D, CRandomFieldGridMap2D, mrpt::maps)

/*---------------------------------------------------------------
						Constructor
  ---------------------------------------------------------------*/
CWirelessPowerGridMap2D::CWirelessPowerGridMap2D(
	TMapRepresentation mapType, double x_min, double x_max, double y_min,
	double y_max, double resolution)
	: CRandomFieldGridMap2D(mapType, x_min, x_max, y_min, y_max, resolution),
	  insertionOptions()
{
	// Set the grid to initial values (and adjusts the KF covariance matrix!)
	//  Also, calling clear() is mandatory to end initialization of our base
	//  class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
	CMetricMap::clear();
}

CWirelessPowerGridMap2D::~CWirelessPowerGridMap2D() = default;
/*---------------------------------------------------------------
						clear
  ---------------------------------------------------------------*/
void CWirelessPowerGridMap2D::internal_clear()
{
	// Just do the generic clear:
	CRandomFieldGridMap2D::internal_clear();

	// Anything else special for this derived class?
	// ...
}

/*---------------------------------------------------------------
						insertObservation
  ---------------------------------------------------------------*/
bool CWirelessPowerGridMap2D::internal_insertObservation(
	const CObservation& obs,
	const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
	MRPT_START

	CPose2D robotPose2D;
	CPose3D robotPose3D;

	if (robotPose)
	{
		robotPose2D = CPose2D(*robotPose);
		robotPose3D = (*robotPose);
	}
	else
	{
		// Default values are (0,0,0)
	}

	if (IS_CLASS(obs, CObservationWirelessPower))
	{
		/********************************************************************
					OBSERVATION TYPE: CObservationWirelessPower
		********************************************************************/
		const auto& o = dynamic_cast<const CObservationWirelessPower&>(obs);
		float sensorReading;

		// Compute the 3D sensor pose in world coordinates:
		CPose2D sensorPose = CPose2D(robotPose3D + o.sensorPoseOnRobot);

		sensorReading = o.power;

		// Normalization:
		sensorReading = (sensorReading - insertionOptions.R_min) /
			(insertionOptions.R_max - insertionOptions.R_min);

		// Update the gross estimates of mean/vars for the whole reading history
		// (see IROS2009 paper):
		m_average_normreadings_mean =
			(sensorReading +
			 m_average_normreadings_count * m_average_normreadings_mean) /
			(1 + m_average_normreadings_count);
		m_average_normreadings_var =
			(square(sensorReading - m_average_normreadings_mean) +
			 m_average_normreadings_count * m_average_normreadings_var) /
			(1 + m_average_normreadings_count);
		m_average_normreadings_count++;

		// Finally, do the actual map update with that value:
		this->insertIndividualReading(
			sensorReading,
			mrpt::math::TPoint2D(sensorPose.x(), sensorPose.y()));

		return true;  // Done!

	}  // end if "CObservationWirelessPower"

	return false;

	MRPT_END
}

/*---------------------------------------------------------------
						computeObservationLikelihood
  ---------------------------------------------------------------*/
double CWirelessPowerGridMap2D::internal_computeObservationLikelihood(
	[[maybe_unused]] const CObservation& obs,
	[[maybe_unused]] const CPose3D& takenFrom) const
{
	THROW_EXCEPTION("Not implemented yet!");
}

uint8_t CWirelessPowerGridMap2D::serializeGetVersion() const { return 5; }
void CWirelessPowerGridMap2D::serializeTo(
	mrpt::serialization::CArchive& out) const
{
	dyngridcommon_writeToStream(out);

	// To ensure compatibility: The size of each cell:
	auto n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
	out << n;

	// Save the map contents:
	n = static_cast<uint32_t>(m_map.size());
	out << n;

// Save the "m_map": This requires special handling for big endian systems:
#if MRPT_IS_BIG_ENDIAN
	for (uint32_t i = 0; i < n; i++)
	{
		out << m_map[i].kf_mean() << m_map[i].dm_mean()
			<< m_map[i].dmv_var_mean;
	}
#else
	// Little endian: just write all at once:
	out.WriteBuffer(
		&m_map[0],
		sizeof(m_map[0]) * m_map.size());  // TODO: Do this endianness safe!!
#endif

	// Version 1: Save the insertion options:
	out << uint8_t(m_mapType) << m_cov << m_stackedCov;

	out << insertionOptions.sigma << insertionOptions.cutoffRadius
		<< insertionOptions.R_min << insertionOptions.R_max
		<< insertionOptions.KF_covSigma << insertionOptions.KF_initialCellStd
		<< insertionOptions.KF_observationModelNoise
		<< insertionOptions.KF_defaultCellMeanValue
		<< insertionOptions.KF_W_size;

	// New in v3:
	out << m_average_normreadings_mean << m_average_normreadings_var
		<< uint64_t(m_average_normreadings_count);

	out << genericMapParams;  // v4
}

// Aux struct used below (must be at global scope for STL):
struct TOldCellTypeInVersion1
{
	float mean, std;
	float w, wr;
};

void CWirelessPowerGridMap2D::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		case 3:
		case 4:
		case 5:
		{
			dyngridcommon_readFromStream(in, version < 5);

			// To ensure compatibility: The size of each cell:
			uint32_t n;
			in >> n;

			if (version < 2)
			{  // Converter from old versions <=1
				ASSERT_(
					n == static_cast<uint32_t>(sizeof(TOldCellTypeInVersion1)));
				// Load the map contents in an aux struct:
				in >> n;
				vector<TOldCellTypeInVersion1> old_map(n);
				in.ReadBuffer(&old_map[0], sizeof(old_map[0]) * old_map.size());

				// Convert to newer format:
				m_map.resize(n);
				for (size_t k = 0; k < n; k++)
				{
					m_map[k].kf_mean() =
						(old_map[k].w != 0) ? old_map[k].wr : old_map[k].mean;
					m_map[k].kf_std() =
						(old_map[k].w != 0) ? old_map[k].w : old_map[k].std;
				}
			}
			else
			{
				ASSERT_EQUAL_(
					n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
				// Load the map contents:
				in >> n;
				m_map.resize(n);

// Read the note in writeToStream()
#if MRPT_IS_BIG_ENDIAN
				for (uint32_t i = 0; i < n; i++)
					in >> m_map[i].kf_mean() >> m_map[i].dm_mean() >>
						m_map[i].dmv_var_mean;
#else
				// Little endian: just read all at once:
				in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
#endif
			}

			// Version 1: Insertion options:
			if (version >= 1)
			{
				uint8_t i;
				in >> i;
				m_mapType = TMapRepresentation(i);

				in >> m_cov >> m_stackedCov;

				in >> insertionOptions.sigma >> insertionOptions.cutoffRadius >>
					insertionOptions.R_min >> insertionOptions.R_max >>
					insertionOptions.KF_covSigma >>
					insertionOptions.KF_initialCellStd >>
					insertionOptions.KF_observationModelNoise >>
					insertionOptions.KF_defaultCellMeanValue >>
					insertionOptions.KF_W_size;
			}

			if (version >= 3)
			{
				uint64_t N;
				in >> m_average_normreadings_mean >>
					m_average_normreadings_var >> N;
				m_average_normreadings_count = N;
			}

			if (version >= 4) in >> genericMapParams;

			m_hasToRecoverMeanAndCov = true;
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

CWirelessPowerGridMap2D::TInsertionOptions::TInsertionOptions() = default;
void CWirelessPowerGridMap2D::TInsertionOptions::dumpToTextStream(
	std::ostream& out) const
{
	out << "\n----------- [CWirelessPowerGridMap2D::TInsertionOptions] "
		   "------------ \n\n";
	internal_dumpToTextStream_common(
		out);  // Common params to all random fields maps:

	out << "\n";
}

/*---------------------------------------------------------------
					loadFromConfigFile
  ---------------------------------------------------------------*/
void CWirelessPowerGridMap2D::TInsertionOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
	// Common data fields for all random fields maps:
	internal_loadFromConfigFile_common(iniFile, section);
}

void CWirelessPowerGridMap2D::getVisualizationInto(
	mrpt::opengl::CSetOfObjects& o) const
{
	MRPT_START
	if (!genericMapParams.enableSaveAs3DObject) return;
	CRandomFieldGridMap2D::getVisualizationInto(o);
	MRPT_END
}
