/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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/CVehicleSimulVirtualBase.h>
#include <mrpt/kinematics/CVehicleVelCmd_DiffDriven.h>

namespace mrpt::kinematics
{
/** Simulates the kinematics of a differential-driven planar mobile
 * robot/vehicle, including odometry errors and dynamics limitations.
 *
 * \ingroup mrpt_kinematics_grp
 */
class CVehicleSimul_DiffDriven : public CVehicleSimulVirtualBase
{
 public:
  using kinematic_cmd_t = CVehicleVelCmd_DiffDriven;

  CVehicleSimul_DiffDriven();
  ~CVehicleSimul_DiffDriven() override;

  /** Change the model of delays used for the orders sent to the robot \sa
   * movementCommand */
  void setDelayModelParams(double TAU_delay_sec = 1.8, double CMD_delay_sec = 0.)
  {
    cTAU = TAU_delay_sec;
    cDELAY = CMD_delay_sec;
  }

  void setV(double v) { m_v = v; }
  void setW(double w) { m_w = w; }
  double getV() { return m_v; }
  double getW() { return m_w; }
  /** Used to command the robot a desired movement:
   * \param lin_vel Linar velocity (m/s)
   * \param ang_vel Angular velocity (rad/s)
   */
  void movementCommand(double lin_vel, double ang_vel);

  void sendVelCmd(const CVehicleVelCmd& cmd_vel) override
  {
    const auto* cmd = dynamic_cast<const kinematic_cmd_t*>(&cmd_vel);
    ASSERTMSG_(
        cmd,
        "Wrong vehicle kinematic class, expected "
        "`CVehicleVelCmd_DiffDriven`");
    movementCommand(cmd->lin_vel, cmd->ang_vel);
  }
  CVehicleVelCmd::Ptr getVelCmdType() const override
  {
    return CVehicleVelCmd::Ptr(new kinematic_cmd_t());
  }

 private:
  /** lin & angular velocity in the robot local frame. */
  double m_v{0}, m_w{0};

  /** Dynamic limitations of the robot.
   * Approximation to non-infinity motor forces: A first order low-pass
   * filter, using:
   *   Command_Time: Time "t" when the last order was received.
   *   Command_v, Command_w: The user-desired velocities.
   *   Command_v0, Command_w0: Actual robot velocities at the moment of
   * user request.
   */
  double Command_Time{0}, Command_v{0}, Command_w{0}, Command_v0{0}, Command_w0{0};

  /** The time-constants for the first order low-pass filter for the
   * velocities changes. */
  double cTAU{.0};
  /** The delay constant for the velocities changes */
  double cDELAY{.0};

  void internal_simulControlStep(const double dt) override;
  void internal_clear() override;
};

}  // namespace mrpt::kinematics
