/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 |
   +------------------------------------------------------------------------+ */

/*  Plane-based Map (PbMap) library
 *  Construction of plane-based maps and localization in it from RGBD Images.
 *  Writen by Eduardo Fernandez-Moral. See docs for <a
 * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
 */

#pragma once

#include <mrpt/config/CConfigFile.h>
#include <mrpt/system/filesystem.h>

namespace mrpt::pbmap
{
/** A class used to define the heuristic parameters and thresholds used to match
 * sets of planes.
 *  These parameters are loaded from a configuration file.
 *
 * \ingroup mrpt_pbmap_grp
 */
struct config_heuristics
{
	// [global]
	/*! Global parameter to indicate the path to previous PbMaps used for place
	 * recognition*/
	std::string path_prev_pbmaps;

	/*! Global parameter indicating the minimum required plane matches for place
	 * recognition*/
	unsigned min_planes_recognition;  // Minimum number of planes to accept a
	// match between two neighborhoods of
	// planes
	bool use_structure;  // Use inferred knowledge
	bool use_completeness;  // Use inferred knowledge

	// [unary]
	/*! Unary constraint threshold to limit the difference of distances between
	 * two pairs camera-plane (to be used in odometry, i.e. when the two cameras
	 * are nearby */
	float dist_d;
	float angle;

	float color_threshold;
	float intensity_threshold;
	float hue_threshold;
	//    float colorDev_threshold;
	float area_threshold, area_threshold_inv;
	float area_full_threshold, area_full_threshold_inv;
	float area_half_threshold,
		area_half_threshold_inv;  // Constraint with "half confidence": one
	// plana is fully observed
	float elongation_threshold, elongation_threshold_inv;  // Unary constraint:
	int graph_mode;  // This var selects the condition to create edges in the
	// graph, either proximity of planar patches or
	// co-visibility in a single frame

	// [binary]
	/*! Binary constraint threshold to limit the difference of distances between
	 * the centers of two pair of planes. This threashold should be loose if the
	 * plane boundaries are not known, which is the most common situation */
	float dist_threshold, dist_threshold_inv;
	float angle_threshold;  // Binary constraint: the angle between the normals
	// of two pair of planes
	float height_threshold;  // Binary constraint: Height from one plane wrt a
	// neighbor (in meters)
	float height_threshold_parallel;  // Binary constraint: Height from one
	// plane wrt a neighbor (in meters) for
	// complete marked planes
	float cos_angle_parallel;

	/*! Load the PbMap registration thresholds from an .ini file */
	void load_params(const std::string& config_file_name)
	{
		ASSERT_FILE_EXISTS_(config_file_name);
		mrpt::config::CConfigFile config_file(config_file_name);

		// global
		path_prev_pbmaps =
			config_file.read_string("global", "path_prev_pbmaps", "", false);
		//  std::cout << path_prev_pbmaps << std::endl;
		use_structure = config_file.read_bool("global", "use_structure", true);
		use_completeness =
			config_file.read_bool("global", "use_completeness", true);
		min_planes_recognition =
			config_file.read_int("global", "min_planes_recognition", 4);
		graph_mode = config_file.read_int("global", "graph_mode", 1);

		// Unary constraints
		dist_d = config_file.read_float("unary", "dist_d", 0.5);
		angle = config_file.read_float("unary", "angle", 50);
		angle = cos(angle * 3.14159 / 180);

		color_threshold =
			config_file.read_float("unary", "color_threshold", 0.09);
		intensity_threshold =
			config_file.read_float("unary", "intensity_threshold", 175.0);
		hue_threshold = config_file.read_float("unary", "hue_threshold", 0.3);
		//      colorDev_threshold =
		//      config_file.read_float("unary","colorDev_threshold",0.005);
		area_threshold = config_file.read_float("unary", "area_threshold", 3.0);
		area_threshold_inv = 1 / area_threshold;
		area_full_threshold =
			config_file.read_float("unary", "area_full_threshold", 1.6);
		area_full_threshold_inv = 1 / area_full_threshold;
		area_half_threshold =
			config_file.read_float("unary", "area_half_threshold", 2.0);
		area_half_threshold_inv = 1 / area_half_threshold;
		elongation_threshold =
			config_file.read_float("unary", "elongation_threshold", 2.9);
		elongation_threshold_inv = 1 / elongation_threshold;

		// Binary constraints
		dist_threshold =
			config_file.read_float("binary", "dist_threshold", 2.0);
		dist_threshold_inv = 1 / dist_threshold;
		angle_threshold =
			config_file.read_float("binary", "angle_threshold", 7.0);
		height_threshold =
			config_file.read_float("binary", "height_threshold", 0.2);
		height_threshold_parallel =
			config_file.read_float("binary", "height_threshold_parallel", 0.2);
		cos_angle_parallel =
			config_file.read_float("binary", "cos_angle_parallel", 0.985);
	};

	/*! Print the threshold for registration */
	void print_params()
	{
		using std::cout;
		using std::endl;

		cout << "Unary thresholds:\n";
		cout << "dist_d " << dist_d << endl;
		cout << "angle " << angle << endl;
		cout << "color_threshold " << color_threshold << endl;
		cout << "intensity_threshold " << intensity_threshold << endl;
		cout << "hue_threshold " << hue_threshold << endl;
		cout << "area_threshold " << area_threshold << endl;
		cout << "elongation_threshold " << elongation_threshold << endl;

		cout << "Binary thresholds:\n";
		cout << "angle_threshold " << angle_threshold << endl;
		cout << "angle " << angle << endl;
		cout << "height_threshold " << height_threshold << endl;
	};
};
}


