/* +------------------------------------------------------------------------+
   |                     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/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <fstream>

using namespace mrpt::nav;

IMPLEMENTS_VIRTUAL_SERIALIZABLE(
	CParameterizedTrajectoryGenerator, CSerializable, mrpt::nav)

static std::string OUTPUT_DEBUG_PATH_PREFIX = "./reactivenav.logs";
static PTG_collision_behavior_t COLLISION_BEHAVIOR =
	mrpt::nav::COLL_BEH_BACK_AWAY;

std::string& CParameterizedTrajectoryGenerator::OUTPUT_DEBUG_PATH_PREFIX()
{
	return ::OUTPUT_DEBUG_PATH_PREFIX;
}

PTG_collision_behavior_t&
	CParameterizedTrajectoryGenerator::COLLISION_BEHAVIOR()
{
	return ::COLLISION_BEHAVIOR;
}

void CParameterizedTrajectoryGenerator::loadDefaultParams()
{
	m_alphaValuesCount = 121;
	refDistance = 6.0;
	m_score_priority = 1.0;
	m_clearance_num_points = 5;
	m_clearance_decimated_paths = 15;
}

bool CParameterizedTrajectoryGenerator::supportVelCmdNOP() const
{
	return false;
}
double CParameterizedTrajectoryGenerator::maxTimeInVelCmdNOP(int path_k) const
{
	return .0;
}

void CParameterizedTrajectoryGenerator::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
{
	MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
		num_paths, uint64_t, m_alphaValuesCount, cfg, sSection);
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(refDistance, double, cfg, sSection);
	MRPT_LOAD_HERE_CONFIG_VAR(
		score_priority, double, m_score_priority, cfg, sSection);
	MRPT_LOAD_HERE_CONFIG_VAR(
		clearance_num_points, double, m_clearance_num_points, cfg, sSection);
	MRPT_LOAD_HERE_CONFIG_VAR(
		clearance_decimated_paths, double, m_clearance_decimated_paths, cfg,
		sSection);

	// Ensure a minimum of resolution:
	mrpt::keep_max(
		m_clearance_num_points,
		static_cast<decltype(m_clearance_num_points)>(refDistance / 1.0));

	// Optional params, for debugging only
	MRPT_LOAD_HERE_CONFIG_VAR(
		vxi, double, m_nav_dyn_state.curVelLocal.vx, cfg, sSection);
	MRPT_LOAD_HERE_CONFIG_VAR(
		vyi, double, m_nav_dyn_state.curVelLocal.vy, cfg, sSection);
	MRPT_LOAD_HERE_CONFIG_VAR_DEGREES(
		wi, double, m_nav_dyn_state.curVelLocal.omega, cfg, sSection);

	MRPT_LOAD_HERE_CONFIG_VAR(
		reltrg_x, double, m_nav_dyn_state.relTarget.x, cfg, sSection);
	MRPT_LOAD_HERE_CONFIG_VAR(
		reltrg_y, double, m_nav_dyn_state.relTarget.y, cfg, sSection);
	MRPT_LOAD_HERE_CONFIG_VAR_DEGREES(
		reltrg_phi, double, m_nav_dyn_state.relTarget.phi, cfg, sSection);

	MRPT_LOAD_HERE_CONFIG_VAR(
		target_rel_speed, double, m_nav_dyn_state.targetRelSpeed, cfg,
		sSection);
}
void CParameterizedTrajectoryGenerator::saveToConfigFile(
	mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
{
	MRPT_START
	const int WN = 25, WV = 30;

	cfg.write(
		sSection, "num_paths", m_alphaValuesCount, WN, WV,
		"Number of discrete paths (`resolution`) in the PTG");
	cfg.write(
		sSection, "refDistance", refDistance, WN, WV,
		"Maximum distance (meters) for building trajectories (visibility "
		"range)");
	cfg.write(
		sSection, "score_priority", m_score_priority, WN, WV,
		"When used in path planning, a multiplying factor (default=1.0) for "
		"the scores for this PTG. Assign values <1 to PTGs with low priority.");
	cfg.write(
		sSection, "clearance_num_points", m_clearance_num_points, WN, WV,
		"Number of steps for the piecewise-constant approximation of clearance "
		"(Default=5).");
	cfg.write(
		sSection, "clearance_decimated_paths", m_clearance_decimated_paths, WN,
		WV,
		"Number of decimated paths for estimation of clearance (Default=15).");

	// Optional params, for debugging only
	cfg.write(
		sSection, "vxi", m_nav_dyn_state.curVelLocal.vx, WN, WV,
		"(Only for debugging) Current robot velocity vx [m/s].");
	cfg.write(
		sSection, "vyi", m_nav_dyn_state.curVelLocal.vy, WN, WV,
		"(Only for debugging) Current robot velocity vy [m/s].");
	cfg.write(
		sSection, "wi", mrpt::RAD2DEG(m_nav_dyn_state.curVelLocal.omega), WN,
		WV, "(Only for debugging) Current robot velocity omega [deg/s].");

	cfg.write(
		sSection, "reltrg_x", m_nav_dyn_state.relTarget.x, WN, WV,
		"(Only for debugging) Relative target x [m].");
	cfg.write(
		sSection, "reltrg_y", m_nav_dyn_state.relTarget.y, WN, WV,
		"(Only for debugging) Relative target y [m].");
	cfg.write(
		sSection, "reltrg_phi", mrpt::RAD2DEG(m_nav_dyn_state.relTarget.phi),
		WN, WV, "(Only for debugging) Relative target phi [deg].");

	cfg.write(
		sSection, "target_rel_speed", m_nav_dyn_state.targetRelSpeed, WN, WV,
		"(Only for debugging) Desired relative speed at target [0,1]");

	MRPT_END
}

void CParameterizedTrajectoryGenerator::internal_readFromStream(
	mrpt::serialization::CArchive& in)
{
	this->deinitialize();

	uint8_t version;
	in >> version;
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		case 3:
		case 4:
			in >> refDistance >> m_alphaValuesCount >> m_score_priority;
			if (version >= 1) in >> m_clearance_num_points;
			if (version == 2)
			{
				bool old_use_approx_clearance;
				in >> old_use_approx_clearance;	 // ignored in v>=3
			}
			if (version >= 4) { in >> m_clearance_decimated_paths; }
			else
			{
				m_clearance_decimated_paths = m_alphaValuesCount;
			}
			break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

void CParameterizedTrajectoryGenerator::internal_writeToStream(
	mrpt::serialization::CArchive& out) const
{
	const uint8_t version = 4;
	out << version;

	out << refDistance << m_alphaValuesCount << m_score_priority
		<< m_clearance_num_points /* v1 */;
	out << m_clearance_decimated_paths /* v4*/;
}

