/* +------------------------------------------------------------------------+
   |                     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 <gtest/gtest.h>
#include <mrpt/poses/CRobot2DPoseEstimator.h>
#include <mrpt/system/datetime.h>

TEST(CRobot2DPoseEstimator, defaultCtor)
{
	mrpt::poses::CRobot2DPoseEstimator rpe;

	mrpt::math::TPose2D p;
	mrpt::math::TTwist2D vl, vg;

	bool ok = rpe.getCurrentEstimate(p, vl, vg);

	EXPECT_FALSE(ok);
}

TEST(CRobot2DPoseEstimator, extrapolateRobotPose)
{
	mrpt::math::TPose2D p = {0, 0, 0};
	const mrpt::math::TTwist2D vl = {2.0, 1.0, -0.5};
	const double dt = 2.0;

	mrpt::math::TPose2D new_p;
	mrpt::poses::CRobot2DPoseEstimator::extrapolateRobotPose(p, vl, dt, new_p);

	EXPECT_DOUBLE_EQ(new_p.x, p.x + dt * vl.vx);
	EXPECT_DOUBLE_EQ(new_p.y, p.y + dt * vl.vy);
	EXPECT_DOUBLE_EQ(new_p.phi, p.phi + dt * vl.omega);

	// now, rotated 90deg:
	p = mrpt::math::TPose2D{10.0, 20.0, mrpt::DEG2RAD(90.0)};
	mrpt::poses::CRobot2DPoseEstimator::extrapolateRobotPose(p, vl, dt, new_p);

	EXPECT_NEAR(new_p.x, p.x - dt * vl.vy, 1e-5);
	EXPECT_NEAR(new_p.y, p.y + dt * vl.vx, 1e-5);
	EXPECT_NEAR(new_p.phi, p.phi + dt * vl.omega, 1e-5);
}

TEST(CRobot2DPoseEstimator, integrateOdometryAndLocalization)
{
	const mrpt::math::TPose2D odo1 = {0, 0, 0};
	const mrpt::math::TPose2D loc1 = {10.0, 20.0, mrpt::DEG2RAD(90.0)};
	const mrpt::math::TTwist2D vel1 = {1.0, 0.0, mrpt::DEG2RAD(10.0)};
	const double dt = 1.0;
	const auto odo2 = odo1 + vel1 * dt;

	mrpt::poses::CRobot2DPoseEstimator rpe;

	rpe.params.max_localiz_age = 2 * dt;
	rpe.params.max_odometry_age = 2 * dt;

	const auto t0 = mrpt::Clock::now();
	const auto t1 = mrpt::system::timestampAdd(t0, dt);

	// t0:
	rpe.processUpdateNewOdometry(odo1, t0, true, vel1);
	rpe.processUpdateNewPoseLocalization(loc1, t0);

	mrpt::math::TPose2D p;
	mrpt::math::TTwist2D vl, vg;
	bool ok = rpe.getCurrentEstimate(p, vl, vg, t0);
	EXPECT_TRUE(ok);
	EXPECT_NEAR(p.x, loc1.x, 1e-5);
	EXPECT_NEAR(p.y, loc1.y, 1e-5);
	EXPECT_NEAR(p.phi, loc1.phi, 1e-5);

	EXPECT_NEAR(vl.vx, vel1.vx, 1e-5);
	EXPECT_NEAR(vl.vy, vel1.vy, 1e-5);
	EXPECT_NEAR(vl.omega, vel1.omega, 1e-5);

	EXPECT_NEAR(vg.vx, -vel1.vy, 1e-5);	 // 90deg rot
	EXPECT_NEAR(vg.vy, vel1.vx, 1e-5);
	EXPECT_NEAR(vg.omega, vel1.omega, 1e-5);

	// t1:
	rpe.processUpdateNewOdometry(odo2, t1, true, vel1);
	ok = rpe.getCurrentEstimate(p, vl, vg, t1);
	EXPECT_TRUE(ok);
	EXPECT_NEAR(p.x, loc1.x - dt * vel1.vy, 1e-5);
	EXPECT_NEAR(p.y, loc1.y + dt * vel1.vx, 1e-5);
	EXPECT_NEAR(p.phi, loc1.phi + dt * vel1.omega, 1e-5);

	// reset:
	rpe.reset();
	ok = rpe.getCurrentEstimate(p, vl, vg, t1);
	EXPECT_FALSE(ok);
}
