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

#include "nav-precomp.h"  // Precomp header
//
#include <mrpt/nav/reactive/CMultiObjectiveMotionOptimizerBase.h>
#include <mrpt/system/string_utils.h>

#include <limits>

using namespace mrpt::nav;

IMPLEMENTS_VIRTUAL_MRPT_OBJECT(
	CMultiObjectiveMotionOptimizerBase, CObject, mrpt::nav)

CMultiObjectiveMotionOptimizerBase::CMultiObjectiveMotionOptimizerBase(
	TParamsBase& params)
	: m_params_base(params)
{
}

int CMultiObjectiveMotionOptimizerBase::decide(
	const std::vector<mrpt::nav::TCandidateMovementPTG>& movs,
	TResultInfo& extra_info)
{
	auto& score_values = extra_info.score_values;
	score_values.resize(movs.size());

	// For each movement:
	for (unsigned int mov_idx = 0; mov_idx < movs.size(); ++mov_idx)
	{
		const auto& m = movs[mov_idx];

		// Mark all vars as NaN so we detect uninitialized values:
		for (auto& p : m_expr_vars)
		{
			p.second = std::numeric_limits<double>::quiet_NaN();
		}

		for (const auto& prop : m.props)
		{
			double& var = m_expr_vars[prop.first];
			var = prop.second;
		}

		// Upon first iteration: compile expressions
		if (m_score_exprs.size() != m_params_base.formula_score.size())
		{
			m_score_exprs.clear();

			for (const auto& f : m_params_base.formula_score)
			{
				auto& se = m_score_exprs[f.first];
				try
				{
					se.compile(
						f.second, m_expr_vars,
						std::string("score: ") + f.first);
				}
				catch (std::exception&)
				{
					m_score_exprs.clear();
					throw;	// rethrow
				}

				// Register formulas also as variables, usable by the assert()
				// expressions:
				{
					auto it = m_expr_vars.find(f.first);
					if (it != m_expr_vars.end())
					{
						THROW_EXCEPTION_FMT(
							"Error: Expression name `%s` already exists as an "
							"input variable.",
							f.first.c_str());
					}
					// Add it:
					m_expr_vars[f.first] =
						std::numeric_limits<double>::quiet_NaN();
				}
			}
		}  // end for each score expr

		// Upon first iteration: compile expressions
		if (m_movement_assert_exprs.size() !=
			m_params_base.movement_assert.size())
		{
			const size_t N = m_params_base.movement_assert.size();
			m_movement_assert_exprs.clear();
			m_movement_assert_exprs.resize(N);
			for (size_t i = 0; i < N; i++)
			{
				const auto& str = m_params_base.movement_assert[i];
				auto& ce = m_movement_assert_exprs[i];

				try
				{
					ce.compile(str, m_expr_vars, "assert");
				}
				catch (std::exception&)
				{
					m_movement_assert_exprs.clear();
					throw;	// rethrow
				}
			}
		}

		// For each score: evaluate it
		for (auto& sc : m_score_exprs)
		{
			// Evaluate:
			double val;
			if (m.speed <= 0)  // Invalid candidate
			{ val = .0; }
			else
			{
				val = sc.second.eval();
			}

			if (val != val /* NaN */)
			{
				THROW_EXCEPTION_FMT(
					"Undefined value evaluating score `%s` for mov_idx=%u!",
					sc.first.c_str(), mov_idx);
			}

			// Store:
			score_values[mov_idx][sc.first] = val;
		}
	}  // end for mov_idx

	// Optional score post-processing: normalize highest value to 1.0
	for (const auto& sScoreName : m_params_base.scores_to_normalize)
	{
		// Find max:
		double maxScore = .0;
		for (const auto& s : score_values)
		{
			const auto it = s.find(sScoreName);
			if (it != s.cend()) mrpt::keep_max(maxScore, it->second);
		}

		// Normalize:
		if (maxScore <= 0)	// all scores=0... let's decide that all are equal,
		// so normalized to "1"
		{
			for (auto& s : score_values)
			{
				auto it = s.find(sScoreName);
				if (it != s.end()) { it->second = 1.0; }
			}
		}
		else if (maxScore > 0 && maxScore != 1.0 /* already normalized! */)
		{
			double K = 1.0 / maxScore;
			for (auto& s : score_values)
			{
				auto it = s.find(sScoreName);
				if (it != s.end()) { it->second *= K; }
			}
		}
	}

	// For each assert, evaluate it (*after* score normalization)
	for (unsigned int mov_idx = 0; mov_idx < movs.size(); ++mov_idx)
	{
		const auto& m = movs[mov_idx];
		// Mark all vars as NaN so we detect uninitialized values:
		for (auto& p : m_expr_vars)
		{
			p.second = std::numeric_limits<double>::quiet_NaN();
		}
		for (const auto& prop : m.props)
		{
			double& var = m_expr_vars[prop.first];
			var = prop.second;
		}

		bool assert_failed = false;
		{
			for (auto& ma : m_movement_assert_exprs)
			{
				const double val = ma.eval();
				if (val == 0)
				{
					assert_failed = true;
					extra_info.log_entries.emplace_back(mrpt::format(
						"[CMultiObjectiveMotionOptimizerBase] "
						"mov_idx=%u ASSERT failed: `%s`",
						mov_idx, ma.get_original_expression().c_str()));
					break;
				}
			}
		}
		if (assert_failed)
		{
			for (auto& e : score_values[mov_idx])
			{
				e.second = .0;
			}
		}
	}  // end mov_idx

	// Run algorithm:
	return impl_decide(movs, extra_info);
}

