/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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/core/lock_helper.h>
#include <mrpt/math/TSegment2D.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/nav/reactive/CWaypointsNavigator.h>
#include <mrpt/poses/CPose2D.h>

using namespace mrpt::nav;
using namespace std;

std::string CWaypointsNavigator::TNavigationParamsWaypoints::getAsText() const
{
	std::string s = TNavigationParams::getAsText();
	if (!multiple_targets.empty())
	{
		s += "multiple_targets:\n";
		int i = 0;
		for (const auto& e : multiple_targets)
		{
			s += mrpt::format("target[%i]:\n", i++);
			s += e.getAsText();
		}
	}
	return s;
}

bool CWaypointsNavigator::TNavigationParamsWaypoints::isEqual(
	const CAbstractNavigator::TNavigationParamsBase& rhs) const
{
	auto o =
		dynamic_cast<const CWaypointsNavigator::TNavigationParamsWaypoints*>(
			&rhs);
	return o != nullptr &&
		CAbstractNavigator::TNavigationParams::isEqual(rhs) &&
		multiple_targets == o->multiple_targets;
}

CWaypointsNavigator::CWaypointsNavigator(CRobot2NavInterface& robot_if)
	: CAbstractNavigator(robot_if)
{
	m_last_alignment_cmd = mrpt::system::now();
}

CWaypointsNavigator::~CWaypointsNavigator() = default;
void CWaypointsNavigator::onNavigateCommandReceived()
{
	CAbstractNavigator::onNavigateCommandReceived();

	std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);

	m_was_aligning = false;

	// This initializes all fields to initial values:
	m_waypoint_nav_status = TWaypointStatusSequence();
}

void CWaypointsNavigator::navigateWaypoints(
	const TWaypointSequence& nav_request)
{
	MRPT_START

	this->onNavigateCommandReceived();

	std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);

	const size_t N = nav_request.waypoints.size();
	ASSERTMSG_(N > 0, "List of waypoints is empty!");

	m_waypoint_nav_status.waypoints.resize(N);
	// Copy waypoints fields data, leave status fields to defaults:
	for (size_t i = 0; i < N; i++)
	{
		ASSERT_(nav_request.waypoints[i].isValid());
		m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
	}
	m_waypoint_nav_status.timestamp_nav_started = mrpt::system::now();

	m_waypoint_nav_status.waypoint_index_current_goal = -1;	 // Not started yet.

	// The main loop navigationStep() will iterate over waypoints and send them
	// to navigate()
	MRPT_END
}

void CWaypointsNavigator::getWaypointNavStatus(
	TWaypointStatusSequence& out_nav_status) const
{
	// Make sure the data structure is not under modification:
	auto lck = mrpt::lockHelper(m_nav_waypoints_cs);

	out_nav_status = m_waypoint_nav_status;
}

/** \callergraph */
void CWaypointsNavigator::cancel()
{
	{
		std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
		m_waypoint_nav_status = TWaypointStatusSequence();
	}
	CAbstractNavigator::cancel();
}

