/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/math/ransac.h>
#include <mrpt/math/geometry.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 Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& x,
	const Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& y,
	const Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& 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 Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& x,
	const Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& 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