double CParameterizedTrajectoryGenerator::index2alpha(
	uint16_t k, const unsigned int num_paths)
{
	ASSERT_LT_(k, num_paths);
	return M_PI * (-1.0 + 2.0 * (k + 0.5) / num_paths);
}

double CParameterizedTrajectoryGenerator::index2alpha(uint16_t k) const
{
	return index2alpha(k, m_alphaValuesCount);
}

uint16_t CParameterizedTrajectoryGenerator::alpha2index(
	double alpha, const unsigned int num_paths)
{
	mrpt::math::wrapToPi(alpha);
	int k = mrpt::round(0.5 * (num_paths * (1.0 + alpha / M_PI) - 1.0));
	if (k < 0) k = 0;
	if (k >= static_cast<int>(num_paths)) k = num_paths - 1;
	return (uint16_t)k;
}

uint16_t CParameterizedTrajectoryGenerator::alpha2index(double alpha) const
{
	return alpha2index(alpha, m_alphaValuesCount);
}

void CParameterizedTrajectoryGenerator::renderPathAsSimpleLine(
	const uint16_t k, mrpt::opengl::CSetOfLines& gl_obj,
	const double decimate_distance, const double max_path_distance) const
{
	const size_t nPointsInPath = getPathStepCount(k);

	bool first = true;
	// Decimate trajectories: we don't need centimeter resolution!
	double last_added_dist = 0.0;
	for (size_t n = 0; n < nPointsInPath; n++)
	{
		const double d = this->getPathDist(
			k, n);	// distance thru path "k" until timestep "n"

		// Draw the TP only until we reach the target of the "motion" segment:
		if (max_path_distance >= 0.0 && d >= max_path_distance) break;

		if (d < last_added_dist + decimate_distance && n != 0)
			continue;  // skip: decimation

		last_added_dist = d;

		const mrpt::math::TPose2D p = this->getPathPose(k, n);

		if (first)
		{
			first = false;
			gl_obj.appendLine(0, 0, 0, p.x, p.y, 0);
		}
		else
			gl_obj.appendLineStrip(p.x, p.y, 0);
	}
}