/** \callergraph */
void CWaypointsNavigator::waypoints_navigationStep()
{
	MRPT_START

	using mrpt::square;

	// --------------------------------------
	//     Waypoint navigation algorithm
	// --------------------------------------
	m_is_aligning =
		false;	// the robot is aligning into a waypoint with a desired heading

	{
		mrpt::system::CTimeLoggerEntry tle(
			m_timlog_delays, "CWaypointsNavigator::navigationStep()");
		std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);

		// shortcut to save typing
		TWaypointStatusSequence& wps = m_waypoint_nav_status;

		if (wps.waypoints.empty() || wps.final_goal_reached)
		{
			// No nav request is pending or it was canceled
		}
		else
		{
			// 0) Get current robot pose:
			CAbstractNavigator::updateCurrentPoseAndSpeeds();

			// 1) default policy: go thru WPs one by one
			const int prev_wp_index = wps.waypoint_index_current_goal;

			mrpt::math::TSegment2D robot_move_seg;
			robot_move_seg.point1.x = m_curPoseVel.pose.x;
			robot_move_seg.point1.y = m_curPoseVel.pose.y;
			if (wps.last_robot_pose.x == TWaypoint::INVALID_NUM)
			{
				robot_move_seg.point2 = robot_move_seg.point1;
			}
			else
			{
				robot_move_seg.point2.x = wps.last_robot_pose.x;
				robot_move_seg.point2.y = wps.last_robot_pose.y;
			}
			wps.last_robot_pose = m_curPoseVel.pose;  // save for next iters

			decltype(m_pending_events) new_events;

			if (wps.waypoint_index_current_goal >= 0)
			{
				auto& wp = wps.waypoints[wps.waypoint_index_current_goal];
				const double dist2target = robot_move_seg.distance(wp.target);
				if (dist2target < wp.allowed_distance ||
					m_was_aligning /* we were already aligning at a WP */)
				{
					bool consider_wp_reached = false;
					if (!wp.target_heading.has_value())
					{
						// Any heading is ok:
						consider_wp_reached = true;
					}
					else
					{
						// Handle pure-rotation robot interface to honor
						// target_heading
						const double ang_err = mrpt::math::angDistance(
							m_curPoseVel.pose.phi, wp.target_heading.value());
						const double tim_since_last_align =
							mrpt::system::timeDifference(
								m_last_alignment_cmd, mrpt::system::now());
						const double ALIGN_WAIT_TIME = 1.5;	 // seconds

						if (std::abs(ang_err) <=
								params_waypoints_navigator
									.waypoint_angle_tolerance &&
							/* give some time for the alignment (if supported in
							   this robot) to finish)*/
							tim_since_last_align > ALIGN_WAIT_TIME)
						{
							consider_wp_reached = true;
						}
						else
						{
							m_is_aligning = true;

							if (!m_was_aligning)
							{
								// 1st time we are aligning:
								// Send vel_cmd to the robot:
								mrpt::kinematics::CVehicleVelCmd::Ptr
									align_cmd = m_robot.getAlignCmd(ang_err);

								MRPT_LOG_INFO_FMT(
									"[CWaypointsNavigator::navigationStep] "
									"Trying to align to heading: %.02f deg. "
									"Relative heading: %.02f deg. "
									"With motion cmd: %s",
									mrpt::RAD2DEG(*wp.target_heading),
									mrpt::RAD2DEG(ang_err),
									align_cmd ? align_cmd->asString().c_str()
											  : "nullptr (operation not "
												"supported by this robot)");

								this->stop(false /*not emergency*/);  // In any
								// case,
								// do a
								// "stop"
								if (align_cmd)
								{
									this->changeSpeeds(*align_cmd);
								}
								else
								{
									consider_wp_reached =
										true;  // this robot does not support
									// "in place" alignment
								}
							}
							else
							{
								MRPT_LOG_THROTTLE_INFO_FMT(
									0.5,
									"[CWaypointsNavigator::navigationStep] "
									"Waiting for the robot to get aligned: "
									"current_heading=%.02f deg "
									"target_heading=%.02f deg",
									mrpt::RAD2DEG(m_curPoseVel.pose.phi),
									mrpt::RAD2DEG(*wp.target_heading));
							}
						}
					}

					if (consider_wp_reached)
					{
						MRPT_LOG_DEBUG_STREAM(
							"[CWaypointsNavigator::navigationStep] Waypoint "
							<< (wps.waypoint_index_current_goal + 1) << "/"
							<< wps.waypoints.size()
							<< " reached."
							   " segment-to-target dist: "
							<< dist2target
							<< ", allowed_dist: " << wp.allowed_distance);

						wp.reached = true;
						wp.skipped = false;
						wp.timestamp_reach = mrpt::system::now();

						new_events.emplace_back(std::bind(
							&CRobot2NavInterface::sendWaypointReachedEvent,
							std::ref(m_robot), wps.waypoint_index_current_goal,
							true /*reason: really reached*/));

						// Was this the final goal??
						if (wps.waypoint_index_current_goal <
							int(wps.waypoints.size() - 1))
						{
							wps.waypoint_index_current_goal++;
						}
						else
						{
							wps.final_goal_reached = true;
							// Make sure the end-navigation event is issued,
							// navigation state switches to IDLE, etc:
							this->performNavigationStepNavigating(false);
						}
					}
				}
			}

			// 2) More advanced policy: if available, use children class methods
			// to decide
			//     which is the best candidate for the next waypoint, if we can
			//     skip current one:
			if (!wps.final_goal_reached &&
				wps.waypoint_index_current_goal >= 0 &&
				wps.waypoints[wps.waypoint_index_current_goal].allow_skip)
			{
				const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose);
				int most_advanced_wp = wps.waypoint_index_current_goal;
				const int most_advanced_wp_at_begin = most_advanced_wp;

				for (int idx = wps.waypoint_index_current_goal;
					 idx < (int)wps.waypoints.size(); idx++)
				{
					if (idx < 0) continue;
					if (wps.waypoints[idx].reached) continue;

					// Is it reachable?
					mrpt::math::TPoint2D wp_local_wrt_robot;
					robot_pose.inverseComposePoint(
						wps.waypoints[idx].target, wp_local_wrt_robot);

					if (params_waypoints_navigator
								.max_distance_to_allow_skip_waypoint > 0 &&
						wp_local_wrt_robot.norm() >
							params_waypoints_navigator
								.max_distance_to_allow_skip_waypoint)
						continue;  // Skip this one, it is too far away

					const bool is_reachable =
						this->impl_waypoint_is_reachable(wp_local_wrt_robot);

					if (is_reachable)
					{
						// Robustness filter: only skip to a future waypoint if
						// it is seen as "reachable" during
						// a given number of timesteps:
						if (++wps.waypoints[idx].counter_seen_reachable >
							params_waypoints_navigator
								.min_timesteps_confirm_skip_waypoints)
						{
							most_advanced_wp = idx;
						}
					}

					// Is allowed to skip it?
					if (!wps.waypoints[idx].allow_skip)
						break;	// Do not keep trying, since we are now allowed
					// to skip this one.
				}

				if (most_advanced_wp >= 0)
				{
					wps.waypoint_index_current_goal = most_advanced_wp;
					for (int k = most_advanced_wp_at_begin;
						 k < most_advanced_wp; k++)
					{
						auto& wp = wps.waypoints[k];
						wp.reached = true;
						wp.skipped = true;
						wp.timestamp_reach = mrpt::system::now();

						new_events.emplace_back(std::bind(
							&CRobot2NavInterface::sendWaypointReachedEvent,
							std::ref(m_robot), k, false /*reason: skipped*/));
					}
				}
			}

			// Insert at the beginning, for these events to be dispatched
			// *before* any "end of nav" event:
			m_pending_events.insert(
				m_pending_events.begin(), new_events.begin(), new_events.end());

			// Still not started and no better guess? Start with the first
			// waypoint:
			if (wps.waypoint_index_current_goal < 0)
				wps.waypoint_index_current_goal = 0;

			// 3) Should I request a new (single target) navigation command?
			//    Only if the temporary goal changed:
			if (wps.waypoint_index_current_goal >= 0 &&
				prev_wp_index != wps.waypoint_index_current_goal)
			{
				ASSERT_(
					wps.waypoint_index_current_goal <
					int(wps.waypoints.size()));
				ASSERT_(params_waypoints_navigator.multitarget_look_ahead >= 0);

				// Notify we have a new "current waypoint"
				// Push back so it's dispatched *after* the wp reached events:
				m_pending_events.emplace_back(std::bind(
					&CRobot2NavInterface::sendNewWaypointTargetEvent,
					std::ref(m_robot), wps.waypoint_index_current_goal));

				// Send the current targets + "multitarget_look_ahead"
				// additional ones to help the local planner.
				CWaypointsNavigator::TNavigationParamsWaypoints nav_cmd;

				// Check skippable flag while traversing from current wp forward
				// "multitarget_look_ahead" steps:
				int wp_last_idx = wps.waypoint_index_current_goal;
				for (int nstep = 0;
					 wp_last_idx < int(wps.waypoints.size()) - 1 &&
					 nstep < params_waypoints_navigator.multitarget_look_ahead;
					 ++nstep)
				{
					if (!m_waypoint_nav_status.waypoints[wp_last_idx]
							 .allow_skip)
						break;
					wp_last_idx++;
				}

				for (int wp_idx = wps.waypoint_index_current_goal;
					 wp_idx <= wp_last_idx; wp_idx++)
				{
					TWaypointStatus& wp = wps.waypoints[wp_idx];
					const bool is_final_wp =
						((wp_idx + 1) == int(wps.waypoints.size()));

					CAbstractNavigator::TargetInfo ti;

					ti.target_coords.x = wp.target.x;
					ti.target_coords.y = wp.target.y;
					ti.target_coords.phi =
						(wp.target_heading.has_value() ? *wp.target_heading
													   : .0);
					ti.target_frame_id = wp.target_frame_id;
					ti.targetAllowedDistance = wp.allowed_distance;
					ti.targetIsRelative = false;
					ti.targetIsIntermediaryWaypoint = !is_final_wp;
					ti.targetDesiredRelSpeed = wp.speed_ratio;

					// For backwards compat. with single-target code, write
					// single target info too for the first, next, waypoint:
					if (wp_idx == wps.waypoint_index_current_goal)
					{
						nav_cmd.target = ti;
					}
					// Append to list of targets:
					nav_cmd.multiple_targets.emplace_back(ti);
				}
				this->processNavigateCommand(&nav_cmd);

				MRPT_LOG_DEBUG_STREAM(
					"[CWaypointsNavigator::navigationStep] Active waypoint "
					"changed. Current status:\n"
					<< this->getWaypointNavStatus().getAsText());
			}
		}
	}

	// Note: navigationStep() called *after* waypoints part to get
	// end-of-navigation events *after*
	//       waypoints-related events:

	m_was_aligning = m_is_aligning;	 // Let the next timestep know about this

	MRPT_END
}