void CMultiObjectiveMotionOptimizerBase::clear() { m_score_exprs.clear(); }
CMultiObjectiveMotionOptimizerBase::Ptr
	CMultiObjectiveMotionOptimizerBase::Factory(
		const std::string& className) noexcept
{
	try
	{
		mrpt::rtti::registerAllPendingClasses();

		// Factory:
		const mrpt::rtti::TRuntimeClassId* classId =
			mrpt::rtti::findRegisteredClass(className);
		if (!classId) return nullptr;

		return mrpt::ptr_cast<CMultiObjectiveMotionOptimizerBase>::from(
			classId->createObject());
	}
	catch (...)
	{
		return CMultiObjectiveMotionOptimizerBase::Ptr();
	}
}

CMultiObjectiveMotionOptimizerBase::TParamsBase::TParamsBase()
{
	// Default scores:
	formula_score["collision_free_distance"] = "collision_free_distance";
	formula_score["path_index_near_target"] =
		"var dif:=std::abs(target_k-move_k); if (dif>(num_paths/2)) { "
		"dif:=num_paths-dif; }; exp(-std::abs(dif / (num_paths/10.0)));";
	formula_score["euclidean_nearness"] =
		"(ref_dist - dist_eucl_final) / ref_dist";
	formula_score["hysteresis"] = "hysteresis";
	formula_score["clearance"] = "clearance";

	// Default:
	scores_to_normalize.emplace_back("clearance");
}

void CMultiObjectiveMotionOptimizerBase::TParamsBase::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& c, const std::string& s)
{
	// Load: formula_score
	{
		formula_score.clear();
		int idx = 1;
		for (;; idx++)
		{
			const std::string sKeyName = mrpt::format("score%i_name", idx),
							  sKeyValue = mrpt::format("score%i_formula", idx);
			const std::string sName = c.read_string(s, sKeyName, "");
			const std::string sValue = c.read_string(s, sKeyValue, "");

			const bool none = (sName.empty() && sValue.empty());
			const bool both = (!sName.empty() && !sValue.empty());

			if (none && idx == 1)
				THROW_EXCEPTION_FMT(
					"Expect at least a first `%s` and `%s` pair defining one "
					"score in section `[%s]`",
					sKeyName.c_str(), sKeyValue.c_str(), s.c_str());

			if (none) break;

			if (!both)
			{
				THROW_EXCEPTION_FMT(
					"Both `%s` and `%s` must be provided in section `[%s]`",
					sKeyName.c_str(), sKeyValue.c_str(), s.c_str());
			}

			formula_score[sName] = sValue;
		}
	}

	// Load: movement_assert
	{
		movement_assert.clear();
		int idx = 1;
		for (;; idx++)
		{
			const std::string sKey = mrpt::format("movement_assert%i", idx);
			const std::string sValue = c.read_string(s, sKey, "");
			if (sValue.empty()) break;
			movement_assert.push_back(sValue);
		}
	}

	{
		scores_to_normalize.clear();
		std::string sLst = c.read_string(s, "scores_to_normalize", "");
		if (!sLst.empty())
		{ mrpt::system::tokenize(sLst, ", \t", scores_to_normalize); }
	}
}

void CMultiObjectiveMotionOptimizerBase::TParamsBase::saveToConfigFile(
	mrpt::config::CConfigFileBase& c, const std::string& s) const
{
	// Save: formula_score
	const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
			  WV = mrpt::config::MRPT_SAVE_VALUE_PADDING();

	{
		const std::string sComment =
			"\n"
			"# Next follows a list of `score%i_{name,formula}` pairs for "
			"i=1,...,N\n"
			"# Each one defines one of the scores that will be evaluated for "
			"each candidate movement.\n"
			"# Multiobjective optimizers will then use those scores to select "
			"the best candidate, \n"
			"# possibly using more parameters that follow below.\n";
		c.write(s, "dummy", "", WN, WV, sComment);

		int idx = 0;
		for (const auto& p : this->formula_score)
		{
			++idx;
			const std::string sKeyName = mrpt::format("score%i_name", idx),
							  sKeyValue = mrpt::format("score%i_formula", idx);
			c.write(s, sKeyName, p.first, WN, WV);
			c.write(s, sKeyValue, p.second, WN, WV);
		}
	}

	// Load: movement_assert
	{
		const std::string sComment =
			"\n"
			"# Next follows a list of `movement_assert%i` exprtk expressions "
			"for i=1,...,N\n"
			"# defining expressions for conditions that any candidate movement "
			"must fulfill\n"
			"# in order to get through the evaluation process. *All* assert "
			"conditions must be satisfied.\n";
		c.write(s, "dummy2", "", WN, WV, sComment);

		for (unsigned int idx = 0; idx < movement_assert.size(); idx++)
		{
			const std::string sKey = mrpt::format("movement_assert%i", idx + 1);
			c.write(s, sKey, movement_assert[idx], WN, WV);
		}
	}

	{
		std::string sLst;
		for (const auto& sc : scores_to_normalize)
		{
			sLst += sc;
			sLst += std::string(",");
		}
		c.write(s, "scores_to_normalize", sLst);
	}
}
