/* +------------------------------------------------------------------------+
   |                     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/3rdparty/tclap/CmdLine.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

int main(int argc, char** argv)
{
	try
	{
		// Declare the supported command line switches ===========
		TCLAP::CmdLine cmd(
			"image2gridmap", ' ', mrpt::system::MRPT_getVersion().c_str());

		TCLAP::ValueArg<std::string> arg_input_file(
			"i", "input", "Input image file (required) (*.png,*.jpg,...)", true,
			"", "map_image.png", cmd);
		TCLAP::ValueArg<std::string> arg_output_file(
			"o", "output", "Name of the output file (*.gridmap, *.gridmap.gz)",
			false, "", "map.gridmap.gz", cmd);
		TCLAP::ValueArg<double> arg_res(
			"r", "res",
			"Resolution: size (in meters) of one pixel in the image (required)",
			true, 0.1, "0.1", cmd);
		TCLAP::ValueArg<double> arg_cx(
			"", "cx",
			"(Use either --cx or --px) X coordinate of the image central pixel "
			"(Default:0)",
			false, 0.0, "0.0", cmd);
		TCLAP::ValueArg<double> arg_cy(
			"", "cy",
			"(Use either --cy or --py) Y coordinate of the image central pixel "
			"(Default:0)",
			false, 0.0, "0.0", cmd);
		TCLAP::ValueArg<double> arg_px(
			"", "px",
			"(Use either --cx or --px) Pixel horizontal coordinate of the "
			"origin of coordinates in the image",
			false, 0.0, "0.0", cmd);
		TCLAP::ValueArg<double> arg_py(
			"", "py",
			"(Use either --cx or --px) Pixel verticl coordinate of the origin "
			"of coordinates in the image",
			false, 0.0, "0.0", cmd);
		TCLAP::SwitchArg arg_overwrite(
			"w", "overwrite", "Force overwrite target file without prompting.",
			cmd, false);

		printf(" image2gridmap - Part of the MRPT\n");
		printf(
			" MRPT C++ Library: %s - Sources timestamp: %s\n",
			mrpt::system::MRPT_getVersion().c_str(),
			mrpt::system::MRPT_getCompilationDate().c_str());
		printf(
			"------------------------------------------------------------------"
			"-\n");

		// Parse arguments:
		if (!cmd.parse(argc, argv))
			throw std::runtime_error("");  // should exit.

		const std::string inputFile = arg_input_file.getValue();
		const double cell_res = arg_res.getValue();

		mrpt::img::CImage img;
		if (!img.loadFromFile(inputFile))
			throw std::runtime_error(mrpt::format(
				"Cannot load the map image file `%s`!", inputFile.c_str()));

		double px, py;
		if ((arg_px.isSet() && !arg_py.isSet()) ||
			(!arg_px.isSet() && arg_py.isSet()))
			throw std::runtime_error(
				"You cannot set only one of --px & --py arguments!");

		if ((arg_cx.isSet() && !arg_cy.isSet()) ||
			(!arg_cx.isSet() && arg_cy.isSet()))
			throw std::runtime_error(
				"You cannot set only one of --cx & --cy arguments!");

		if (arg_cx.isSet() && arg_px.isSet())
			throw std::runtime_error(
				"You cannot set BOTH --cx & --px arguments!");

		if (arg_px.isSet())
		{
			px = arg_px.getValue();
			py = arg_py.getValue();
		}
		else
		{
			px = -arg_cx.getValue() / cell_res + img.getWidth() / 2;
			py = -arg_cy.getValue() / cell_res + img.getHeight() / 2;
		}

		mrpt::maps::COccupancyGridMap2D grid;
		grid.loadFromBitmap(img, cell_res, {px, py});

		const std::string sOutFile = arg_output_file.isSet()
			? arg_output_file.getValue()
			: mrpt::system::fileNameChangeExtension(inputFile, "gridmap.gz");
		std::cout << "Output map file: " << sOutFile << std::endl;

		if (mrpt::system::fileExists(sOutFile) && !arg_overwrite.isSet())
		{
			std::cerr << "Output file already exists, aborting. Use `-w` flag "
						 "to overwrite.\n";
			return 1;
		}

		{
			mrpt::io::CFileGZOutputStream f(sOutFile);
			mrpt::serialization::archiveFrom(f) << grid;
		}

		std::cout << "All done.\n";

		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << mrpt::exception_to_str(e) << std::endl;
		return 1;
	}
}
