/* +---------------------------------------------------------------------------+
   |                     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 details in https://www.mrpt.org/License   |
   +---------------------------------------------------------------------------+
   */

#include <mrpt/core/exceptions.h>
#include <mrpt/io/CFileInputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/system/datetime.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>

#include <string>
#include <vector>

namespace mrpt::poses::internal
{
inline void getPoseFromString(const std::string& s, mrpt::poses::CPose2D& p)
{
	using namespace std;
	using namespace mrpt::system;
	using namespace mrpt::poses;

	p.fromStringRaw(s);

}  // end of getPoseFromString

/** Set the current object value from a string (e.g.: "x y z qx qy qz qw")
 *
 * Specialization for files in the TUM format
 * \exception std::exception On invalid format
 */
template <bool QUAT_REPR = true, bool TUM_FORMAT = true>
void getPoseFromString(const std::string& s, mrpt::poses::CPose3D& p)
{
	std::vector<std::string> curr_tokens;
	mrpt::system::tokenize(s, " ", curr_tokens);

	ASSERTMSG_(
		curr_tokens.size() == 7,
		mrpt::format(
			"Invalid number of tokens in given string\n"
			"\tExpected:    7\n"
			"\tTokens size: %" USIZE_STR "\n",
			curr_tokens.size()));

	mrpt::math::CQuaternionDouble quat;
	quat.r(atof(curr_tokens[6].c_str()));
	quat.x(atof(curr_tokens[3].c_str()));
	quat.y(atof(curr_tokens[4].c_str()));
	quat.z(atof(curr_tokens[5].c_str()));
	double roll, pitch, yaw;
	quat.rpy(roll, pitch, yaw);

	p.setFromValues(
		atof(curr_tokens[0].c_str()),  // x
		atof(curr_tokens[1].c_str()),  // y
		atof(curr_tokens[2].c_str()),  // z
		roll, pitch, yaw);
}  // end of fromStringQuat

template <>
inline void getPoseFromString</*QUAT_REPR=*/false, /*TUM_FORMAT=*/false>(
	const std::string& s, mrpt::poses::CPose3D& p)
{
	p.fromStringRaw(s);
}  // end of getPoseFromString

/**Invalid form. TUM ground truth files are always in Quaternion form. */
template <>
inline void getPoseFromString<false, true>(
	const std::string& s, mrpt::poses::CPose3D& p)
{
	THROW_EXCEPTION("Invalid combination: QUAT_REPR=false, TUM_FORMAT=true");
}

/**\brief Specialization for strings in Quaternion form */
template <>
inline void getPoseFromString</*QUAT_REPR=*/true, /*TUM_FORMAT=*/false>(
	const std::string& s, mrpt::poses::CPose3D& p)
{
	mrpt::poses::CPose3DQuat p_quat;
	p_quat.fromStringRaw(s);
	p = mrpt::poses::CPose3D(p_quat);
}  // end of getPoseFromString

}  // namespace mrpt::poses::internal

/** \name Parsing of textfiles with poses */
/** \{ */
/** \brief Parse the textfile and fill in the corresponding \a poses vector.
 *
 * The file to be parsed is to contain 2D or 3D poses along with their
 * corresponding timestamps, one line for each.
 *
 * The expected format is the following:
 *
 * - For 2D Poses: timestamp x y theta (in rad)
 * - For 3D Poses in RPY form : x y z yaw pitch roll
 * - For 3D Poses in Quaternion form : x y z qw qx qy qz
 * - For 3D Poses in Quaternion form [TUM Datasets] : x y z qx qy qz qw
 *
 * The 2D format abides to the groundtruth file format used by the
 * <em>GridMapNavSimul</em> application
 *
 * The TUM format is compatible with the groundtruth format for the
 * TUM RGBD datasets as generated by the * <em>rgbd_dataset2rawlog</em> MRPT
 * tool.
 *
 * \param[in] fname Filename from which the timestamps and poses are read
 * \param[out] poses_vec std::vector which is to contain the 2D poses.
 * \param[out] timestamps std::vector which is to contain the timestamps
 * for the corresponding ground truth poses. Ignore this argument if
 * timestamps are not needed.
 * \param[in] substract_init_offset If true, the filled poses are to start from
 * 0, that means, that if the first found pose is non-zero, it's going to be
 * considered and offset and substracted from all poses in the file.
 *
 * \sa
 * https://www.mrpt.org/Collection_of_Kinect_RGBD_datasets_with_ground_truth_CVPR_TUM_2011
 *
 * \ingroup mrpt_poses_grp
 */
template <class POSE_T>
void readFileWithPoses(
	const std::string& fname, std::vector<POSE_T>* poses_vec,
	std::vector<mrpt::system::TTimeStamp>* timestamps = NULL,
	bool substract_init_offset = false)
{
	MRPT_START

	using namespace std;
	using namespace mrpt::system;
	using namespace mrpt::poses;
	using namespace internal;

	// make sure file exists
	ASSERTMSG_(
		fileExists(fname),
		format(
			"\nFile %s was not found.\n"
			"Either specify a valid filename or set set the "
			"m_visualize_GT flag to false\n",
			fname.c_str()));

	CFileInputStream file_GT(fname);
	ASSERTMSG_(
		file_GT.fileOpenCorrectly(),
		"\nreadGTFileRGBD_TUM: Couldn't openGT file\n");
	ASSERTMSG_(poses_vec, "std::vector<POSE_T>* is not valid.");

	string curr_line;

	// move to the first non-commented line
	for (size_t i = 0; file_GT.readLine(curr_line); i++)
	{
		if (curr_line.at(0) != '#') { break; }
	}

	// handle the first pose as an offset
	POSE_T pose_offset_opposite;
	if (substract_init_offset)
	{
		POSE_T pose_offset;
		getPoseFromString(
			std::string(
				curr_line.begin() + curr_line.find_first_of(" \t") + 1,
				curr_line.end()),
			pose_offset);
		pose_offset_opposite = pose_offset.getOppositeScalar();
	}

	// parse the file - get timestamp and pose and fill in the vector
	for (; file_GT.readLine(curr_line);)
	{
		// timestamp
		if (timestamps)
		{
			std::string timestamp_str = std::string(
				curr_line.begin(),
				curr_line.begin() + curr_line.find_first_of(" \t"));
			timestamps->push_back(atof(timestamp_str.c_str()));
		}

		POSE_T curr_pose;
		getPoseFromString(
			std::string(
				curr_line.begin() + curr_line.find_first_of(" \t") + 1,
				curr_line.end()),
			curr_pose);

		// scalar substraction of initial offset
		if (substract_init_offset)
		{ curr_pose.addComponents(pose_offset_opposite); }

		// push the newly created pose
		poses_vec->push_back(curr_pose);
	}  // end for loop

	file_GT.close();

	MRPT_END
}
