/* +------------------------------------------------------------------------+
   |                     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/config/CConfigFileBase.h>
#include <mrpt/hwdrivers/CGenericSensor.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationVelodyneScan.h>
#include <mrpt/typemeta/TEnumType.h>

namespace mrpt::hwdrivers
{
/** A C++ interface to Velodyne laser scanners (HDL-64, HDL-32, VLP-16), working
 * on Linux and Windows.
 * (Using this class requires WinPCap as a run-time dependency in Windows).
 * It can receive data from real devices via an Ethernet connection or parse a
 * WireShark PCAP file for offline processing.
 * The choice of online vs. offline operation is taken upon calling \a
 * initialize(): if a PCAP input file has been defined,
 * offline operation takes place and network is not listened for incomming
 * packets.
 *
 * Parsing dual return scans requires a VLP-16 with firmware version 3.0.23 or
 * newer. While converting the scan into a
 * point cloud in mrpt::obs::CObservationVelodyneScan you can select whether to
 * keep the strongest, the last or both laser returns.
 *
 * XML calibration files are not mandatory for VLP-16 and HDL-32, but they are
 * for HDL-64.
 *
 * <h2>Grabbing live data (as a user)</h2> <hr>
 *  - Use the application
 * [velodyne-view](http://www.mrpt.org/list-of-mrpt-apps/application-velodyne-view/)
 * to visualize the LIDAR output in real-time (optionally saving to a PCAP file)
 * or to playback a PCAP file.
 *  - Use
 * [rawlog-grabber](http://www.mrpt.org/list-of-mrpt-apps/application-rawlog-grabber/)
 * to record a dataset in MRPT's format together with any other set of sensors.
 * See example config file:
 * [MRPT\share\mrpt\config_files\rawlog-grabber\velodyne.ini](https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/rawlog-grabber/velodyne.ini)
 *
 * <h2>Grabbing live data (programmatically)</h2> <hr>
 *  - See CGenericSensor for a general overview of the sequence of methods to
 * be called: loadConfig(), initialize(), doProcess().
 *  - Or use this class inside the application
 * [rawlog-grabber](http://www.mrpt.org/list-of-mrpt-apps/application-rawlog-grabber/).
 * See example config files:
 * [MRPT\share\mrpt\config_files\rawlog-grabber\velodyne.ini](https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/rawlog-grabber/velodyne.ini)
 *
 * See the source code of the example application `[MRPT]/apps/velodyne-view`
 * ([velodyne-view web
 * page](http://www.mrpt.org/list-of-mrpt-apps/application-velodyne-view/)) for
 * more details.
 *
 * <h2>Playing back a PCAP file:</h2><hr>
 *  It is common to save Velodyne datasets as Wireshark's PCAP files.
 *  These files can be played back with tools like
 * [bittwist](http://bittwist.sourceforge.net/), which emit all UDP packets in
 * the PCAP log.
 *  Then, use this class to receive the packets as if they come from the real
 * sensor.
 *
 *  Alternatively, if MRPT is linked against libpcap, this class can directly
 * parse a PCAP file to simulate reading from a device offline.
 *  See method setPCAPInputFile() and config file parameter ``
 *
 *  To compile with PCAP support: In Debian/Ubuntu, install libpcap-dev. In
 * Windows, install WinPCap developer packages + the regular WinPCap driver.
 *
 *  <h2>Configuration and usage:</h2> <hr>
 * Data is returned as observations of type:
 *  - mrpt::obs::CObservationVelodyneScan for one or more "data packets" (refer
 * to Velodyne usage manual)
 *  - mrpt::obs::CObservationGPS for GPS (GPRMC) packets, if available via the
 * synchronization interface of the device.
 *  See those classes for documentation on their fields.
 *
 * Configuration includes setting the device IP (optional) and sensor model
 * (mandatory only if a calibration file is not provided).
 * These parameters can be set programmatically (see methods of this class), or
 * via a configuration file with CGenericSensor::loadConfig() (see example
 * config file section below).
 *
 * <h2>About timestamps:</h2><hr>
 *  Each gathered observation of type mrpt::obs::CObservationVelodyneScan is
 * populated with two timestamps, one for the local PC timestamp and,
 *  if available, another one for the GPS-stamped timestamp. Refer to the
 * observation docs for details.
 *
 * <h2>Format of parameters for loading from a .ini file</h2><hr>
 *  \code
 *  PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
 * -------------------------------------------------------
 *   [supplied_section_name]
 *   # ---- Sensor description ----
 *   #calibration_file         = PUT_HERE_FULL_PATH_TO_CALIB_FILE.xml      //
 * Optional but recommended: put here your vendor-provided calibration file
 *   model                    = VLP16      // Can be any of: `VLP16`, `HDL32`,
 * `HDL64`  (It is used to load default calibration file. Parameter not required
 * if `calibration_file` is provided.
 *   #pos_packets_min_period  = 0.5        // (Default=0.5 seconds) Minimum
 * period to leave between reporting position packets. Used to decimate the
 * large number of packets of this type.
 *   # How long to wait, after loss of GPS signal, to report timestamps as "not
 * based on satellite time". 30 secs, with typical velodyne clock drifts, means
 * a ~1.7 ms typical drift.
 *   #pos_packets_timing_timeout = 30      // (Default=30 seconds)
 *   # ---- Online operation ----
 *
 *   # IP address of the device. UDP packets from other IPs will be ignored.
 * Leave commented or blank
 *   # if only one scanner is present (no IP filtering)
 *   #device_ip       = XXX.XXX.XXX.XXX
 *
 *   #rpm             = 600        // Sensor RPM (Default: unchanged). Requires
 * setting `device_ip`
 *   #return_type     = STRONGEST  // Any of: 'STRONGEST', 'LAST', 'DUAL'
 * (Default: unchanged). Requires setting `device_ip`
 *
 *   # ---- Offline operation ----
 *   # If uncommented, this class will read from the PCAP instead of connecting
 * and listeling
 *   # for online network packets.
 *   # pcap_input     = PUT_FULL_PATH_TO_PCAP_LOG_FILE.pcap
 *   # pcap_read_once = false   // Do not loop
 *   # pcap_read_fast = false    // fast forward skipping non-velodyne packets
 *   # pcap_read_full_scan_delay_ms = 100 // Used to simulate a reasonable
 * number of full scans / second
 *   # pcap_repeat_delay = 0.0   // seconds
 *
 *   # ---- Save to PCAP file ----
 *   # If uncommented, a PCAP file named
 * `[pcap_output_prefix]_[DATE_TIME].pcap` will be
 *   # written simultaneously to the normal operation of this class.
 *   # pcap_output     = velodyne_log
 *
 *   # 3D position of the sensor on the vehicle:
 *   pose_x     = 0    // 3D position (meters)
 *   pose_y     = 0
 *   pose_z     = 0
 *   pose_yaw   = 0    // 3D orientation (degrees)
 *   pose_pitch = 0
 *   pose_roll  = 0
 *
 *  \endcode
 *
 *
 * <h2>Copyright notice</h2><hr>
 * Portions of this class are based on code from velodyne ROS node in
 * https://github.com/ros-drivers/velodyne
 *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
 *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
 *  License: Modified BSD Software License Agreement
 *
 * \note New in MRPT 1.4.0
 * \ingroup mrpt_hwdrivers_grp
 */
