/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#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;
}