void CParameterizedTrajectoryGenerator::initTPObstacles(
	std::vector<double>& TP_Obstacles) const
{
	TP_Obstacles.resize(m_alphaValuesCount);
	for (size_t k = 0; k < m_alphaValuesCount; k++)
		initTPObstacleSingle(k, TP_Obstacles[k]);
}
void CParameterizedTrajectoryGenerator::initTPObstacleSingle(
	[[maybe_unused]] uint16_t k, double& TP_Obstacle_k) const
{
	// Always initialize to the maximum possible free space:
	TP_Obstacle_k = refDistance;
}

bool CParameterizedTrajectoryGenerator::debugDumpInFiles(
	const std::string& ptg_name) const
{
	using namespace mrpt::system;
	using namespace std;

	const char* sPath =
		CParameterizedTrajectoryGenerator::OUTPUT_DEBUG_PATH_PREFIX().c_str();

	mrpt::system::createDirectory(sPath);
	mrpt::system::createDirectory(mrpt::format("%s/PTGs", sPath));

	const string sFilTxt_x =
		mrpt::format("%s/PTGs/PTG%s_x.txt", sPath, ptg_name.c_str());
	const string sFilTxt_y =
		mrpt::format("%s/PTGs/PTG%s_y.txt", sPath, ptg_name.c_str());
	const string sFilTxt_phi =
		mrpt::format("%s/PTGs/PTG%s_phi.txt", sPath, ptg_name.c_str());
	const string sFilTxt_t =
		mrpt::format("%s/PTGs/PTG%s_t.txt", sPath, ptg_name.c_str());
	const string sFilTxt_d =
		mrpt::format("%s/PTGs/PTG%s_d.txt", sPath, ptg_name.c_str());

	ofstream fx(sFilTxt_x.c_str());
	if (!fx.is_open()) return false;
	ofstream fy(sFilTxt_y.c_str());
	if (!fy.is_open()) return false;
	ofstream fp(sFilTxt_phi.c_str());
	if (!fp.is_open()) return false;
	ofstream fd(sFilTxt_d.c_str());
	if (!fd.is_open()) return false;

	const size_t nPaths = getAlphaValuesCount();

	// Text version:
	fx << "% PTG data file for 'x'. Each row is the trajectory for a different "
		  "'alpha' parameter value."
	   << endl;
	fy << "% PTG data file for 'y'. Each row is the trajectory for a different "
		  "'alpha' parameter value."
	   << endl;
	fp << "% PTG data file for 'phi'. Each row is the trajectory for a "
		  "different 'alpha' parameter value."
	   << endl;
	fd << "% PTG data file for 'd'. Each row is the trajectory for a different "
		  "'alpha' parameter value."
	   << endl;

	vector<size_t> path_length(nPaths);
	for (size_t k = 0; k < nPaths; k++)
		path_length[k] = getPathStepCount(k);

	size_t maxPoints = 0;
	for (size_t k = 0; k < nPaths; k++)
		maxPoints = max(maxPoints, path_length[k]);

	for (size_t k = 0; k < nPaths; k++)
	{
		for (size_t n = 0; n < maxPoints; n++)
		{
			const size_t nn = std::min(n, path_length[k] - 1);
			const mrpt::math::TPose2D p = this->getPathPose(k, nn);
			fx << p.x << " ";
			fy << p.y << " ";
			fp << p.phi << " ";
			fd << this->getPathDist(k, nn) << " ";
		}
		fx << endl;
		fy << endl;
		fp << endl;
		fd << endl;
	}

	return true;
}

bool CParameterizedTrajectoryGenerator::isInitialized() const
{
	return m_is_initialized;
}