class CVelodyneScanner : public mrpt::hwdrivers::CGenericSensor
{
	DEFINE_GENERIC_SENSOR(CVelodyneScanner)
   public:
	/** Default: 2368. Change it if required. */
	static short int VELODYNE_DATA_UDP_PORT;
	/** Default: 8308. Change it if required. */
	static short int VELODYNE_POSITION_UDP_PORT;

	/** LIDAR model types */
	enum model_t
	{
		VLP16 = 1,
		HDL32 = 2,
		HDL64 = 3
	};

	/** LIDAR return type */
	enum return_type_t
	{
		UNCHANGED = 0,
		STRONGEST,
		LAST,
		DUAL
	};

	/** Hard-wired properties of LIDARs depending on the model */
	struct TModelProperties
	{
		double maxRange;
	};
	using model_properties_list_t = std::map<model_t, TModelProperties>;
	/** Access to default sets of parameters for Velodyne LIDARs */
	struct TModelPropertiesFactory
	{
		/** Singleton access */
		static const model_properties_list_t& get();
		/** Return human-readable string: "`VLP16`,`XXX`,..." */
		static std::string getListKnownModels();
	};

   protected:
	bool m_initialized;
	/** Default: "VLP16" */
	model_t m_model{CVelodyneScanner::VLP16};
	/** Default: 0.5 seconds */
	double m_pos_packets_min_period{0.5};
	/** Default: 30 seconds */
	double m_pos_packets_timing_timeout{30.0};
	/** Default: "" (no IP-based filtering) */
	std::string m_device_ip;
	/** Default: true Output whole frames and not data packets  */
	bool m_return_frames{true};
	/** Default: true Output PCAP Info msgs */
	bool m_pcap_verbose{true};
	/** Default: "" (do not operate from an offline file) */
	std::string m_pcap_input_file;
	/** Default: "" (do not dump to an offline file) */
	std::string m_pcap_output_file;
	mrpt::poses::CPose3D m_sensorPose;
	/** Device calibration file (supplied by vendor in an XML file) */
	mrpt::obs::VelodyneCalibration m_velodyne_calib;
	mrpt::system::TTimeStamp m_last_pos_packet_timestamp;

