/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/containers/CDynamicGrid3D.h>
#include <mrpt/graphs/ScalarFactorGraph.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/serialization/CSerializable.h>
#include <mrpt/system/COutputLogger.h>

namespace mrpt::maps
{
/** The contents of each voxel in a CRandomFieldGridMap3D map.
 * \ingroup mrpt_maps_grp
 **/
#if defined(MRPT_IS_X86_AMD64)	// Pragma defined to ensure no structure
// packing: since we'll serialize
// TRandomFieldVoxel to streams, we want it not
// to depend on compiler options, etc.
#pragma pack(push, 1)
#endif
struct TRandomFieldVoxel
{
	/** Mean and sigma (standard deviation) estimated values for the voxel. */
	double mean_value, stddev_value;

	/** Constructor */
	TRandomFieldVoxel(double _mean_value = .0, double _stddev_value = .0)
		: mean_value(_mean_value), stddev_value(_stddev_value)
	{
	}
};
#if defined(MRPT_IS_X86_AMD64)
#pragma pack(pop)
#endif

/** CRandomFieldGridMap3D represents a 3D regular grid where each voxel is
 * associated one real-valued property which is to be estimated by this class.
 *
 *  This class implements a Gaussian Markov Random Field (GMRF) estimator, with
 * each voxel being connected to its
 *   6 immediate neighbors (Up, down, left, right, front, back).
 *  - See papers:
 *    - "Time-variant gas distribution mapping with obstacle information",
 * Monroy, J. G., Blanco, J. L., & Gonzalez-Jimenez, J. Autonomous Robots,
 * 40(1), 1-16, 2016.
 *
 *  Note that this class does not derive from mrpt::maps::CMetricMap since the
 * estimated values do not have sensor-especific semantics,
 *  i.e. the grid can be used to estimate temperature, gas concentration, etc.
 *
 *  Usage:
 *  - Define grid size with either constructor or via `setSize()`.
 *  - Initialize the map with `initialize()`. This resets the contents of the
 * map, so previously-added observations will be lost.
 *  - Add observations of 3D voxels with `insertIndividualReading()`
 *
 * Custom connectivity patterns can be defined with setVoxelsConnectivity().
 *
 * \sa mrpt::maps::CRandomFieldGridMap3D
 * \ingroup mrpt_maps_grp
 * \note [New in MRPT 1.5.0]
 */
class CRandomFieldGridMap3D
	: public mrpt::containers::CDynamicGrid3D<TRandomFieldVoxel>,
	  public mrpt::serialization::CSerializable,
	  public mrpt::system::COutputLogger
{
	using BASE = mrpt::containers::CDynamicGrid3D<TRandomFieldVoxel>;

	DEFINE_SERIALIZABLE(CRandomFieldGridMap3D, mrpt::maps)
   public:
	/** [default:false] Enables a profiler to show a performance report at
	 * application end. */
	static bool ENABLE_GMRF_PROFILER;

	/** Constructor.
	 * If you set call_initialize_now to false, the object will be initialized
	 * immediately (without the heavy initialization of the GMRF),
	 * but you then must call `setSize()` or `clear()` later to properly
	 * initialize the object before using it to insert observations.
	 */
	CRandomFieldGridMap3D(
		double x_min = -2, double x_max = 2, double y_min = -2,
		double y_max = 2, double z_min = -2, double z_max = 2,
		double voxel_size = 0.5, bool call_initialize_now = true);

	/** Erases all added observations and start again with an empty gridmap. */
	void clear() override;

	/** Save the current estimated mean values to a CSV file (compatible with
	 * Paraview) with fields `x y z mean_value`.
	 * Optionally, std deviations can be also saved to another file with fields
	 * `x y z stddev_value`, if `filName_stddev` is provided.
	 * \return false on error writing to file
	 */
	bool saveAsCSV(
		const std::string& filName_mean,
		const std::string& filName_stddev = std::string()) const;

	/** Parameters common to any derived class.
	 *  Derived classes should derive a new struct from this one, plus
	 * mrpt::config::CLoadableOptions,
	 *  and call the internal_* methods where appropiate to deal with the
	 * variables declared here.
	 *  Derived classes instantions of their "TInsertionOptions" MUST set the
	 * pointer "m_insertOptions_common" upon construction.
	 */
	struct TInsertionOptions : public mrpt::config::CLoadableOptions
	{
		/** Default values loader */
		TInsertionOptions();

		/** See mrpt::config::CLoadableOptions */
		void loadFromConfigFile(
			const mrpt::config::CConfigFileBase& source,
			const std::string& section) override;

		/** See mrpt::config::CLoadableOptions */
		void dumpToTextStream(std::ostream& out) const override;

		/** @name Gaussian Markov Random Fields method
			@{ */
		/** The information (Lambda) of fixed map constraints */
		double GMRF_lambdaPrior{0.01f};
		/** (Default:false) Skip the computation of the variance, just compute
		 * the mean */
		bool GMRF_skip_variance{false};
		/** @} */
	};

	/** \sa updateMapEstimation() */
	TInsertionOptions insertionOptions;

	/** Changes the size of the grid, maintaining previous contents. \sa setSize
	 */
	void resize(
		double new_x_min, double new_x_max, double new_y_min, double new_y_max,
		double new_z_min, double new_z_max,
		const TRandomFieldVoxel& defaultValueNewvoxels,
		double additionalMarginMeters = 2.0) override;

	/** Changes the size of the grid, erasing previous contents.If
	 * `resolution_z`<0, the same resolution will be used for all dimensions
	 * x,y,z as given in `resolution_xy` \sa resize.*/
	void setSize(
		const double x_min, const double x_max, const double y_min,
		const double y_max, const double z_min, const double z_max,
		const double resolution_xy, const double resolution_z = -1.0,
		const TRandomFieldVoxel* fill_value = nullptr) override;

	/** Base class for user-supplied objects capable of describing voxels
	 * connectivity, used to build prior factors of the MRF graph. \sa
	 * setvoxelsConnectivity() */
	struct ConnectivityDescriptor
	{
		using Ptr = std::shared_ptr<ConnectivityDescriptor>;
		// Virtual destructor for polymorphic type.
		virtual ~ConnectivityDescriptor() = default;
		/** Implement the check of whether node i=(icx,icy,icz) is connected
		 * with node j=(jcx,jcy,jcy).
		 * This visitor method will be called only for immediate neighbors.
		 * \return true if connected (and the "information" value should be also
		 * updated in out_edge_information), false otherwise.
		 */
		virtual bool getEdgeInformation(
			/** The parent map on which we are running */
			const CRandomFieldGridMap3D* parent,
			/** (cx,cy,cz) for node "i" */
			size_t icx, size_t icy, size_t icz,
			/** (cx,cy,cz) for node "j" */
			size_t jcx, size_t jcy, size_t jcz,
			/** Must output here the inverse of the variance of the constraint
			   edge. */
			double& out_edge_information) = 0;
	};

	/** Sets a custom object to define the connectivity between voxels. Must
	 * call clear() or setSize() afterwards for the changes to take place. */
	void setVoxelsConnectivity(
		const ConnectivityDescriptor::Ptr& new_connectivity_descriptor);

	enum TVoxelInterpolationMethod
	{
		gimNearest = 0,
		gimBilinear
	};

	/** Direct update of the map with a reading in a given position of the map.
	 * \return false if point is out of the grid extension.
	 */
	bool insertIndividualReading(
		/** [in] The value observed in the (x,y,z) position */
		const double sensorReading,
		/** [in] The variance of the sensor observation */
		const double sensorVariance,
		/** [in] The (x,y,z) location */
		const mrpt::math::TPoint3D& point,
		/** [in] Voxel interpolation method: how many voxels will be
		   affected by the reading */
		const TVoxelInterpolationMethod method,
		/** [in] Run a global map update after inserting this observation
		   (algorithm-dependant) */
		const bool update_map);

	/** Run the method-specific procedure required to ensure that the mean &
	 * variances are up-to-date with all inserted observations, using parameters
	 * in insertionOptions */
	void updateMapEstimation();

   protected:
	/** Internal: called called after each change of resolution, size, etc. to
	 * build the prior factor information */
	void internal_initialize(bool erase_prev_contents = true);

	/** Empty: default */
	ConnectivityDescriptor::Ptr m_gmrf_connectivity;

	mrpt::graphs::ScalarFactorGraph m_gmrf;

	struct TObservationGMRF
		: public mrpt::graphs::ScalarFactorGraph::UnaryFactorVirtualBase
	{
		/** Observation value */
		double obsValue;
		/** "Information" of the observation (=inverse of the variance) */
		double Lambda;

		double evaluateResidual() const override;
		double getInformation() const override;
		void evalJacobian(double& dr_dx) const override;

		TObservationGMRF(CRandomFieldGridMap3D& parent)
			: obsValue(.0), Lambda(.0), m_parent(&parent)
		{
		}

	   private:
		CRandomFieldGridMap3D* m_parent;
	};

	struct TPriorFactorGMRF
		: public mrpt::graphs::ScalarFactorGraph::BinaryFactorVirtualBase
	{
		/** "Information" of the observation (=inverse of the variance) */
		double Lambda;

		double evaluateResidual() const override;
		double getInformation() const override;
		void evalJacobian(double& dr_dx_i, double& dr_dx_j) const override;

		TPriorFactorGMRF(CRandomFieldGridMap3D& parent)
			: Lambda(.0), m_parent(&parent)
		{
		}

	   private:
		CRandomFieldGridMap3D* m_parent;
	};

	/** Vector with the active observations and their respective Information,
	 * for each map cell. */
	std::vector<std::deque<TObservationGMRF>> m_mrf_factors_activeObs;
	/** Vector with the precomputed priors for each GMRF model */
	std::deque<TPriorFactorGMRF> m_mrf_factors_priors;
};

}  // namespace mrpt::maps
