/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/reverse_bytes.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/obs/CSinCosLookUpTableFor2DScans.h>
#include <mrpt/obs/VelodyneCalibration.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/serialization/CSerializable.h>

#include <vector>

namespace mrpt
{
namespace poses
{
class CPose3DInterpolator;
}
namespace obs
{
/** A `CObservation`-derived class for RAW DATA (and optionally, point cloud) of
 * scans from 3D Velodyne LIDAR scanners.
 * A scan comprises one or more "velodyne packets" (refer to Velodyne user
 * manual). Normally, a full 360 degrees sweep is included in one observation
 * object. Note that if a pointcloud is generated inside this class, each point
 * is annotated with per-point information about its exact azimuth and laser_id
 * (ring number), an information that is loss when inserting the observation in
 * a regular mrpt::maps::CPointsMap.
 *
 * <h2>Main data fields:</h2><hr>
 * - CObservationVelodyneScan::scan_packets with raw data packets.
 * - CObservationVelodyneScan::point_cloud normally empty after grabbing for
 * efficiency, can be generated calling \a
 * CObservationVelodyneScan::generatePointCloud()
 *
 * Dual return mode is supported (see mrpt::hwdrivers::CVelodyneScanner).
 *
 * Axes convention for point cloud (x,y,z) coordinates:
 *
 *  <div align=center> <img src="velodyne_axes.jpg"> </div>
 *
 * If it can be assumed that the sensor moves SLOWLY through the environment
 * (i.e. its pose can be approximated to be the same since the beginning to the
 * end of one complete scan)
 * then this observation can be converted / loaded into the following other
 * classes:
 *  - Maps of points (these require first generating the pointcloud in this
 * observation object with
 * mrpt::obs::CObservationVelodyneScan::generatePointCloud() ):
 *    - mrpt::maps::CPointsMap::loadFromVelodyneScan() (available in all
 * derived classes)
 *    - and the generic method:mrpt::maps::CPointsMap::insertObservation()
 *  - mrpt::opengl::CPointCloud and mrpt::opengl::CPointCloudColoured is
 * supported by first converting
 *    this scan to a mrpt::maps::CPointsMap-derived class, then loading it into
 * the opengl object.
 *
 * Otherwise, the following API exists for accurate reconstruction of the
 * sensor path in SE(3) over time:
 *  - CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory()
 *
 *  Note that this object has \b two timestamp fields:
 *  - The standard CObservation::timestamp field in the base class, which
 * should contain the accurate satellite-based UTC timestamp, and
 *  - the field CObservationVelodyneScan::originalReceivedTimestamp, with the
 * local computer-based timestamp based on the reception of the message in the
 * computer.
 *  Both timestamps correspond to the firing of the <b>first</b> laser in the
 * <b>first</b> CObservationVelodyneScan::scan_packets packet.
 *
 * \note New in MRPT 1.4.0
 * \sa CObservation, mrpt::maps::CPointsMap, mrpt::hwdrivers::CVelodyneScanner
 * \ingroup mrpt_obs_grp
 */
class CObservationVelodyneScan : public CObservation
{
	DEFINE_SERIALIZABLE(CObservationVelodyneScan, mrpt::obs)

   public:
	/** @name Raw scan fixed parameters
		@{ */
	static const int SIZE_BLOCK = 100;
	static const int RAW_SCAN_SIZE = 3;
	static const int SCANS_PER_BLOCK = 32;
	static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);

	static const uint16_t ROTATION_MAX_UNITS =
		36000; /**< hundredths of degrees */

	static constexpr float ROTATION_RESOLUTION = 0.01f; /**< degrees */
	static constexpr float DISTANCE_MAX = 130.0f; /**< meters */
	static constexpr float DISTANCE_RESOLUTION = 0.002f; /**< meters */

	/** Blocks 0-31 */
	static const uint16_t UPPER_BANK = 0xeeff;
	/** Blocks 32-63 */
	static const uint16_t LOWER_BANK = 0xddff;

	static const int SCANS_PER_FIRING = 16;