	// offline operation:
	/** opaque ptr: "pcap_t*" */
	void* m_pcap{nullptr};
	/** opaque ptr: "pcap_t*" */
	void* m_pcap_out{nullptr};
	/** opaque ptr: "pcap_dumper_t *" */
	void* m_pcap_dumper{nullptr};
	/** opaque ptr: bpf_program* */
	void* m_pcap_bpf_program{nullptr};
	bool m_pcap_file_empty{true};
	/** number of pkts read from the file so far (for debugging) */
	unsigned int m_pcap_read_count;
	/** Default: false */
	bool m_pcap_read_once{false};
	/** (Default: false) If false, will use m_pcap_read_full_scan_delay_ms */
	bool m_pcap_read_fast{false};
	/** (Default:100 ms) delay after each full scan read from a PCAP log */
	double m_pcap_read_full_scan_delay_ms{100};
	/** Default: 0 (in seconds) */
	double m_pcap_repeat_delay{0.0};

	/** See the class documentation at the top for expected parameters */
	void loadConfig_sensorSpecific(
		const mrpt::config::CConfigFileBase& configSource,
		const std::string& section) override;

   public:
	CVelodyneScanner();
	~CVelodyneScanner() override;

	/** @name Change configuration parameters; to be called BEFORE initialize();
	 * see above for the list of parameters and their meaning
	 * @{ */
	/** See supported model names in the general discussion docs for
	 * mrpt::hwdrivers::CVelodyneScanner */
	void setModelName(const model_t model) { m_model = model; }
	model_t getModelName() const { return m_model; }
	/** Set the minimum period between the generation of
	 * mrpt::obs::CObservationGPS observations from Velodyne Position RMC GPS
	 * packets */
	void setPosPacketsMinPeriod(double period_seconds)
	{
		m_pos_packets_min_period = period_seconds;
	}
	double getPosPacketsMinPeriod() const { return m_pos_packets_min_period; }
	/** Set how long to wait, after loss of GPS signal, to report timestamps as
	 * "not based on satellite time". 30 secs, with typical velodyne clock
	 * drifts, means a ~1.7 ms typical drift. */
	void setPosPacketsTimingTimeout(double timeout)
	{
		m_pos_packets_timing_timeout = timeout;
	}
	double getPosPacketsTimingTimeout() const
	{
		return m_pos_packets_timing_timeout;
	}

	/** UDP packets from other IPs will be ignored. Default: empty string, means
	 * do not filter by IP */
	void setDeviceIP(const std::string& ip) { m_device_ip = ip; }
	const std::string& getDeviceIP() const { return m_device_ip; }
	/** Enables/disables PCAP info messages to console (default: true) */
	void setPCAPVerbosity(const bool verbose) { m_pcap_verbose = verbose; }
	/** Enables reading from a PCAP file instead of live UDP packet listening */
	void setPCAPInputFile(const std::string& pcap_file)
	{
		m_pcap_input_file = pcap_file;
	}
	const std::string& getPCAPInputFile() const { return m_pcap_input_file; }
	/** Enables dumping to a PCAP file in parallel to returning regular MRPT
	 * objects. Default="": no pcap log. */
	void setPCAPOutputFile(const std::string& out_pcap_file)
	{
		m_pcap_output_file = out_pcap_file;
	}
	const std::string& getPCAPOutputFile() const { return m_pcap_output_file; }
	void setPCAPInputFileReadOnce(bool read_once)
	{
		m_pcap_read_once = read_once;
	}
	bool getPCAPInputFileReadOnce() const { return m_pcap_read_once; }
	const mrpt::obs::VelodyneCalibration& getCalibration() const
	{
		return m_velodyne_calib;
	}
	void setCalibration(const mrpt::obs::VelodyneCalibration& calib)
	{
		m_velodyne_calib = calib;
	}
	/** Returns false on error. \sa
	 * mrpt::obs::VelodyneCalibration::loadFromXMLFile() */
	bool loadCalibrationFile(const std::string& velodyne_xml_calib_file_path);

	/** Changes among STRONGEST, LAST, DUAL return types (via HTTP post
	 * interface).
	 * Can be called at any instant, before or after initialize().
	 * Requires setting a device IP address.
	 * \return false on error */
	bool setLidarReturnType(return_type_t ret_type);

	/** Changes Lidar RPM (valid range: 300-600) (via HTTP post interface).
	 * Can be called at any instant, before or after initialize().
	 * Requires setting a device IP address.
	 * \return false on error*/
	bool setLidarRPM(int rpm);

	/** Switches the LASER on/off (saves energy when not measuring) (via HTTP
	 * post interface).
	 * Can be called at any instant, before or after initialize().
	 * Requires setting a device IP address.
	 * \return false on error*/
	bool setLidarOnOff(bool on);