void CParameterizedTrajectoryGenerator::updateNavDynamicState(
	const CParameterizedTrajectoryGenerator::TNavDynamicState& newState,
	const bool force_update)
{
	// Make sure there is a real difference: notifying a PTG that a
	// condition changed may imply a significant computational cost if paths
	// need to be re-evaluated on the fly, etc. so the cost of the
	// comparison here is totally worth:
	if (force_update || m_nav_dyn_state != newState)
	{
		ASSERT_(
			newState.targetRelSpeed >= .0 &&
			newState.targetRelSpeed <= 1.0);  // sanity check
		m_nav_dyn_state = newState;

		// 1st) Build PTG paths without counting for target slow-down:
		m_nav_dyn_state_target_k = INVALID_PTG_PATH_INDEX;

		this->onNewNavDynamicState();

		// 2nd) Save the special path for slow-down:
		if (this->supportSpeedAtTarget())
		{
			int target_k = -1;
			double target_norm_d;
			// bool is_exact = // JLB removed this constraint for being too
			// restrictive.
			this->inverseMap_WS2TP(
				m_nav_dyn_state.relTarget.x, m_nav_dyn_state.relTarget.y,
				target_k, target_norm_d, 1.0 /*large tolerance*/);
			if (target_norm_d > 0.01 && target_norm_d < 0.99 && target_k >= 0 &&
				target_k < m_alphaValuesCount)
			{
				m_nav_dyn_state_target_k = target_k;
				this->onNewNavDynamicState();  // Recalc
			}
		}
	}
}

void CParameterizedTrajectoryGenerator::initialize(
	const std::string& cacheFilename, const bool verbose)
{
	if (m_is_initialized) return;

	const std::string sCache = !cacheFilename.empty() ? cacheFilename
													  : std::string("cache_") +
			mrpt::system::fileNameStripInvalidChars(getDescription()) +
			std::string(".bin.gz");

	this->internal_initialize(sCache, verbose);
	m_is_initialized = true;
}
void CParameterizedTrajectoryGenerator::deinitialize()
{
	if (!m_is_initialized) return;
	this->internal_deinitialize();
	m_is_initialized = false;
}

void CParameterizedTrajectoryGenerator::internal_TPObsDistancePostprocess(
	const double ox, const double oy, const double new_tp_obs_dist,
	double& inout_tp_obs) const
{
	const bool is_obs_inside_robot_shape = isPointInsideRobotShape(ox, oy);
	if (!is_obs_inside_robot_shape)
	{
		mrpt::keep_min(inout_tp_obs, new_tp_obs_dist);
		return;
	}

	// Handle the special case of obstacles *inside* the robot at the begining
	// of the PTG path:
	switch (COLLISION_BEHAVIOR())
	{
		case COLL_BEH_STOP: inout_tp_obs = .0; break;

		case COLL_BEH_BACK_AWAY:
		{
			if (new_tp_obs_dist < getMaxRobotRadius())
			{
				// This means that we are getting apart of the obstacle:
				// ignore it to allow the robot to get off the near-collision:
				// Don't change inout_tp_obs.
				return;
			}
			else
			{
				// This means we are already in collision and trying to get even
				// closer
				// to the obstacle: totally disprove this action:
				inout_tp_obs = .0;
			}
		}
		break;

		default:
			THROW_EXCEPTION("Obstacle postprocessing enum not implemented!");
	}
}

void mrpt::nav::CParameterizedTrajectoryGenerator::initClearanceDiagram(
	ClearanceDiagram& cd) const
{
	cd.resize(m_alphaValuesCount, m_clearance_decimated_paths);
	for (unsigned int decim_k = 0; decim_k < m_clearance_decimated_paths;
		 decim_k++)
	{
		const auto real_k = cd.decimated_k_to_real_k(decim_k);
		const size_t numPathSteps = getPathStepCount(real_k);
		const double numStepsPerIncr =
			(numPathSteps - 1.0) / double(m_clearance_num_points);

		auto& cl_path = cd.get_path_clearance_decimated(decim_k);
		for (double step_pointer_dbl = 0.0; step_pointer_dbl < numPathSteps;
			 step_pointer_dbl += numStepsPerIncr)
		{
			const size_t step = mrpt::round(step_pointer_dbl);
			const double dist_over_path = this->getPathDist(real_k, step);
			cl_path[dist_over_path] = 1.0;	// create entry in map<>
		}
	}
}

void CParameterizedTrajectoryGenerator::updateClearance(
	const double ox, const double oy, ClearanceDiagram& cd) const
{
	// Initialize CD on first call:
	ASSERT_(cd.get_actual_num_paths() == m_alphaValuesCount);
	ASSERT_(m_clearance_num_points > 0 && m_clearance_num_points < 10000);

	// evaluate in derived-class: this function also keeps the minimum
	// automatically.
	for (uint16_t decim_k = 0; decim_k < cd.get_decimated_num_paths();
		 decim_k++)
	{
		const auto real_k = cd.decimated_k_to_real_k(decim_k);
		this->evalClearanceSingleObstacle(
			ox, oy, real_k, cd.get_path_clearance_decimated(decim_k));
	}
}