void CWaypointsNavigator::navigationStep()
{
	MRPT_START
	// the robot is aligning into a waypoint with a desired heading
	m_is_aligning = false;

	mrpt::system::CTimeLoggerEntry tle(
		m_navProfiler, "CWaypointsNavigator::navigationStep()");

	if (m_navigationState != SUSPENDED) { waypoints_navigationStep(); }

	// Call base navigation step to execute one-single waypoint navigation, as
	// usual:
	if (!m_is_aligning)
	{
		CAbstractNavigator::navigationStep();  // This internally locks
		// "m_nav_cs"
	}
	else
	{
		// otherwise, at least, process pending events:
		CAbstractNavigator::dispatchPendingNavEvents();
	}

	MRPT_END
}

/** \callergraph */
void CWaypointsNavigator::onStartNewNavigation() {}
/** \callergraph */
bool CWaypointsNavigator::isRelativePointReachable(
	const mrpt::math::TPoint2D& wp_local_wrt_robot) const
{
	return impl_waypoint_is_reachable(wp_local_wrt_robot);
}

void CWaypointsNavigator::loadConfigFile(const mrpt::config::CConfigFileBase& c)
{
	MRPT_START

	params_waypoints_navigator.loadFromConfigFile(c, "CWaypointsNavigator");
	CAbstractNavigator::loadConfigFile(c);

	MRPT_END
}

