/* +------------------------------------------------------------------------+
   |                     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 "poses-precomp.h"	// Precompiled headers
//
#include <mrpt/math/wrap2pi.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CRobot2DPoseEstimator.h>
#include <mrpt/system/datetime.h>

#include <iostream>
#include <mutex>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace std;

/* --------------------------------------------------------
				Ctor
   -------------------------------------------------------- */
CRobot2DPoseEstimator::CRobot2DPoseEstimator() { reset(); }
/* --------------------------------------------------------
				Dtor
   -------------------------------------------------------- */
CRobot2DPoseEstimator::~CRobot2DPoseEstimator() = default;
/* --------------------------------------------------------
				reset
   -------------------------------------------------------- */
void CRobot2DPoseEstimator::reset()
{
	std::lock_guard<std::mutex> lock(m_cs);

	m_last_loc_time.reset();
	m_last_odo_time.reset();

	m_last_loc = TPose2D(0, 0, 0);
	m_loc_odo_ref = TPose2D(0, 0, 0);
	m_last_odo = TPose2D(0, 0, 0);

	m_robot_vel_local = TTwist2D(.0, .0, .0);
}

/** Updates the filter so the pose is tracked to the current time */
void CRobot2DPoseEstimator::processUpdateNewPoseLocalization(
	const TPose2D& newPose, Clock::time_point cur_tim)
{
	std::lock_guard<std::mutex> lock(m_cs);

	// Overwrite old localization data:
	m_last_loc_time = cur_tim;
	m_last_loc = newPose;

	// And create interpolated odometry data to work as "reference pose":
	if (m_last_odo_time)
	{
		const double dT = timeDifference(m_last_odo_time.value(), cur_tim);
		extrapolateRobotPose(m_last_odo, m_robot_vel_local, dT, m_loc_odo_ref);
	}
}

/** Updates the filter so the pose is tracked to the current time */
void CRobot2DPoseEstimator::processUpdateNewOdometry(
	const TPose2D& newGlobalOdometry, Clock::time_point cur_tim,
	bool hasVelocities, const mrpt::math::TTwist2D& velLocal)
{
	MRPT_START

	std::lock_guard<std::mutex> lock(m_cs);

	if (m_last_odo_time)
	{
		const double dT = timeDifference(m_last_odo_time.value(), cur_tim);
		if (dT <= 0)
			std::cerr << "[CRobot2DPoseEstimator::processUpdateNewOdometry] "
						 "WARNING: Diff. in timestamps between odometry should "
						 "be >0, and it's "
					  << dT << "\n";
	}

	// First, update velocities:
	if (hasVelocities) { m_robot_vel_local = velLocal; }
	else
	{  // Note: JLBC 23/Nov/2016: I have removed an estimation of velocity from
		// increments of odometry
		// because it was really bad. Just don't make up things: if the user
		// doesn't provide us velocities,
		// we don't use velocities.
		m_robot_vel_local = TTwist2D(.0, .0, .0);
	}

	// And now times & odo:
	m_last_odo_time = cur_tim;
	m_last_odo = newGlobalOdometry;

	MRPT_END
}

bool CRobot2DPoseEstimator::getCurrentEstimate(
	mrpt::math::TPose2D& pose, mrpt::math::TTwist2D& velLocal,
	mrpt::math::TTwist2D& velGlobal, mrpt::Clock::time_point tim_query) const
{
	if (!m_last_odo_time || !m_last_loc_time) return false;

	const double dTimeLoc = timeDifference(m_last_loc_time.value(), tim_query);
	if (dTimeLoc > params.max_localiz_age) return false;

	//  Overall estimate:
	// last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
	const TPose2D p =
		(CPose2D(m_last_loc) + (CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref)))
			.asTPose();

	// Add the extrapolation:
	const double dTimeOdo = timeDifference(m_last_odo_time.value(), tim_query);
	if (dTimeOdo > params.max_odometry_age) return false;

	extrapolateRobotPose(p, m_robot_vel_local, dTimeOdo, pose);

	// Constant speed model:
	velLocal = m_robot_vel_local;
	velGlobal = velLocal;
	velGlobal.rotate(pose.phi);

	return true;
}

/** get the current estimate
 * \return true is the estimate can be trusted. False if the real observed data
 * is too old.
 */
bool CRobot2DPoseEstimator::getLatestRobotPose(TPose2D& pose) const
{
	if (!m_last_odo_time && !m_last_loc_time)
	{
		pose = TPose2D(0, 0, 0);
		return false;
	}

	bool ret_odo;
	if (m_last_odo_time && m_last_loc_time)
		ret_odo = (m_last_odo_time.value() > m_last_loc_time.value());
	else if (m_last_odo_time)
		ret_odo = true;
	else
		ret_odo = false;

	if (ret_odo)
		pose = (CPose2D(m_last_loc) +
				(CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref)))
				   .asTPose();
	else
		pose = m_last_loc;

	return true;
}

// An auxiliary method to extrapolate the pose of a robot located at "p"
//  with velocities (v,w) after a time delay "delta_time".
void CRobot2DPoseEstimator::extrapolateRobotPose(
	const TPose2D& p, const mrpt::math::TTwist2D& velLocal,
	const double delta_time, TPose2D& new_p)
{
	if (velLocal.vx == 0 && velLocal.vy == 0 && velLocal.omega == 0)
	{  // Still
		new_p = p;
	}
	else if (std::abs(velLocal.vy) > 1e-2)
	{  // non-Ackermann-like vehicle: extrapolate as a straight line:
		const TPoint2D dp =
			TPoint2D(delta_time * velLocal.vx, delta_time * velLocal.vy);
		TPoint2D pg;
		CPose2D(p).composePoint(dp, pg);
		new_p.x = pg.x;
		new_p.y = pg.y;
		new_p.phi = p.phi + delta_time * velLocal.omega;
	}
	else
	{
		// vy==0, assume we are in a Ackermann-like steering vehicle: compute an
		// arc:
		const double R = velLocal.vx / velLocal.omega;	// Radius
		const double theta = velLocal.omega * delta_time;  // traversed arc
		const double cc = cos(p.phi);
		const double ss = sin(p.phi);

		const double arc_x = R * sin(theta);
		const double arc_y = R * (1 - cos(theta));

		new_p.x = p.x + cc * arc_x - ss * arc_y;
		new_p.y = p.y + ss * arc_x + cc * arc_y;
		new_p.phi = p.phi + theta;
	}
}

bool CRobot2DPoseEstimator::getLatestRobotPose(CPose2D& pose) const
{
	mrpt::math::TPose2D p;
	bool v = getLatestRobotPose(p);
	if (v)
	{
		pose.x(p.x);
		pose.y(p.y);
		pose.phi(p.phi);
	}
	return v;
}