	/** Switches whole frame (points in a single revolution) on/off publication
	 * to data packet publication. When on, getNextObservation() will return
	 * true whenever a frame is avaliable, when off, getNextObservation() will
	 * return true whenever a data packet is avaliable. The default is on. When
	 * listening to data packets on a PCAP, pcap_read_fast is enforced.
	 */
	void setFramePublishing(bool on);

	/** @} */

	/** Polls the UDP port for incoming data packets. The user *must* call this
	 * method in a timely fashion to grab data as it it generated by the device.
	 *  The minimum call rate should be the expected number of data
	 * packets/second (!=scans/second). Checkout Velodyne user manual if in
	 * doubt.
	 *
	 * \param[out] outScan Upon return, an empty smart pointer will be found
	 * here if no new data was available. Otherwise, a valid scan.
	 * \param[out] outGPS  Upon return, an empty smart pointer will be found
	 * here if no new GPS data was available. Otherwise, a valid GPS reading.
	 * \return true if no error ocurred (even if there was no new observation).
	 * false if any communication error occurred.
	 */
	bool getNextObservation(
		mrpt::obs::CObservationVelodyneScan::Ptr& outScan,
		mrpt::obs::CObservationGPS::Ptr& outGPS);

	// See docs in parent class
	void doProcess() override;

	/** Tries to initialize the sensor driver, after setting all the parameters
	 * with a call to loadConfig.
	 * Velodyne specifics: this method sets up the UDP listening sockets, so
	 * all relevant params MUST BE SET BEFORE calling this.
	 *  \exception This method must throw an exception with a descriptive
	 * message if some critical error is found.
	 */
	void initialize() override;

	/** Close the UDP sockets set-up in \a initialize(). This is called
	 * automatically upon destruction */
	void close();

	/** Users normally would prefer calling \a getNextObservation() instead.
	 * This method polls the UDP data port and returns one Velodyne DATA packet
	 * (1206 bytes) and/or one POSITION packet. Refer to Velodyne users manual.
	 * Approximate timestamps (based on this computer clock) are returned for
	 * each kind of packets, or INVALID_TIMESTAMP if timeout ocurred waiting for
	 * a packet.
	 * \return true on all ok. false only for pcap reading EOF
	 */
	bool receivePackets(
		mrpt::system::TTimeStamp& data_pkt_timestamp,
		mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket& out_data_pkt,
		mrpt::system::TTimeStamp& pos_pkt_timestamp,
		mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket&
			out_pos_pkt);

   private:
	/** Handles for the UDP sockets, or INVALID_SOCKET (-1) */
	using platform_socket_t =
#ifdef _WIN32
#if MRPT_WORD_SIZE == 64
		uint64_t
#else
		uint32_t
#endif
#else
		int
#endif
		;

	platform_socket_t m_hDataSock, m_hPositionSock;

	static mrpt::system::TTimeStamp internal_receive_UDP_packet(
		platform_socket_t hSocket, uint8_t* out_buffer,
		const size_t expected_packet_size,
		const std::string& filter_only_from_IP);

	bool internal_read_PCAP_packet(
		mrpt::system::TTimeStamp& data_pkt_time, uint8_t* out_data_buffer,
		mrpt::system::TTimeStamp& pos_pkt_time, uint8_t* out_pos_buffer);

	/** In progress RX scan */
	mrpt::obs::CObservationVelodyneScan::Ptr m_rx_scan;

	mrpt::obs::gnss::Message_NMEA_RMC m_last_gps_rmc;
	mrpt::system::TTimeStamp m_last_gps_rmc_age;
	int m_lidar_rpm{0};
	return_type_t m_lidar_return{UNCHANGED};

	bool internal_send_http_post(const std::string& post_data);

};	// end of class
}  // namespace mrpt::hwdrivers
MRPT_ENUM_TYPE_BEGIN(mrpt::hwdrivers::CVelodyneScanner::model_t)
using namespace mrpt::hwdrivers;
MRPT_FILL_ENUM_MEMBER(CVelodyneScanner, VLP16);
MRPT_FILL_ENUM_MEMBER(CVelodyneScanner, HDL32);
MRPT_FILL_ENUM_MEMBER(CVelodyneScanner, HDL64);
MRPT_ENUM_TYPE_END()

MRPT_ENUM_TYPE_BEGIN(mrpt::hwdrivers::CVelodyneScanner::return_type_t)
using namespace mrpt::hwdrivers;
MRPT_FILL_ENUM_MEMBER(CVelodyneScanner, UNCHANGED);
MRPT_FILL_ENUM_MEMBER(CVelodyneScanner, STRONGEST);
MRPT_FILL_ENUM_MEMBER(CVelodyneScanner, LAST);
MRPT_FILL_ENUM_MEMBER(CVelodyneScanner, DUAL);
MRPT_ENUM_TYPE_END()