void CWaypointsNavigator::saveConfigFile(mrpt::config::CConfigFileBase& c) const
{
	CAbstractNavigator::saveConfigFile(c);
	params_waypoints_navigator.saveToConfigFile(c, "CWaypointsNavigator");
}

void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::
	loadFromConfigFile(
		const mrpt::config::CConfigFileBase& c, const std::string& s)
{
	MRPT_LOAD_CONFIG_VAR(max_distance_to_allow_skip_waypoint, double, c, s);
	MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
	MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
	MRPT_LOAD_CONFIG_VAR(multitarget_look_ahead, int, c, s);
}

void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::
	saveToConfigFile(
		mrpt::config::CConfigFileBase& c, const std::string& s) const
{
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		max_distance_to_allow_skip_waypoint,
		"Max distance to `foresee` waypoints [meters]. (<0: unlimited)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		min_timesteps_confirm_skip_waypoints,
		"Min timesteps a `future` waypoint must be seen as reachable to become "
		"the active one.");
	MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT(
		"waypoint_angle_tolerance", waypoint_angle_tolerance,
		"Angular error tolerance for waypoints with an assigned heading [deg] "
		"(Default: 5 deg)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		multitarget_look_ahead,
		">=0 number of waypoints to forward to the underlying navigation "
		"engine, to ease obstacles avoidance when a waypoint is blocked "
		"(Default=0 : none)");
}

CWaypointsNavigator::TWaypointsNavigatorParams::TWaypointsNavigatorParams()
	: waypoint_angle_tolerance(mrpt::DEG2RAD(5.0))

{
}

/** \callergraph */
bool CWaypointsNavigator::checkHasReachedTarget(const double targetDist) const
{
	bool ret;
	const TWaypointStatus* wp = nullptr;
	const auto& wps = m_waypoint_nav_status;
	if (m_navigationParams->target.targetIsIntermediaryWaypoint)
	{
		ret = false;
	}
	else if (wps.timestamp_nav_started != INVALID_TIMESTAMP)
	{
		wp = (!wps.waypoints.empty() && wps.waypoint_index_current_goal >= 0 &&
			  wps.waypoint_index_current_goal < (int)wps.waypoints.size())
			? &wps.waypoints[wps.waypoint_index_current_goal]
			: nullptr;
		ret =
			(wp == nullptr &&
			 targetDist <= m_navigationParams->target.targetAllowedDistance) ||
			(wp->reached);
	}
	else
	{
		ret = (targetDist <= m_navigationParams->target.targetAllowedDistance);
	}
	MRPT_LOG_DEBUG_STREAM(
		"CWaypointsNavigator::checkHasReachedTarget() called "
		"with targetDist="
		<< targetDist << " return=" << ret
		<< " waypoint: " << (wp == nullptr ? std::string("") : wp->getAsText())
		<< "wps.timestamp_nav_started="
		<< (wps.timestamp_nav_started == INVALID_TIMESTAMP
				? "INVALID_TIMESTAMP"
				: mrpt::system::dateTimeLocalToString(
					  wps.timestamp_nav_started)));
	return ret;
}
