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

namespace mrpt::graphslam::deciders
{
template <class GRAPH_T>
CICPCriteriaNRD<GRAPH_T>::CICPCriteriaNRD()
	: params(*this)	 // pass reference to self when initializing the parameters
// m_mahal_distance_ICP_odom("Mahalanobis dist (ICP - odom)")
{
	using namespace mrpt::math;
	this->initializeLoggers("CICPCriteriaNRD");

	m_is_using_3DScan = false;

	m_use_angle_difference_node_reg = true;
	m_use_distance_node_reg = true;
	this->resetPDF(&m_latest_odometry_PDF);

	// m_mahal_distance_ICP_odom.resizeWindow(1000); // use the last X
	// mahalanobis distance values

	m_times_used_ICP = 0;
	m_times_used_odom = 0;

	this->logFmt(mrpt::system::LVL_DEBUG, "Initialized class object");
}

template <class GRAPH_T>
bool CICPCriteriaNRD<GRAPH_T>::updateState(
	[[maybe_unused]] mrpt::obs::CActionCollection::Ptr action,
	mrpt::obs::CSensoryFrame::Ptr observations,
	mrpt::obs::CObservation::Ptr observation)
{
	MRPT_START
	this->m_time_logger.enter("updateState");

	using namespace mrpt::obs;
	using namespace mrpt::poses;

	bool registered_new_node = false;

	if (observation)
	{  // Observation-Only Rawlog
		// delegate the action to the method responsible
		if (IS_CLASS(*observation, CObservation2DRangeScan))
		{  // 2D
			mrpt::obs::CObservation2DRangeScan::Ptr curr_laser_scan =
				std::dynamic_pointer_cast<CObservation2DRangeScan>(observation);
			registered_new_node = updateState2D(curr_laser_scan);
		}
		else if (IS_CLASS(*observation, CObservation3DRangeScan))
		{  // 3D
			CObservation3DRangeScan::Ptr curr_laser_scan =
				std::dynamic_pointer_cast<CObservation3DRangeScan>(observation);
			registered_new_node = updateState3D(curr_laser_scan);
		}
		else if (IS_CLASS(*observation, CObservationOdometry))
		{  // odometry
			// if it exists use the odometry information to reject wrong ICP
			// matches
			CObservationOdometry::Ptr obs_odometry =
				std::dynamic_pointer_cast<CObservationOdometry>(observation);

			// not incremental - gives the absolute odometry reading - no InfMat
			// either
			m_curr_odometry_only_pose = obs_odometry->odometry;
			m_latest_odometry_PDF.mean =
				m_curr_odometry_only_pose - m_last_odometry_only_pose;
		}
	}
	else
	{  // action-observations rawlog
		// Action part
		if (action->getBestMovementEstimation())
		{
			// if it exists use the odometry information to reject wrong ICP
			// matches
			CActionRobotMovement2D::Ptr robot_move =
				action->getBestMovementEstimation();
			CPosePDF::Ptr increment = robot_move->poseChange.get_ptr();
			CPosePDFGaussianInf increment_gaussian;
			increment_gaussian.copyFrom(*increment);
			m_latest_odometry_PDF += increment_gaussian;
		}

		// observations part
		if (observations->getObservationByClass<CObservation2DRangeScan>())
		{  // 2D
			CObservation2DRangeScan::Ptr curr_laser_scan =
				observations->getObservationByClass<CObservation2DRangeScan>();
			registered_new_node = updateState2D(curr_laser_scan);
		}
		else if (observations->getObservationByClass<CObservation3DRangeScan>())
		{  // 3D - EXPERIMENTAL, has not been tested
			CObservation3DRangeScan::Ptr curr_laser_scan =
				observations->getObservationByClass<CObservation3DRangeScan>();
			registered_new_node = updateState3D(curr_laser_scan);
		}
	}

	this->m_time_logger.leave("updateState");
	return registered_new_node;

	MRPT_END
}  // end of updateState

template <class GRAPH_T>
bool CICPCriteriaNRD<GRAPH_T>::updateState2D(
	mrpt::obs::CObservation2DRangeScan::Ptr scan2d)
{
	MRPT_START
	bool registered_new_node = false;

	m_curr_laser_scan2D = scan2d;
	if (!m_last_laser_scan2D)
	{
		// initialize the last_laser_scan here - afterwards updated inside the
		// checkRegistrationCondition*D method
		m_last_laser_scan2D = m_curr_laser_scan2D;
	}
	else
	{
		registered_new_node = checkRegistrationCondition2D();
	}

	return registered_new_node;
	MRPT_END
}  // end of updateState2D

template <class GRAPH_T>
bool CICPCriteriaNRD<GRAPH_T>::checkRegistrationCondition2D()
{
	MRPT_START

	using namespace mrpt::math;

	bool registered_new_node = false;

	// Constraint that *may* update incrementally the m_since_prev_node_PDF.
	constraint_t rel_edge;
	mrpt::slam::CICP::TReturnInfo icp_info;

	this->getICPEdge(
		*m_last_laser_scan2D, *m_curr_laser_scan2D, &rel_edge, nullptr,
		&icp_info);

	// Debugging directives
	MRPT_LOG_DEBUG(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
	MRPT_LOG_DEBUG_FMT(
		"ICP Alignment operation:\tnIterations: %d\tgoodness: %.f\n",
		icp_info.nIterations, icp_info.goodness);
	MRPT_LOG_DEBUG_FMT(
		"Current ICP constraint: \n\tEdge: %s\n\tNorm: %f",
		rel_edge.getMeanVal().asString().c_str(), rel_edge.getMeanVal().norm());
	MRPT_LOG_DEBUG_FMT(
		"Corresponding Odometry constraint: \n\tEdge: %s\n\tNorm: %f",
		m_latest_odometry_PDF.getMeanVal().asString().c_str(),
		m_latest_odometry_PDF.getMeanVal().norm());

	// evaluate the mahalanobis distance of the above..
	// If over an (adaptive) threshold, trust the odometry
	double mahal_distance =
		rel_edge.mahalanobisDistanceTo(m_latest_odometry_PDF);
	// m_mahal_distance_ICP_odom.addNewMeasurement(mahal_distance);

	// TODO - Find out a proper criterion
	// How do I filter out the "bad" 2DRangeScans?
	// double mahal_distance_lim = m_mahal_distance_ICP_odom.getMedian();
	// double mahal_distance_lim = m_mahal_distance_ICP_odom.getMean();
	// double mahal_distance_lim =
	// m_mahal_distance_ICP_odom.getMean() +
	// m_mahal_distance_ICP_odom.getStdDev();
	double mahal_distance_lim = 0.18;  // visual introspection

	//
	// check whether to use ICP or odometry Edge.
	//
	// if the norm of the odometry edge is 0, no odometry edge available
	// => use ICP
	if (mahal_distance < mahal_distance_lim ||
		m_latest_odometry_PDF.getMeanVal().norm() == 0)
	{
		MRPT_LOG_DEBUG("Using ICP Edge");
		m_times_used_ICP++;
	}
	else
	{
		MRPT_LOG_DEBUG("Using Odometry Edge");
		rel_edge.copyFrom(m_latest_odometry_PDF);
		m_times_used_odom++;
	}
	MRPT_LOG_DEBUG_FMT("\tMahalanobis Distance = %f", mahal_distance);
	MRPT_LOG_DEBUG_FMT(
		"Times that the ICP Edge was used: %d/%d", m_times_used_ICP,
		m_times_used_ICP + m_times_used_odom);

	// update the PDF until last registered node
	this->m_since_prev_node_PDF += rel_edge;
	m_last_laser_scan2D = m_curr_laser_scan2D;
	registered_new_node = this->checkRegistrationCondition();

	// reset the odometry tracking as well.
	m_last_odometry_only_pose = m_curr_odometry_only_pose;
	this->resetPDF(&m_latest_odometry_PDF);

	MRPT_LOG_DEBUG("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
	return registered_new_node;
	MRPT_END
}  // end of checkRegistrationCondition2D

template <class GRAPH_T>
bool CICPCriteriaNRD<GRAPH_T>::updateState3D(
	mrpt::obs::CObservation3DRangeScan::Ptr scan3d)
{
	THROW_EXCEPTION("Not yet implemented.");
	return false;
}  // end of updateState3D

template <class GRAPH_T>
bool CICPCriteriaNRD<GRAPH_T>::checkRegistrationCondition3D()
{
	THROW_EXCEPTION("Not yet implemented.");
	return false;
}  // end of checkRegistrationCondition3D

template <class GRAPH_T>
bool CICPCriteriaNRD<GRAPH_T>::checkRegistrationCondition()
{
	MRPT_START
	MRPT_LOG_DEBUG("In checkRegistrationCondition");
	using namespace mrpt::math;

	// Criterions for adding a new node
	// - Covered distance since last node > registration_max_distance
	// - Angle difference since last node > registration_max_angle

	bool angle_crit = false;
	if (m_use_angle_difference_node_reg)
	{
		angle_crit =
			fabs(wrapToPi(this->m_since_prev_node_PDF.getMeanVal().phi())) >
			params.registration_max_angle;
	}
	bool distance_crit = false;
	if (m_use_distance_node_reg)
	{
		distance_crit = this->m_since_prev_node_PDF.getMeanVal().norm() >
			params.registration_max_distance;
	}

	// actual check
	bool registered = false;
	if (distance_crit || angle_crit)
	{ registered = this->registerNewNodeAtEnd(); }

	return registered;
	MRPT_END
}  // end of checkRegistrationCondition

template <class GRAPH_T>
void CICPCriteriaNRD<GRAPH_T>::loadParams(const std::string& source_fname)
{
	MRPT_START
	parent_t::loadParams(source_fname);

	params.loadFromConfigFileName(
		source_fname, "NodeRegistrationDeciderParameters");
	// m_mahal_distance_ICP_odom.loadFromConfigFileName(source_fname,
	//"NodeRegistrationDeciderParameters");

	// set the logging level if given by the user
	mrpt::config::CConfigFile source(source_fname);
	// Minimum verbosity level of the logger
	int min_verbosity_level = source.read_int(
		"NodeRegistrationDeciderParameters", "class_verbosity", 1, false);
	this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
	MRPT_LOG_DEBUG("Successfully loaded parameters.");
	MRPT_END
}  // end of loadParams

template <class GRAPH_T>
void CICPCriteriaNRD<GRAPH_T>::printParams() const
{
	parent_t::printParams();
	params.dumpToConsole();
}  // end of printParams

template <class GRAPH_T>
void CICPCriteriaNRD<GRAPH_T>::getDescriptiveReport(
	std::string* report_str) const
{
	MRPT_START
	using namespace std;

	const std::string report_sep(2, '\n');
	const std::string header_sep(80, '#');

	// Report on graph
	stringstream class_props_ss;
	class_props_ss << "ICP Goodness-based Registration Procedure Summary: "
				   << std::endl;
	class_props_ss << header_sep << std::endl;

	// time and output logging
	const std::string time_res = this->m_time_logger.getStatsAsText();
	const std::string output_res = this->getLogAsString();

	// merge the individual reports
	report_str->clear();
	parent_t::getDescriptiveReport(report_str);

	*report_str += class_props_ss.str();
	*report_str += report_sep;

	// loggers results
	*report_str += time_res;
	*report_str += report_sep;

	*report_str += output_res;
	*report_str += report_sep;

	MRPT_END
}  // end of getDescriptiveReport

template <class GRAPH_T>
CICPCriteriaNRD<GRAPH_T>::TParams::TParams(decider_t& d) : decider(d)
{
}

template <class GRAPH_T>
void CICPCriteriaNRD<GRAPH_T>::TParams::dumpToTextStream(
	std::ostream& out) const
{
	MRPT_START

	using namespace mrpt::math;

	out << "------------------[ ICP Fixed Intervals Node Registration "
		   "]------------------\n";
	out << mrpt::format(
		"Max distance for registration = %.2f m\n", registration_max_distance);
	out << mrpt::format(
		"Max Angle for registration    = %.2f deg\n",
		RAD2DEG(registration_max_angle));

	decider.range_ops_t::params.dumpToTextStream(out);

	MRPT_END
}
template <class GRAPH_T>
void CICPCriteriaNRD<GRAPH_T>::TParams::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& source, const std::string& section)
{
	MRPT_START

	using namespace mrpt::math;

	registration_max_distance = source.read_double(
		section, "registration_max_distance", 0.5 /* meter */, false);
	registration_max_angle = source.read_double(
		section, "registration_max_angle", 10 /* degrees */, false);
	registration_max_angle = DEG2RAD(registration_max_angle);

	// load the icp parameters - from "ICP" section explicitly
	decider.range_ops_t::params.loadFromConfigFile(source, "ICP");

	MRPT_END
}
}  // namespace mrpt::graphslam::deciders
