/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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 <mrpt/config/CConfigFile.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/hwdrivers/CCameraSensor.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/filesystem.h>	 // for ASSERT_FILE_EXISTS_
#include <mrpt/vision/CStereoRectifyMap.h>

#include <iostream>

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::gui;
using namespace mrpt::vision;
using namespace mrpt::system;
using namespace mrpt::img;
using namespace mrpt::obs;
using namespace mrpt::system;
using namespace mrpt::config;
using namespace mrpt::img;
using namespace std;

// ------------------------------------------------------
//				TestStereoRectify
// ------------------------------------------------------
void TestStereoRectify(int argc, char** argv)
{
	CTimeLogger timlog;
	mrpt::vision::CStereoRectifyMap rectifyMap;

	// Parse optional arguments:
	if (argc != 1 && argc != 2)
	{
		cout
			<< "Usage:\n"
			<< argv[0]
			<< " ==> Run with default camera parameters (from rawlog file)\n"
			<< argv[0]
			<< "[params.cfg] ==> Load stereo camera parameters from cfg file\n";
	}
	if (argc == 2)
	{
		const string sCfgFile = argv[1];
		ASSERT_FILE_EXISTS_(sCfgFile);

		// Load params from file:
		mrpt::img::TStereoCamera params;
		params.loadFromConfigFile(
			"CAMERA_PARAMS", mrpt::config::CConfigFile(sCfgFile));

		// Prepare rectify map:
		timlog.enter("rectifyMap.setFromCamParams");
		rectifyMap.setFromCamParams(params);
		timlog.leave("rectifyMap.setFromCamParams");
	}

	// Show to the user a list of possible camera drivers and creates and open
	// the selected camera.
	cout << "Please, select the input stereo camera or rawlog file (with "
			"stereo images)...\n";

	mrpt::hwdrivers::CCameraSensor::Ptr cam =
		mrpt::hwdrivers::prepareVideoSourceFromUserSelection();
	if (!cam) return;

	cout << "Video stream open OK\n";

	// Create 3D window:
	CDisplayWindow3D win("Demo of stereo rectification", 1280, 600);

	// Create 2 viewports, one for each image:
	std::vector<COpenGLViewport::Ptr> gl_views(2);
	{
		COpenGLScene::Ptr& theScene = win.get3DSceneAndLock();
		gl_views[0] = theScene->getViewport("main");
		ASSERT_(gl_views[0]);
		gl_views[1] = theScene->createViewport("right_image");

		// Assign sizes:
		gl_views[0]->setViewportPosition(0, 0, .5, 1.);
		gl_views[1]->setViewportPosition(.5, 0, .5, 1.);

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

	win.setPos(10, 10);
	//	win.addTextMessage(...

	bool enable_rectify = true;
	bool enable_draw_epipolar_lines = true;
	CImage img_left_rectified,
		img_right_rectified;  // Declared here to serve as a memory buffer
	// (avoid deallocating/allocating)

	cout << "Close the window to end.\n";
	while (win.isOpen())
	{
		win.addTextMessage(5, 5, format("%.02fFPS", win.getRenderingFPS()), 0);
		win.addTextMessage(
			5, 25,
			format(
				"'r': Switch rectify (Now is: %s) | '+'/'-': Modify "
				"alpha (Now is: %.02f)",
				enable_rectify ? "ON" : "OFF", rectifyMap.getAlpha()),
			10);
		win.addTextMessage(
			5, 50,
			format(
				"'s': Switch resize output to 320x240 (Now is: %s) | 'c': "
				"Switch no-disparity (Now is: %s) | 'e': Switch epipolar lines",
				rectifyMap.isEnabledResizeOutput() ? "ON" : "OFF",
				rectifyMap.isEnabledBothCentersCoincide() ? "ON" : "OFF"),
			11);

		std::this_thread::sleep_for(1ms);

		// Grab new video frame:
		CObservation::Ptr obs = cam->getNextFrame();
		if (obs)
		{
			if (IS_CLASS(*obs, CObservationStereoImages))
			{
				// Get the observation object:
				CObservationStereoImages::Ptr o =
					std::dynamic_pointer_cast<CObservationStereoImages>(obs);

				// If the rectification maps are still not ready, prepare them
				// now:
				if (!rectifyMap.isSet())
				{
					timlog.enter("rectifyMap.setFromCamParams");
					rectifyMap.setFromCamParams(*o);
					timlog.leave("rectifyMap.setFromCamParams");

					/*mrpt::img::TStereoCamera params;
					o->getStereoCameraParams(params);
					cout << params.dumpAsText() << endl;*/
				}

				win.get3DSceneAndLock();

				if (enable_rectify)
				{
					// Rectify:
					timlog.enter("rectifyMap.rectify()");

					rectifyMap.rectify(
						o->imageLeft, o->imageRight, img_left_rectified,
						img_right_rectified);

					timlog.leave("rectifyMap.rectify()");
				}
				else
				{
					// Don't rectify:
					img_left_rectified = o->imageLeft;
					img_right_rectified = o->imageRight;
				}

				// Draw lines:
				if (enable_draw_epipolar_lines)
				{
					const unsigned int LINES_SEP = 40;
					const unsigned int w = img_left_rectified.getWidth();
					const unsigned int h = img_left_rectified.getHeight();
					for (unsigned int y = 0; y < h; y += LINES_SEP)
					{
						img_left_rectified.line(
							0, y, w - 1, y, mrpt::img::TColor::red(), 2);
						img_right_rectified.line(
							0, y, w - 1, y, mrpt::img::TColor::red(), 2);
					}
				}

				gl_views[0]->setImageView(img_left_rectified);
				gl_views[1]->setImageView(img_right_rectified);

				win.addTextMessage(
					150, 5, mrpt::system::timeToString(o->timestamp), 2);

				win.unlockAccess3DScene();
				win.repaint();
			}

			if (win.keyHit())
			{
				mrptKeyModifier kmods;
				int key = win.getPushedKey(&kmods);

				if (key == MRPTK_ESCAPE) break;
				if (key == 'r' || key == 'R') enable_rectify = !enable_rectify;
				if (key == 'e' || key == 'E')
					enable_draw_epipolar_lines = !enable_draw_epipolar_lines;
				if (key == '+' || key == '-')
				{
					double alpha =
						rectifyMap.getAlpha() + (key == '-' ? -0.1 : 0.1);
					alpha = std::min(1., std::max(0., alpha));
					rectifyMap.setAlpha(alpha);
				}
				if (key == 's' || key == 'S')
				{
					rectifyMap.enableResizeOutput(
						!rectifyMap.isEnabledResizeOutput(), 320, 240);
				}
				if (key == 'c' || key == 'C')
				{
					rectifyMap.enableBothCentersCoincide(
						!rectifyMap.isEnabledBothCentersCoincide());
				}
			}
		}
	}
}

// ------------------------------------------------------
//						MAIN
// ------------------------------------------------------
int main(int argc, char** argv)
{
	try
	{
		TestStereoRectify(argc, argv);
		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
		return -1;
	}
	catch (...)
	{
		printf("Untyped exception!!");
		return -1;
	}
}