void CParameterizedTrajectoryGenerator::updateClearancePost(
	ClearanceDiagram& cd, const std::vector<double>& TP_obstacles) const
{
	// Used only when in approx mode (Removed 30/01/2017)
}

void CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle(
	const double ox, const double oy, const uint16_t k,
	ClearanceDiagram::dist2clearance_t& inout_realdist2clearance,
	bool treat_as_obstacle) const
{
	bool had_collision = false;

	const size_t numPathSteps = getPathStepCount(k);
	// We don't have steps enough (?). Just ignore clearance for this short
	// path in this "k" direction:
	if (numPathSteps <= inout_realdist2clearance.size())
	{
		std::cerr << "[CParameterizedTrajectoryGenerator::"
					 "evalClearanceSingleObstacle] Warning: k="
				  << k << " numPathSteps is only=" << numPathSteps
				  << " num of clearance steps="
				  << inout_realdist2clearance.size();
		return;
	}

	const double numStepsPerIncr =
		(numPathSteps - 1.0) / (inout_realdist2clearance.size());

	double step_pointer_dbl = 0.0;
	const mrpt::math::TPoint2D og(ox, oy);	// obstacle in "global" frame
	mrpt::math::TPoint2D ol;  // obstacle in robot frame

	for (auto& e : inout_realdist2clearance)
	{
		step_pointer_dbl += numStepsPerIncr;
		const size_t step = mrpt::round(step_pointer_dbl);
		const double dist_over_path = e.first;
		double& inout_clearance = e.second;

		if (had_collision)
		{
			// We found a collision in a previous step along this "k" path, so
			// it does not make sense to evaluate the clearance of a pose which
			// is not reachable:
			inout_clearance = .0;
			continue;
		}

		const mrpt::math::TPose2D pose = getPathPose(k, step);

		// obstacle to robot clearance:
		ol = pose.inverseComposePoint(og);
		const double this_clearance = treat_as_obstacle
			? this->evalClearanceToRobotShape(ol.x, ol.y)
			: ol.norm();
		if (this_clearance <= .0 && treat_as_obstacle &&
			(dist_over_path > 0.5 ||
			 std::abs(mrpt::math::angDistance(
				 std::atan2(oy, ox), index2alpha(k))) < mrpt::DEG2RAD(45.0)))
		{
			// Collision:
			had_collision = true;
			inout_clearance = .0;
		}
		else
		{
			// The obstacle is not a direct collision.
			const double this_clearance_norm =
				this_clearance / this->refDistance;

			// Update minimum in output structure
			mrpt::keep_min(inout_clearance, this_clearance_norm);
		}
	}
}

mrpt::math::TTwist2D CParameterizedTrajectoryGenerator::getPathTwist(
	uint16_t k, uint32_t step) const
{
	if (step > 0)
	{
		// Numerical estimate of "global" (wrt current, start of path) direction
		// of motion:
		const auto curPose = getPathPose(k, step);
		const auto prevPose = getPathPose(k, step - 1);
		const double dt = getPathStepDuration();
		ASSERT_GT_(dt, .0);

		auto vel = mrpt::math::TTwist2D(
			curPose.x - prevPose.x, curPose.y - prevPose.y,
			mrpt::math::angDistance(prevPose.phi, curPose.phi));
		vel *= (1.0 / dt);
		return vel;
	}
	else
	{
		// Initial velocity:
		return m_nav_dyn_state.curVelLocal;
	}
}

bool CParameterizedTrajectoryGenerator::TNavDynamicState::operator==(
	const TNavDynamicState& o) const
{
	return (curVelLocal == o.curVelLocal) && (relTarget == o.relTarget) &&
		(targetRelSpeed == o.targetRelSpeed);
}

void mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::
	writeToStream(mrpt::serialization::CArchive& out) const
{
	const uint8_t version = 0;
	out << version;
	// Data:
	out << curVelLocal << relTarget << targetRelSpeed;
}

void mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::
	readFromStream(mrpt::serialization::CArchive& in)
{
	uint8_t version;
	in >> version;
	switch (version)
	{
		case 0: in >> curVelLocal >> relTarget >> targetRelSpeed; break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}
