/* +------------------------------------------------------------------------+
   |                     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 "obs-precomp.h"  // Precompiled headers
//
#include <mrpt/containers/yaml.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/carmen_log_tools.h>
#include <mrpt/system/string_utils.h>

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

// Read the declaration in the .h file for documentation.
bool mrpt::obs::carmen_log_parse_line(
	std::istream& in_stream,
	std::vector<mrpt::obs::CObservation::Ptr>& out_observations,
	const mrpt::system::TTimeStamp& time_start_log)
{
	/** global parameters loaded in previous calls. */
	static mrpt::containers::yaml global_log_params;

	out_observations.clear();  // empty output container

	// Try to get line:
	string line;
	while (line.empty())
	{
		if (!in_stream) return false;  // End of file
		std::getline(in_stream, line);
		line = trim(line);
	};

	// Now we have a line: analyze it:
	if (strStartsI(line, "ROBOTLASER"))
	{
		// ROBOTLASER message
		// ---------------------------
		std::istringstream S;  // Read from the string as if it was a stream
		S.str(line);

		CObservation2DRangeScan::Ptr obsLaser_ptr =
			std::make_shared<CObservation2DRangeScan>();
		CObservation2DRangeScan* obsLaser =
			obsLaser_ptr.get();	 // Faster access

		// Parse:
		int laser_type;	 //  SICK_LMS = 0, SICK_PLS = 1, HOKUYO_URG = 2,
		//  SIMULATED_LASER = 3,
		double start_angle, angular_resolution, accuracy;
		int remission_mode;	 // OFF = 0, DIRECT = 1, NORMALIZED = 2

		if (!(S >> obsLaser->sensorLabel >> laser_type >> start_angle >>
			  obsLaser->aperture >> angular_resolution >> obsLaser->maxRange >>
			  accuracy >> remission_mode))
			THROW_EXCEPTION_FMT(
				"Error parsing line from CARMEN log (params):\n'%s'\n",
				line.c_str());

		size_t nRanges;
		S >> nRanges;

		obsLaser->resizeScan(nRanges);

		for (size_t i = 0; i < nRanges; i++)
		{
			float range;
			if (!(S >> range))
				THROW_EXCEPTION_FMT(
					"Error parsing line from CARMEN log (ranges):\n'%s'\n",
					line.c_str());
			obsLaser->setScanRange(i, range);
			// Valid value?
			obsLaser->setScanRangeValidity(
				i, !(range >= obsLaser->maxRange || range <= 0));
		}

		size_t remmision_count;
		if (!(S >> remmision_count))
			THROW_EXCEPTION_FMT(
				"Error parsing line from CARMEN log (remmision_count):\n'%s'\n",
				line.c_str());

		vector<double> remission;
		remission.resize(remmision_count);

		for (size_t i = 0; i < remmision_count; i++)
		{
			if (!(S >> remission[i]))
				THROW_EXCEPTION_FMT(
					"Error parsing line from CARMEN log (remmision "
					"vals):\n'%s'\n",
					line.c_str());
		}

		mrpt::math::TPose2D globalLaserPose;
		mrpt::math::TPose2D globalRobotPose;

		if (!(S >> globalLaserPose.x >> globalLaserPose.y >>
			  globalLaserPose.phi >> globalRobotPose.x >> globalRobotPose.y >>
			  globalRobotPose.phi))
			THROW_EXCEPTION_FMT(
				"Error parsing line from CARMEN log (poses):\n'%s'\n",
				line.c_str());

		// Compute pose of laser on the robot:
		obsLaser->sensorPose =
			CPose3D(CPose2D(globalLaserPose) - CPose2D(globalRobotPose));

		double tv, rv, fw_dist, side_dist, turn_axis;
		S >> tv >> rv >> fw_dist >> side_dist >> turn_axis;

		double timestamp;
		string robotName;
		S >> timestamp >> robotName;

		const mrpt::system::TTimeStamp obs_time = time_start_log +
			std::chrono::microseconds(static_cast<uint64_t>(1e-6 * timestamp));

		obsLaser->timestamp = obs_time;

		// Create odometry observation:
		{
			CObservationOdometry::Ptr obsOdo_ptr =
				std::make_shared<CObservationOdometry>();

			obsOdo_ptr->timestamp = obs_time;
			obsOdo_ptr->odometry = CPose2D(globalRobotPose);
			obsOdo_ptr->sensorLabel = "ODOMETRY";

			out_observations.push_back(obsOdo_ptr);
		}

		// Send out laser observation:
		out_observations.push_back(obsLaser_ptr);

	}  // end ROBOTLASER
	else if (strStartsI(line, "FLASER") || strStartsI(line, "RLASER"))
	{
		// [F,R]LASER message
		// FLASER num_readings [range_readings] x y theta odom_x odom_y
		// odom_theta
		// ---------------------------
		std::istringstream S;  // Read from the string as if it was a stream
		S.str(line);

		CObservation2DRangeScan::Ptr obsLaser_ptr =
			std::make_shared<CObservation2DRangeScan>();
		CObservation2DRangeScan* obsLaser =
			obsLaser_ptr.get();	 // Faster access

		// Parse:
		size_t nRanges;

		if (!(S >> obsLaser->sensorLabel >> nRanges))
			THROW_EXCEPTION_FMT(
				"Error parsing line from CARMEN log (params):\n'%s'\n",
				line.c_str());

		// Params:
		{
			double maxRange = 81.0;
			double resolutionDeg = 0.5;
			using namespace std::string_literals;

			if (line[0] == 'F')
			{  // front:
				maxRange = global_log_params.getOrDefault<double>(
					"robot_front_laser_max", 81.0);
				resolutionDeg = global_log_params.getOrDefault<double>(
					"laser_front_laser_resolution", 0.5);
			}
			else if (line[0] == 'R')
			{  // rear:
				maxRange = global_log_params.getOrDefault<double>(
					"robot_rear_laser_max", 81.0);
				resolutionDeg = global_log_params.getOrDefault<double>(
					"laser_rear_laser_resolution", 0.5);
			}
			obsLaser->maxRange = d2f(maxRange);
			obsLaser->aperture = d2f(DEG2RAD(resolutionDeg) * nRanges);
		}

		obsLaser->resizeScan(nRanges);

		for (size_t i = 0; i < nRanges; i++)
		{
			float range;
			if (!(S >> range))
				THROW_EXCEPTION_FMT(
					"Error parsing line from CARMEN log (ranges):\n'%s'\n",
					line.c_str());
			obsLaser->setScanRange(i, range);
			// Valid value?
			obsLaser->setScanRangeValidity(
				i, !(range >= obsLaser->maxRange || range <= 0));
		}

		mrpt::math::TPose2D globalLaserPose;
		mrpt::math::TPose2D globalRobotPose;
		if (!(S >> globalLaserPose.x >> globalLaserPose.y >>
			  globalLaserPose.phi >> globalRobotPose.x >> globalRobotPose.y >>
			  globalRobotPose.phi))
			THROW_EXCEPTION_FMT(
				"Error parsing line from CARMEN log (poses):\n'%s'\n",
				line.c_str());

		// Compute pose of laser on the robot:
		obsLaser->sensorPose =
			CPose3D(CPose2D(globalLaserPose) - CPose2D(globalRobotPose));

		double timestamp;
		string robotName;
		S >> timestamp >> robotName;

		const mrpt::system::TTimeStamp obs_time = time_start_log +
			std::chrono::microseconds(static_cast<uint64_t>(1e-6 * timestamp));

		obsLaser->timestamp = obs_time;

		// Create odometry observation:
		{
			CObservationOdometry::Ptr obsOdo_ptr =
				std::make_shared<CObservationOdometry>();

			obsOdo_ptr->timestamp = obs_time;
			obsOdo_ptr->odometry = CPose2D(globalRobotPose);
			obsOdo_ptr->sensorLabel = "ODOMETRY";

			out_observations.push_back(obsOdo_ptr);
		}

		// Send out laser observation:
		out_observations.push_back(obsLaser_ptr);

	}  // end RAWLASER
	else if (strStartsI(line, "PARAM "))
	{
		// PARAM message
		// ---------------------------
		std::istringstream S;  // Read from the string as if it was a stream
		S.str(line);

		string key, val;
		S >> key;  // This is "PARAM"

		if (!(S >> key >> val))
			THROW_EXCEPTION_FMT(
				"Error parsing line from CARMEN log (PARAM):\n'%s'\n",
				line.c_str());

		if (!key.empty() && !val.empty())

			global_log_params[lowerCase(key)] = val;

	}  // end RAWLASER

	return true;  // OK
}
