/* +------------------------------------------------------------------------+
   |                     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/holonomic/CHolonomicVFF.h>
#include <mrpt/serialization/CArchive.h>

using namespace mrpt;
using namespace std;
using namespace mrpt::nav;

IMPLEMENTS_SERIALIZABLE(CLogFileRecord_VFF, CHolonomicLogFileRecord, mrpt::nav)
IMPLEMENTS_SERIALIZABLE(
	CHolonomicVFF, CAbstractHolonomicReactiveMethod, mrpt::nav)

/*---------------------------------------------------------------
						initialize
  ---------------------------------------------------------------*/
CHolonomicVFF::CHolonomicVFF(const mrpt::config::CConfigFileBase* INI_FILE)
	: CAbstractHolonomicReactiveMethod("CHolonomicVFF")
{
	if (INI_FILE != nullptr) initialize(*INI_FILE);
}

void CHolonomicVFF::initialize(const mrpt::config::CConfigFileBase& INI_FILE)
{
	options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
}
void CHolonomicVFF::saveConfigFile(mrpt::config::CConfigFileBase& c) const
{
	options.saveToConfigFile(c, getConfigFileSectionName());
}

/*---------------------------------------------------------------
						navigate
  ---------------------------------------------------------------*/
void CHolonomicVFF::navigate(const NavInput& ni, NavOutput& no)
{
	const auto ptg = getAssociatedPTG();
	const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;

	// Create a log record for returning data.
	no.logRecord = std::make_shared<CLogFileRecord_VFF>();

	// Forces vector:
	mrpt::math::TPoint2D resultantForce(0, 0), instantaneousForce(0, 0);

	// Obstacles:
	{
		const size_t n = ni.obstacles.size();
		const double inc_ang = 2 * M_PI / n;
		double ang = -M_PI + 0.5 * inc_ang;
		for (size_t i = 0; i < n; i++, ang += inc_ang)
		{
			// Compute force strength:
			// const double mod = exp(- obstacles[i] );
			const double mod = std::min(1e6, 1.0 / ni.obstacles[i]);

			// Add repulsive force:
			instantaneousForce.x = -cos(ang) * mod;
			instantaneousForce.y = -sin(ang) * mod;
			resultantForce += instantaneousForce;
		}
	}

	const double obstcl_weight = 20.0 / ni.obstacles.size();
	resultantForce *= obstcl_weight;

	const double obstacleNearnessFactor =
		std::min(1.0, 6.0 / resultantForce.norm());

	// Target:
	ASSERT_(!ni.targets.empty());
	const auto trg = *ni.targets.rbegin();

	const double ang = atan2(trg.y, trg.x);
	const double mod = options.TARGET_ATTRACTIVE_FORCE;
	resultantForce += mrpt::math::TPoint2D(cos(ang) * mod, sin(ang) * mod);

	// Result:
	no.desiredDirection = (resultantForce.y == 0 && resultantForce.x == 0)
		? 0
		: atan2(resultantForce.y, resultantForce.x);

	// Speed control: Reduction factors
	// ---------------------------------------------
	if (m_enableApproachTargetSlowDown)
	{
		const double targetNearnessFactor = std::min(
			1.0,
			trg.norm() /
				(options.TARGET_SLOW_APPROACHING_DISTANCE / ptg_ref_dist));
		no.desiredSpeed = ni.maxRobotSpeed *
			std::min(obstacleNearnessFactor, targetNearnessFactor);
	}
}

uint8_t CHolonomicVFF::serializeGetVersion() const { return 0; }
void CHolonomicVFF::serializeTo(mrpt::serialization::CArchive& out) const
{
	out << options.TARGET_ATTRACTIVE_FORCE
		<< options.TARGET_SLOW_APPROACHING_DISTANCE;
}
void CHolonomicVFF::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
			in >> options.TARGET_ATTRACTIVE_FORCE >>
				options.TARGET_SLOW_APPROACHING_DISTANCE;
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

uint8_t CLogFileRecord_VFF::serializeGetVersion() const { return 0; }
void CLogFileRecord_VFF::serializeTo(mrpt::serialization::CArchive& out) const
{
}

void CLogFileRecord_VFF::serializeFrom(
	[[maybe_unused]] mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

/*---------------------------------------------------------------
						TOptions
  ---------------------------------------------------------------*/
CHolonomicVFF::TOptions::TOptions()

	= default;

void CHolonomicVFF::TOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& source, const std::string& section)
{
	MRPT_START

	// Load from config text:
	MRPT_LOAD_CONFIG_VAR(
		TARGET_SLOW_APPROACHING_DISTANCE, double, source, section);
	MRPT_LOAD_CONFIG_VAR(TARGET_ATTRACTIVE_FORCE, double, source, section);

	MRPT_END
}

void CHolonomicVFF::TOptions::saveToConfigFile(
	mrpt::config::CConfigFileBase& c, const std::string& s) const
{
	MRPT_START

	MRPT_SAVE_CONFIG_VAR_COMMENT(
		TARGET_SLOW_APPROACHING_DISTANCE, "For stopping gradually");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		TARGET_ATTRACTIVE_FORCE,
		"Dimension-less (may have to be tuned depending on the density of "
		"obstacle sampling)");

	MRPT_END
}
