/* +------------------------------------------------------------------------+
   |                     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/CPose3DInterpolator.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/topography.h>
#include <test_mrpt_common.h>

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

TEST(TopographyReconstructPathFrom3RTK, sampleDataset)
{
	mrpt::poses::CPose3DInterpolator robot_path;

	mrpt::obs::CRawlog rawlog;

	const string dataset_fil = UNITTEST_BASEDIR() +
		string("/share/mrpt/datasets/test_rtk_path.rawlog");
	if (!mrpt::system::fileExists(dataset_fil))
	{
		cerr << "WARNING: Skipping test due to missing file: " << dataset_fil
			 << "\n";
		return;
	}
	if (!rawlog.loadFromRawLogFile(dataset_fil))
	{
		cerr << "WARNING: Skipping test due to error loading file: "
			 << dataset_fil << "\n";
	}
	else
	{
		mrpt::topography::TPathFromRTKInfo rtk_path_info;

		// -------------------------------------------
		// Run path reconstruction:
		// -------------------------------------------
		mrpt::topography::path_from_rtk_gps(
			robot_path, rawlog,
			0,	// first entry
			rawlog.size() - 1,	// last entry
			false,	// Isn't a GUI
			false,	// disableGPSInterp
			1,	// path_smooth_filter_size
			&rtk_path_info);

		EXPECT_EQ(robot_path.size(), 75u);

		// Expected values:
		// 1226225355.000000 279.705647 216.651473 8.517821 0.194222 -0.083873
		// -0.045293
		// 1226225380.000000 377.095830 233.343569 9.724171 0.177037 -0.073565
		// -0.019024
		const mrpt::Clock::time_point t1(
			std::chrono::seconds(1226225355 + 11644473600));
		const mrpt::Clock::time_point t2(
			std::chrono::seconds(1226225380 + 11644473600));
		const CPose3D pose_GT_1(
			279.696, 216.623, 9.21315, 0.195764, -0.0319733, -0.0420478);
		const CPose3D pose_GT_2(
			377.087, 233.311, 10.474, 0.178932, -0.0212096, -0.0154982);

		CPose3D pose1, pose2;
		bool valid;
		robot_path.interpolate(t1, pose1, valid);
		EXPECT_TRUE(valid);

		robot_path.interpolate(t2, pose2, valid);
		EXPECT_TRUE(valid);

		CVectorDouble p1vec(12), p2vec(12);
		pose1.getAs12Vector(p1vec);
		pose2.getAs12Vector(p2vec);

		CVectorDouble p1vec_gt(12), p2vec_gt(12);
		pose_GT_1.getAs12Vector(p1vec_gt);
		pose_GT_2.getAs12Vector(p2vec_gt);
		EXPECT_NEAR((p1vec - p1vec_gt).sum_abs(), 0, 1e-3);
		EXPECT_NEAR((p2vec - p2vec_gt).sum_abs(), 0, 1e-3);
	}
}
