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

/**
 * slerp_demo
 * Execute a Spherical Linear Interpolation given 2 poses.
 */

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/img/TColor.h>
#include <mrpt/math/slerp.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/system/CTicTac.h>

#include <chrono>
#include <iostream>
#include <thread>

using namespace std;
using namespace mrpt::literals;	 // _deg
using namespace mrpt::math;
using namespace mrpt::gui;
using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::img;

// ------------------------------------------------------
//				TestSLERP
// ------------------------------------------------------
void TestSLERP()
{
	CDisplayWindow3D win("Example of SLERP animation", 640, 480);

	Scene::Ptr& theScene = win.get3DSceneAndLock();

	win.setCameraAzimuthDeg(-50);
	win.setCameraElevationDeg(40);
	win.setCameraZoom(19);
	win.setCameraPointingToPoint(2, 2, 0);

	// Modify the scene:
	// ------------------------------------------------------
	{
		auto obj = mrpt::opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1);
		obj->setColor(0.4f, 0.4f, 0.4f);
		theScene->insert(obj);
	}

	// Initialize the start, end pose of the animation
	const TPose3D pose_a(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
	const TPose3D pose_b(3, 4, 1, 120.0_deg, 40.0_deg, 50.0_deg);

	{
		// XYZ corner at A:
		auto obj = mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 2.0);
		obj->setPose(pose_a);
		theScene->insert(obj);
	}
	{
		// XYZ corner at B:
		auto obj = mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 2.0);
		obj->setPose(pose_b);
		theScene->insert(obj);
	}
	{
		// SLERP animated corner:
		auto obj = mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 4.0);
		obj->setName("slerp_obj");
		obj->setPose(pose_a);
		theScene->insert(obj);
	}

	// IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
	win.unlockAccess3DScene();

	cout << "\n Close the window to exit.\n";

	mrpt::system::CTicTac tic;
	static const double MOVE_PERIOD = 1.0;
	static const double MOVE_PERIOD2 = 2 * MOVE_PERIOD;

	while (win.isOpen())
	{
		// Compute the time:
		double t = ::fmod(tic.Tac(), MOVE_PERIOD2);
		if (t < MOVE_PERIOD) t /= MOVE_PERIOD;
		else
			t = 1 - (t - MOVE_PERIOD) / MOVE_PERIOD;

		// SLERP & LERP interpolation:
		TPose3D pose_interp;
		mrpt::math::slerp(pose_a, pose_b, t, pose_interp);

		// Move the scene:
		Scene::Ptr& theScene = win.get3DSceneAndLock();

		auto obj1 = theScene->getByName("slerp_obj");
		obj1->setPose(pose_interp);

		// Show text:
		win.addTextMessage(5, 5, mrpt::format("t=%.03f", t), 0 /*id*/);

		// IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
		win.unlockAccess3DScene();

		// Update window:
		win.forceRepaint();
		std::this_thread::sleep_for(5ms);
	};
}

// ------------------------------------------------------
//						MAIN
// ------------------------------------------------------
int main()
{
	try
	{
		TestSLERP();
		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
		return -1;
	}
}
