/* +------------------------------------------------------------------------+
   |                     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 <mrpt/config/CConfigFile.h>
#include <mrpt/system/os.h>

#include <chrono>
#include <thread>

#include "DifOdometry_Datasets.h"

using namespace std;
using namespace mrpt;

const char* default_cfg_txt =
	"; ---------------------------------------------------------------\n"
	"; FILE: Difodo Parameters.txt\n"
	";\n"
	";  MJT @ JANUARY-2015\n"
	"; ---------------------------------------------------------------\n\n"

	"[DIFODO_CONFIG]\n\n"

	";downsample: 1 - 640x480, 2 - 320x240, 4 - 160x120 \n"
	"downsample = 2 \n\n"

	";Indicate the number of rows and columns. They must be equal or inferior "
	"to what is indicated with the 'downsample' variable). \n"
	"rows = 240 \n"
	"cols = 320 \n"
	"ctf_levels = 5 \n\n"

	";Absolute path of the rawlog file \n"
	"filename = "
	"C:/Users/Mariano/Desktop/rawlog_rgbd_dataset_freiburg1_desk/"
	"rgbd_dataset_freiburg1_desk.rawlog \n";
//"filename = .../file.rawlog \n";

// ------------------------------------------------------
//						MAIN
// ------------------------------------------------------

int main(int num_arg, char* argv[])
{
	try
	{
		//						Read function arguments
		//----------------------------------------------------------------------
		bool use_config_file = false;
		string filename;
		CDifodoDatasets odo;

		if (num_arg < 2)
			;
		else if (string(argv[1]) == "--help")
		{
			printf("\n\t       Arguments of the function 'main' \n");
			printf(
				"=============================================================="
				"\n\n");
			printf(" --help: Shows this menu... \n\n");
			printf(" --config FICH.txt: Load FICH.txt as config file \n\n");
			printf(
				" --create-config FICH.txt: Save the default config parameters "
				"\n\n");
			printf(" \t\t\t   in FICH.txt and close the program \n\n");
			printf(
				" --save-logfile: Enable saving a file with results of the "
				"pose estimate \n\n");
			system::os::getch();
			return 1;
		}
		else if (string(argv[1]) == "--create-config")
		{
			filename = argv[2];
			cout << endl << "Config_file name: " << filename;
			ofstream new_file(filename.c_str());
			new_file << string(default_cfg_txt);
			new_file.close();
			cout << endl << "File saved" << endl;
			system::os::getch();
			return 1;
		}
		else
		{
			for (int i = 1; i < num_arg; i++)
			{
				if (string(argv[i]) == "--save-logfile")
				{
					odo.save_results = true;
					odo.CreateResultsFile();
				}

				if (string(argv[i]) == "--config")
				{
					use_config_file = true;
					filename = argv[i + 1];
				}
			}
		}

		// Initial steps. Load configuration from file or default
		//------------------------------------------------------

		if (!use_config_file)
		{
			config::CConfigFileMemory configDifodo(default_cfg_txt);
			odo.loadConfiguration(configDifodo);
		}
		else
		{
			config::CConfigFile configDifodo(filename);
			odo.loadConfiguration(configDifodo);
		}

		odo.initializeScene();

		//==============================================================================
		//									Main operation
		//==============================================================================

		int pushed_key = 0;
		bool working = false, stop = false;

		// Necessary step before starting
		odo.reset();

		while (!stop)
		{
			if (odo.window.keyHit()) pushed_key = odo.window.getPushedKey();
			else
				pushed_key = 0;

			switch (pushed_key)
			{
				// Capture 1 new frame and calculate odometry
				case 'n':
				case 'N':
					if (odo.dataset_finished)
					{
						working = false;
						cout << endl << "End of dataset.";
						if (odo.f_res.is_open()) odo.f_res.close();
					}
					else
					{
						odo.loadFrame();
						odo.odometryCalculation();
						if (odo.save_results == 1) odo.writeTrajectoryFile();

						cout << endl
							 << "Difodo runtime(ms): " << odo.execution_time;
						odo.updateScene();
					}

					break;

					// Start and stop continuous odometry
				case 'S':
				case 's':
					working = !working;
					break;

					// Close the program
				case 'E':
				case 'e':
					stop = true;
					if (odo.f_res.is_open()) odo.f_res.close();
					break;
			}

			if (working)
			{
				if (odo.dataset_finished)
				{
					working = false;
					cout << endl << "End of dataset.";
					if (odo.f_res.is_open()) odo.f_res.close();
				}
				else
				{
					odo.loadFrame();
					odo.odometryCalculation();
					if (odo.save_results == 1) odo.writeTrajectoryFile();

					cout << endl
						 << "Difodo runtime(ms): " << odo.execution_time;
					odo.updateScene();
				}
			}
			using namespace std::chrono_literals;
			std::this_thread::sleep_for(5ms);
		}

		return 0;
	}
	catch (const std::exception& e)
	{
		std::cout << "MRPT exception caught: " << mrpt::exception_to_str(e)
				  << std::endl;
		return -1;
	}
}