	static const int PACKET_SIZE = 1206;
	static const int POS_PACKET_SIZE = 512;
	static const int BLOCKS_PER_PACKET = 12;
	static const int PACKET_STATUS_SIZE = 4;
	static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);

	static const uint8_t RETMODE_STRONGEST = 0x37;
	static const uint8_t RETMODE_LAST = 0x38;
	static const uint8_t RETMODE_DUAL = 0x39;
	/** @} */

	/** @name Scan data
		@{ */

#pragma pack(push, 1)
	struct laser_return_t
	{
	   private:
		uint16_t m_distance;
		uint8_t m_intensity;

	   public:
		inline uint16_t distance() const
		{
			return mrpt::toNativeEndianness(m_distance);
		}
		inline uint8_t intensity() const { return m_intensity; }
	};
	/** Raw Velodyne data block.
	 *  Each block contains data from either the upper or lower laser
	 *  bank.  The device returns three times as many upper bank blocks. */
	struct raw_block_t
	{
	   private:
		uint16_t m_header;	///< Block id: UPPER_BANK or LOWER_BANK
		uint16_t m_rotation;  ///< 0-35999, divide by 100 to get degrees
	   public:
		laser_return_t laser_returns[SCANS_PER_BLOCK];

		inline uint16_t header() const
		{
			return mrpt::toNativeEndianness(m_header);
		}
		inline uint16_t rotation() const
		{
			return mrpt::toNativeEndianness(m_rotation);
		}
	};

	/** One unit of data from the scanner (the payload of one UDP DATA packet)
	 */
	struct TVelodyneRawPacket
	{
	   public:
		raw_block_t blocks[BLOCKS_PER_PACKET];

	   private:
		/** us from top of hour */
		uint32_t m_gps_timestamp;

	   public:
		/** 0x37: strongest, 0x38: last, 0x39: dual return */
		uint8_t laser_return_mode;
		/** 0x21: HDL-32E, 0x22: VLP-16 */
		uint8_t velodyne_model_ID;

		inline uint32_t gps_timestamp() const
		{
			return mrpt::toNativeEndianness(m_gps_timestamp);
		}
	};

	/** Payload of one POSITION packet */
	struct TVelodynePositionPacket
	{
	   public:
		char unused1[198];

	   private:
		uint32_t m_gps_timestamp;
		uint32_t m_unused2;

	   public:
		/** the full $GPRMC message, as received by Velodyne, terminated with
		 * "\r\n\0" */
		char NMEA_GPRMC[72 + 234];

		inline uint32_t gps_timestamp() const
		{
			return mrpt::toNativeEndianness(m_gps_timestamp);
		}
		inline uint32_t unused2() const
		{
			return mrpt::toNativeEndianness(m_unused2);
		}
	};
#pragma pack(pop)

	/** The maximum range allowed by the device, in meters (e.g. 100m). Stored
	 * here by the driver while capturing based on the sensor model. */
	double minRange{1.0}, maxRange{130.0};
	/** The 6D pose of the sensor on the robot/vehicle frame of reference */
	mrpt::poses::CPose3D sensorPose;
	/** The main content of this object: raw data packet from the LIDAR. \sa
	 * point_cloud */
	std::vector<TVelodyneRawPacket> scan_packets;
	/** The calibration data for the LIDAR device. See
	 * mrpt::hwdrivers::CVelodyneScanner and mrpt::obs::VelodyneCalibration for
	 * details. */
	mrpt::obs::VelodyneCalibration calibration;
	/** The local computer-based timestamp based on the reception of the message
	 * in the computer. \sa has_satellite_timestamp, CObservation::timestamp in
	 * the base class, which should contain the accurate satellite-based UTC
	 * timestamp.  */
	mrpt::system::TTimeStamp originalReceivedTimestamp{INVALID_TIMESTAMP};
	/** If true, CObservation::timestamp has been generated from accurate
	 * satellite clock. Otherwise, no GPS data is available and timestamps are
	 * based on the local computer clock. */
	bool has_satellite_timestamp{false};

