/* +------------------------------------------------------------------------+
   |                     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/math/CMatrixF.h>

namespace mrpt::obs
{
/** Used in CObservation3DRangeScan::unprojectInto() */
struct TRangeImageFilterParams
{
	/** Only used if <b>both</b> rangeMask_min and rangeMask_max are present.
	 * This switches which condition must fulfill a range `D` to be accepted as
	 * valid:
	 *  - `rangeCheckBetween=true` : valid =  (D>=rangeMask_min &&
	 * D<=rangeMask_max)
	 *  - `rangeCheckBetween=false`: valid = !(D>=rangeMask_min &&
	 * D<=rangeMask_max)
	 *
	 * \note Default value:true */
	bool rangeCheckBetween{true};
	/** (Default: nullptr) If provided, each data range will be tested to be
	 * greater-than (rangeMask_min) or less-than (rangeMask_max) each element in
	 * these matrices
	 * for each direction (row,col). Values of 0.0f mean no filtering at those
	 * directions.
	 * If both `rangeMask_min` and `rangeMask_max` are provided, the joint
	 * filtering operation is determined by `rangeCheckBetween` */
	const mrpt::math::CMatrixF *rangeMask_min{nullptr}, *rangeMask_max{nullptr};

	/** If enabled, the range pixels of points that do NOT pass the mask filter
	 * will be marked as invalid ranges (=0) in the source 3D observation
	 * object. */
	bool mark_invalid_ranges{false};

	TRangeImageFilterParams() = default;
};

/** Mainly for internal use within
 * CObservation3DRangeScan::unprojectInto() */
struct TRangeImageFilter
{
	TRangeImageFilterParams fp;
	/** Returns true if the point (r,c) with depth D passes all filters. */
	inline bool do_range_filter(size_t r, size_t c, const float D) const;
	inline TRangeImageFilter(const TRangeImageFilterParams& filter_params)
		: fp(filter_params)
	{
	}
	inline TRangeImageFilter() = default;
};

// ======== Implementation ========
bool TRangeImageFilter::do_range_filter(size_t r, size_t c, const float D) const
{
	// Filters:
	if (D <= .0f) return false;
	// Greater-than/Less-than filters:
	bool pass_gt = true, pass_lt = true;
	bool has_min_filter = false, has_max_filter = false;
	if (fp.rangeMask_min)
	{
		const float min_d = fp.rangeMask_min->coeff(r, c);
		if (min_d != .0f)
		{
			has_min_filter = true;
			pass_gt = (D >= min_d);
		}
	}
	if (fp.rangeMask_max)
	{
		const float max_d = fp.rangeMask_max->coeff(r, c);
		if (max_d != .0f)
		{
			has_max_filter = true;
			pass_lt = (D <= max_d);
		}
	}
	if (has_min_filter && has_max_filter)
	{
		return fp.rangeCheckBetween ? (pass_gt && pass_lt)
									: !(pass_gt && pass_lt);
	}
	else
		return pass_gt && pass_lt;
}
}  // namespace mrpt::obs
