/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/kinematics/CVehicleVelCmd.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TTwist2D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/system/datetime.h>

namespace mrpt::kinematics
{
/** This class can be used to simulate the kinematics and dynamics of a
 * differential driven planar mobile robot, including odometry errors and
 * dynamics limitations.
 *  Main API methods are:
 *  - movementCommand: Call this for send a command to the robot. This comamnd
 * will be
 *    delayed and passed throught a first order low-pass filter to simulate
 *    robot dynamics.
 *  - simulateInterval: Call this for run the simulator for the desired time
 * period.
 *
 * \ingroup mrpt_kinematics_grp
 */
class CVehicleSimulVirtualBase
{
   public:
	CVehicleSimulVirtualBase();
	virtual ~CVehicleSimulVirtualBase();

	/** @name Kinematic simulation and control interface
	 * @{ */
	/** Runs the simulator during "dt" seconds. It will be split into periods of
	 * "m_firmware_control_period". */
	void simulateOneTimeStep(const double dt);

	/** Returns the instantaneous, ground truth pose in world coordinates */
	const mrpt::math::TPose2D& getCurrentGTPose() const { return m_GT_pose; }
	/** Brute-force move robot to target coordinates ("teleport") */
	void setCurrentGTPose(const mrpt::math::TPose2D& pose);

	/** Returns the current pose according to (noisy) odometry \sa
	 * setOdometryErrors */
	const mrpt::math::TPose2D& getCurrentOdometricPose() const
	{
		return m_odometry;
	}
	/** Brute-force overwrite robot odometry  */
	template <typename T>
	void setCurrentOdometricPose(const T& pose)
	{
		m_odometry = mrpt::math::TPose2D(pose);
	}

	/** Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in
	 * world coordinates */
	const mrpt::math::TTwist2D& getCurrentGTVel() const { return m_GT_vel; }
	/** Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in
	 * the robot local frame */
	mrpt::math::TTwist2D getCurrentGTVelLocal() const;

	/** Returns the instantaneous, odometric velocity vector (vx,vy,omega) in
	 * world coordinates */
	const mrpt::math::TTwist2D& getCurrentOdometricVel() const
	{
		return m_odometric_vel;
	}
	/** Returns the instantaneous, odometric velocity vector (vx,vy,omega) in
	 * the robot local frame */
	mrpt::math::TTwist2D getCurrentOdometricVelLocal() const;

	/** Get the current simulation time */
	double getTime() const { return m_time; }
	/** Sends a velocity command to the robot. The number of components and
	 * their meaning depends
	 * on the vehicle-kinematics derived class */
	virtual void sendVelCmd(const CVehicleVelCmd& cmd_vel) = 0;
	/** Gets an empty velocity command object that can be queried to find out
	 * the number of velcmd components,... */
	virtual CVehicleVelCmd::Ptr getVelCmdType() const = 0;

	/** Enable/Disable odometry errors. Errors in odometry are 1 sigma Gaussian
	 * values per second */
	void setOdometryErrors(
		bool enabled, double Ax_err_bias = 1e-3, double Ax_err_std = 10e-3,
		double Ay_err_bias = 1e-3, double Ay_err_std = 10e-3,
		double Aphi_err_bias = mrpt::DEG2RAD(1e-3),
		double Aphi_err_std = mrpt::DEG2RAD(10e-3))
	{
		m_use_odo_error = enabled;
		m_Ax_err_bias = Ax_err_bias;
		m_Ax_err_std = Ax_err_std;
		m_Ay_err_bias = Ay_err_bias;
		m_Ay_err_std = Ay_err_std;
		m_Aphi_err_bias = Aphi_err_bias;
		m_Aphi_err_std = Aphi_err_std;
	}

	void resetStatus();	 //! Reset all simulator variables to 0 (except the
	//! simulation time). \sa resetTime
	/** Reset time counter \sa resetStatus */
	void resetTime();

	/** @} */

   protected:
	/** @name State vector
	 *  @{ */
	/** simulation running time */
	double m_time;
	/** ground truth pose in world coordinates. */
	mrpt::math::TPose2D m_GT_pose;
	/** Velocity in (x,y,omega) */
	mrpt::math::TTwist2D m_GT_vel;
	/** Velocity in (x,y,omega) */
	mrpt::math::TTwist2D m_odometric_vel;
	mrpt::math::TPose2D m_odometry;
	/** @} */
	/** The period at which the low-level controller updates velocities
	 * (Default: 0.5 ms) */
	double m_firmware_control_period{500e-6};

	/** Whether to corrupt odometry with noise */
	bool m_use_odo_error{false};
	double m_Ax_err_bias, m_Ax_err_std;
	double m_Ay_err_bias, m_Ay_err_std;
	double m_Aphi_err_bias, m_Aphi_err_std;

	virtual void internal_simulControlStep(const double dt) = 0;
	/** Resets all pending cmds */
	virtual void internal_clear() = 0;

   private:
};

}  // namespace mrpt::kinematics