	mrpt::system::TTimeStamp getOriginalReceivedTimeStamp()
		const override;	 // See base class docs

	/** See \a point_cloud and \a scan_packets */
	struct TPointCloud
	{
		std::vector<float> x, y, z;
		/** Color [0,255] */
		std::vector<uint8_t> intensity;
		/** Timestamp for each point (if `generatePerPointTimestamp`=true in
		 * `TGeneratePointCloudParameters`), or empty vector if not populated
		 * (default). */
		std::vector<mrpt::system::TTimeStamp> timestamp;
		/** Original azimuth of each point (if `generatePerPointAzimuth`=true,
		 * empty otherwise ) */
		std::vector<float> azimuth;
		/** Laser ID ("ring number") for each point (0-15 for a VLP-16, etc.) */
		std::vector<int16_t> laser_id;
		/** The list of point indices (in x,y,z,...) generated for each laserID
		 * (ring number). Generated only if `generatePointsForLaserID`=true in
		 * `TGeneratePointCloudParameters`*/
		std::vector<std::vector<uint64_t>> pointsForLaserID;

		inline std::size_t size() const { return x.size(); }
		void reserve(std::size_t n);
		/** Sets all vectors to zero length */
		void clear();
		/** Like clear(), but also enforcing freeing memory */
		void clear_deep();
	};

	/** Optionally, raw data can be converted into a 3D point cloud (local
	 * coordinates wrt the sensor, not the vehicle)
	 * with intensity (graylevel) information. See axes convention in
	 * mrpt::obs::CObservationVelodyneScan \sa generatePointCloud()
	 */
	TPointCloud point_cloud;
	/** @} */

	/** @name Related to conversion to 3D point cloud
	 * @{ */
	struct TGeneratePointCloudParameters
	{
		TGeneratePointCloudParameters();
		/** Minimum azimuth, in degrees (Default=0). Points will be generated
		 * only the the area of interest [minAzimuth, maxAzimuth] */
		double minAzimuth_deg{.0};
		/** Minimum azimuth, in degrees (Default=360). Points will be generated
		 * only the the area of interest [minAzimuth, maxAzimuth] */
		double maxAzimuth_deg{360.};
		/** Minimum (default=1.0f) and maximum (default: Infinity)
		 * distances/ranges for a point to be considered. Points must pass this
		 * (application specific) condition and also the minRange/maxRange
		 * values in CObservationVelodyneScan (sensor-specific). */
		float minDistance{1.0f}, maxDistance{std::numeric_limits<float>::max()};
		/** The limits of the 3D box (default=infinity) in sensor (not vehicle)
		 * local coordinates for the ROI filter \sa filterByROI */
		float ROI_x_min{-std::numeric_limits<float>::max()},
			ROI_x_max{std::numeric_limits<float>::max()},
			ROI_y_min{-std::numeric_limits<float>::max()},
			ROI_y_max{std::numeric_limits<float>::max()},
			ROI_z_min{-std::numeric_limits<float>::max()},
			ROI_z_max{std::numeric_limits<float>::max()};
		/** The limits of the 3D box (default=0) in sensor (not vehicle) local
		 * coordinates for the nROI filter \sa filterBynROI */
		float nROI_x_min{0}, nROI_x_max{0}, nROI_y_min{0}, nROI_y_max{0},
			nROI_z_min{0}, nROI_z_max{0};
		/** (Default:2.0 meters) Minimum distance between a point and its two
		 * neighbors to be considered an invalid point. */
		float isolatedPointsFilterDistance{2.0f};

