/* +------------------------------------------------------------------------+
   |                     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/gui/CDisplayWindow3D.h>
#include <mrpt/hwdrivers/CGenericSensor.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/COutputLogger.h>

namespace mrpt::hwdrivers
{
/** This is the base, abstract class for "software drivers" interfaces to 2D
 * scanners (laser range finders).
 *  Physical devices may be interfaced through a serial port, a USB
 * connection,etc. but this class
 *   abstract those details throught the "binding" of the specific scanner
 * driver to a given I/O channel,
 *   which must be set by calling "hwdrivers::C2DRangeFinderAbstract::bindIO".
 * See also the derived classes.
 *
 *  There is support for "exclusion polygons", areas where points, if detected,
 * should be marked as invalid.
 *  Those areas are useful in cases where the scanner always detects part of
 * the vehicle itself, and those
 *   points want to be ignored (see
 * C2DRangeFinderAbstract::loadExclusionAreas).
 *
 * \sa comms::CSerialPort
 * \ingroup mrpt_hwdrivers_grp
 */
class C2DRangeFinderAbstract : public mrpt::system::COutputLogger,
							   public mrpt::hwdrivers::CGenericSensor
{
   private:
	mrpt::obs::CObservation2DRangeScan m_lastObservation;
	bool m_lastObservationIsNew{false};
	bool m_hardwareError{false};

	/** For being thread-safe.
	 */
	std::mutex m_csChangeStream, m_csLastObservation;

	/** A dynamic object used as buffer in doProcess */
	mrpt::obs::CObservation2DRangeScan::Ptr m_nextObservation;

	/** A list of optional exclusion polygons, in coordinates relative to the
	 * vehicle, that is, taking into account the "sensorPose". */
	mrpt::obs::CObservation2DRangeScan::TListExclusionAreasWithRanges
		m_lstExclusionPolys;
	/** A list of pairs of angles <init,end> such as all sensor ranges falling
	 * in those forbiden angles will be marked as invalid. */
	std::vector<std::pair<double, double>> m_lstExclusionAngles;

	/** If true, shows a 3D window with a preview of the grabber data */
	bool m_showPreview{false};
	mrpt::gui::CDisplayWindow3D::Ptr m_win;

   protected:
	/** The I/O channel (will be nullptr if not bound). */
	std::shared_ptr<mrpt::io::CStream> m_stream;

	/** Should be call by derived classes at "loadConfig" (loads exclusion areas
	 *AND exclusion angles).
	 *  This loads a sequence of vertices of a polygon given by its (x,y)
	 *coordinates relative to the vehicle, that is, taking into account the
	 *"sensorPose".
	 *   - exclusionZone%u_x
	 *   - exclusionZone%u_y
	 *   for %u=1,2,3,...
	 *   All points within the 2D polygon will be ignored, for any Z, unless an
	 *optional entry is found:
	 *   - exclusionZone%u_z=[z_min z_max]
	 *	  In that case, only the points within the 2D polygon AND the given
	 *range in Z will be ignored.
	 *
	 *  The number of zones is variable, but they must start at 1 and be
	 *consecutive.
	 *
	 * This also loads any other common params (e.g. 'preview')
	 * \sa filterByExclusionAreas
	 */
	void loadCommonParams(
		const mrpt::config::CConfigFileBase& configSource,
		const std::string& iniSection);

	/** Mark as invalid those points which (x,y) coordinates fall within the
	 * exclusion polygons.
	 * \sa loadExclusionAreas
	 */
	void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan& obs) const;

	/** Mark as invalid those ranges in a set of forbiden angle ranges.
	 * \sa loadExclusionAreas
	 */
	void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan& obs) const;

	/** Must be called inside the capture method to allow optional GUI preview
	 * of scans */
	void processPreview(const mrpt::obs::CObservation2DRangeScan& obs);

   public:
	/** Default constructor */
	C2DRangeFinderAbstract();
	/** Destructor */
	~C2DRangeFinderAbstract() override;

	/** Enables GUI visualization in real-time */
	void showPreview(bool enable = true) { m_showPreview = enable; }
	/** Binds the object to a given I/O channel.
	 *  The stream object must not be deleted before the destruction of this
	 * class.
	 * \sa comms::CSerialPort
	 */
	void bindIO(const std::shared_ptr<mrpt::io::CStream>& streamIO);

	/** Get the last observation from the sensor, if available, and unmarks it
	 * as being "the last one" (thus a new scan must arrive or subsequent calls
	 * will find no new observations).
	 */
	void getObservation(
		bool& outThereIsObservation,
		mrpt::obs::CObservation2DRangeScan& outObservation,
		bool& hardwareError);

	/** Main method for a CGenericSensor */
	void doProcess() override;

	/** Specific laser scanner "software drivers" must process here new data
	 * from the I/O stream, and, if a whole scan has arrived, return it.
	 *  This method MUST BE CALLED in a timely fashion by the user to allow the
	 * proccessing of incoming data. It can be run in a different thread safely.
	 */
	virtual void doProcessSimple(
		bool& outThereIsObservation,
		mrpt::obs::CObservation2DRangeScan& outObservation,
		bool& hardwareError) = 0;

	/** Enables the scanning mode (which may depend on the specific laser
	 * device); this must be called before asking for observations to assure
	 * that the protocol has been initializated.
	 * \return If everything works "true", or "false" if there is any error.
	 */
	virtual bool turnOn() = 0;

	/** Disables the scanning mode (this can be used to turn the device in low
	 * energy mode, if available)
	 * \return If everything works "true", or "false" if there is any error.
	 */
	virtual bool turnOff() = 0;

	/** Returns the empirical, filtered estimation for the period at which whole
	 * scans are being returned from calls to doProcessSimple()
	 * \note:  Units: seconds */
	double getEstimatedScanPeriod() const { return m_estimated_scan_period; }

   protected:
	/** Must be called from doProcessSimple() implementations */
	void internal_notifyGoodScanNow();

	/** Must be called from doProcessSimple() implementations.
	 * Returns true if ok, false if this seems strange and should
	 * return an error condition to the user. */
	bool internal_notifyNoScanReceived();

   private:
	/// Used in internal_notifyGoodScanNow()
	mrpt::system::TTimeStamp m_last_good_scan{INVALID_TIMESTAMP};
	/// Updated in internal_notifyGoodScanNow()
	double m_estimated_scan_period{1.0};
	/// Used in internal_notifyNoScanReceived()
	int m_failure_waiting_scan_counter{0}, m_max_missed_scan_failures{10};

};	// End of class
}  // namespace mrpt::hwdrivers
