/* +------------------------------------------------------------------------+
   |                     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/CVectorDynamic.h>
#include <mrpt/math/TLine2D.h>
#include <mrpt/math/TPlane.h>
#include <mrpt/math/ransac.h>

namespace mrpt::math
{
using std::vector;

/** @addtogroup ransac_grp
 * @{ */

/** @name RANSAC detectors
	@{
  */

/** Fit a number of 3-D planes to a given point cloud, automatically determining
 * the number of existing planes by means of the provided threshold and minimum
 * number of supporting inliers.
 * \param out_detected_planes The output list of pairs: number of supporting
 * inliers, detected plane.
 * \param threshold The maximum distance between a point and a temptative plane
 * such as the point is considered an inlier.
 * \param min_inliers_for_valid_plane  The minimum number of supporting inliers
 * to consider a plane as valid.
 */
template <typename NUMTYPE>
void ransac_detect_3D_planes(
	const CVectorDynamic<NUMTYPE>& x, const CVectorDynamic<NUMTYPE>& y,
	const CVectorDynamic<NUMTYPE>& z,
	std::vector<std::pair<size_t, TPlane>>& out_detected_planes,
	const double threshold, const size_t min_inliers_for_valid_plane = 10);

/** Fit a number of 2-D lines to a given point cloud, automatically determining
 * the number of existing lines by means of the provided threshold and minimum
 * number of supporting inliers.
 * \param out_detected_lines The output list of pairs: number of supporting
 * inliers, detected line.
 * \param threshold The maximum distance between a point and a temptative line
 * such as the point is considered an inlier.
 * \param min_inliers_for_valid_line  The minimum number of supporting inliers
 * to consider a line as valid.
 */
template <typename NUMTYPE>
void ransac_detect_2D_lines(
	const CVectorDynamic<NUMTYPE>& x, const CVectorDynamic<NUMTYPE>& y,
	std::vector<std::pair<size_t, TLine2D>>& out_detected_lines,
	const double threshold, const size_t min_inliers_for_valid_line = 5);

/** A stub for ransac_detect_3D_planes() with the points given as a
 * mrpt::maps::CPointsMap
 */
template <class POINTSMAP>
inline void ransac_detect_3D_planes(
	const POINTSMAP* points_map,
	std::vector<std::pair<size_t, TPlane>>& out_detected_planes,
	const double threshold, const size_t min_inliers_for_valid_plane)
{
	CVectorFloat xs, ys, zs;
	points_map->getAllPoints(xs, ys, zs);
	ransac_detect_3D_planes(
		xs, ys, zs, out_detected_planes, threshold,
		min_inliers_for_valid_plane);
}

/** @} */
/** @} */  // end of grouping

}  // namespace mrpt::math