		/** Enable ROI filter (Default:false): add points inside a given 3D box
		 */
		bool filterByROI{false};
		/** Enable nROI filter (Default:false): do NOT add points inside a given
		 * 3D box */
		bool filterBynROI{false};
		/** (Default:false) Simple filter to remove spurious returns (e.g. Sun
		 * reflected on large water extensions) */
		bool filterOutIsolatedPoints{false};
		/** (Default:true) In VLP16 dual mode, keep both or just one of the
		 * returns. */
		bool dualKeepStrongest{true}, dualKeepLast{true};
		/** (Default:false) If `true`, populate the vector timestamp */
		bool generatePerPointTimestamp{false};
		/** (Default:false) If `true`, populate the vector azimuth */
		bool generatePerPointAzimuth{false};
		/** (Default:false) If `true`, populate pointsForLaserID */
		bool generatePointsForLaserID{false};
	};

	/** Derive from this class to generate pointclouds into custom containers.
	 * \sa generatePointCloud() */
	struct PointCloudStorageWrapper
	{
		virtual void reserve(std::size_t n) {}
		virtual void resizeLaserCount([[maybe_unused]] std::size_t n) {}

		/** Process the insertion of a new (x,y,z) point to the cloud, in
		 * sensor-centric coordinates, with the exact timestamp of that LIDAR
		 * ray */
		virtual void add_point(
			float pt_x, float pt_y, float pt_z, uint8_t pt_intensity,
			const mrpt::system::TTimeStamp& tim, const float azimuth,
			uint16_t laser_id) = 0;
	};

	/** Generates the point cloud into the point cloud data fields in \a
	 * CObservationVelodyneScan::point_cloud
	 * where it is stored in local coordinates wrt the sensor (neither the
	 * vehicle nor the world).
	 * So, this method does not take into account the possible motion of the
	 * sensor through the world as it collects LIDAR scans.
	 * For high dynamics, see the more costly API
	 * generatePointCloudAlongSE3Trajectory()
	 * \note Points with ranges out of [minRange,maxRange] are discarded; as
	 * well, other filters are available in \a params.
	 * \sa generatePointCloudAlongSE3Trajectory(),
	 * TGeneratePointCloudParameters
	 */
	void generatePointCloud(
		const TGeneratePointCloudParameters& params =
			TGeneratePointCloudParameters());

	/** \overload For custom data storage as destination of the pointcloud. */
	void generatePointCloud(
		PointCloudStorageWrapper& dest,
		const TGeneratePointCloudParameters& params =
			TGeneratePointCloudParameters());

	/** Results for generatePointCloudAlongSE3Trajectory() */
	struct TGeneratePointCloudSE3Results
	{
		/** Number of points in the observation */
		size_t num_points{0};
		/** Number of points for which a valid interpolated SE(3) pose could be
		 * determined */
		size_t num_correctly_inserted_points{0};
	};

	/** An alternative to generatePointCloud() for cases where the motion of the
	 * sensor as it grabs one scan (360 deg horiz FOV) cannot be ignored.
	 * \param[in] vehicle_path Timestamped sequence of known poses for the
	 * VEHICLE. Recall that the sensor has a relative pose wrt the vehicle
	 * according to CObservationVelodyneScan::getSensorPose() &
	 * CObservationVelodyneScan::setSensorPose()
	 * \param[out] out_points The generated points, in the same coordinates
	 * frame than \a vehicle_path. Points are APPENDED to the list, so prevous
	 * contents are kept.
	 * \param[out] results_stats Stats
	 * \param[in] params Filtering and other parameters
	 * \sa generatePointCloud(), TGeneratePointCloudParameters
	 */
	void generatePointCloudAlongSE3Trajectory(
		const mrpt::poses::CPose3DInterpolator& vehicle_path,
		std::vector<mrpt::math::TPointXYZIu8>& out_points,
		TGeneratePointCloudSE3Results& results_stats,
		const TGeneratePointCloudParameters& params =
			TGeneratePointCloudParameters());

	/** @} */

	void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
	{
		out_sensorPose = sensorPose;
	}  // See base class docs
	void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
	{
		sensorPose = newSensorPose;
	}  // See base class docs
	void getDescriptionAsText(
		std::ostream& o) const override;  // See base class docs

};	// End of class def.

}  // namespace obs

namespace typemeta
{  // Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(CObservationVelodyneScan, ::mrpt::obs)
}

}  // namespace mrpt
