/* +------------------------------------------------------------------------+
   |                     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.h>  //MRPT_HAS_OPENNI2

/** @file Include this file only from your user code if you have OPENNI2 */

namespace mrpt::hwdrivers
{
class COpenNI2Generic::CDevice
{
   public:
	using Ptr = std::shared_ptr<CDevice>;
	enum
	{
		COLOR_STREAM,
		DEPTH_STREAM,
		IR_STREAM,
		STREAM_TYPE_SIZE  // this last value is to know the number of possible
		// channels, leave it always at the end!
	};
#if MRPT_HAS_OPENNI2
   private:
	class CStream
	{
	   public:
		using Ptr = std::shared_ptr<CStream>;

	   private:
		std::ostream& m_log;
		openni::Device& m_device;
		std::string m_strName;
		openni::SensorType m_type;
		openni::VideoStream m_stream;
		openni::PixelFormat m_format;
		bool m_verbose;

	   public:
		CStream(
			openni::Device& device, openni::SensorType type,
			openni::PixelFormat format, std::ostream& log, bool verbose);
		virtual ~CStream();
		const std::string& getName() const { return m_strName; }
		bool isValid() const;
		bool isMirrorSupported() const;
		bool setMirror(bool flag);
		void setCloseRange(int& value);
		virtual bool open(int w, int h, int fps);
		virtual bool start();
		virtual void destroy();
		virtual bool getFrame(
			openni::VideoFrameRef& frame, mrpt::system::TTimeStamp& timestamp,
			bool& there_is_obs, bool& hardware_error);

		int getFrameWidth() const
		{
			return m_stream.getVideoMode().getResolutionX();
		}
		int getFrameHeight() const
		{
			return m_stream.getVideoMode().getResolutionY();
		}
		double getHFov() const { return m_stream.getHorizontalFieldOfView(); }
		double getFx() const
		{
			return getFrameWidth() / (2.0 * tan(getHFov() / 2.0));
		}
		double getVFov() const { return m_stream.getVerticalFieldOfView(); }
		double getFy() const
		{
			return getFrameHeight() / (2.0 * tan(getVFov() / 2.0));
		}
		double getCx() const { return (getFrameWidth() - 1) * 0.5; }
		double getCy() const { return (getFrameHeight() - 1) * 0.5; }
		void disableAutoExposure()
		{
			m_stream.getCameraSettings()->setAutoExposureEnabled(false);
		}
		void enableAutoExposure()
		{
			m_stream.getCameraSettings()->setAutoExposureEnabled(true);
		}

		void getCameraParam(mrpt::img::TCamera& param) const
		{
			param.ncols = getFrameWidth();
			param.nrows = getFrameHeight();
			param.fx(getFx());
			param.fy(getFy());
			param.cx(getCx());
			param.cy(getCy());
		}

		static Ptr create(
			openni::Device& device, openni::SensorType type,
			openni::PixelFormat format, std::ostream& log, bool verbose);
	};
	openni::DeviceInfo m_info;
	openni::Device m_device;
	CStream::Ptr m_streams[STREAM_TYPE_SIZE];
	bool m_mirror;
	std::stringstream m_log;
	bool m_verbose;

	bool synchMirrorMode();
	bool startStreams();

	inline void resize(mrpt::img::CImage& rgb, int w, int h)
	{
		rgb.resize(w, h, mrpt::img::CH_RGB);
	}
	inline void resize(mrpt::math::CMatrix_u16& depth, int w, int h)
	{
		depth.resize(h, w);
	}
	inline void resize(mrpt::obs::CObservation3DRangeScan& obs, int w, int h)
	{
		resize(obs.intensityImage, w, h);
		obs.rangeImage_setSize(h, w);
	}

	inline void setPixel(
		const openni::RGB888Pixel& src, mrpt::img::CImage& rgb, int x, int y)
	{
		rgb.setPixel(x, y, (src.r << 16) + (src.g << 8) + src.b);
	}
	inline void setPixel(
		const openni::DepthPixel& src, mrpt::math::CMatrix_u16& depth_mm, int x,
		int y)
	{
		depth_mm(y, x) = src;
	}

	template <class NI_PIXEL, class MRPT_DATA>
	void copyRow(const char* src, MRPT_DATA& rgb, int w, const int y)
	{
		const auto* s = (const NI_PIXEL*)src;
		for (int xc = 0; xc < w; ++xc, ++s)
		{
			int x = xc;
			if (isMirrorMode()) { x = w - xc - 1; }
			setPixel(*s, rgb, x, y);
		}
	}

	template <class NI_PIXEL, class MRPT_DATA>
	void copyFrame(openni::VideoFrameRef& frame, MRPT_DATA& dst)
	{
		const char* data = (const char*)frame.getData();
		const int stride = frame.getStrideInBytes();
		const int width = frame.getWidth();
		const int height = frame.getHeight();
		resize(dst, width, height);
		for (int y = 0; y < height; ++y, data += stride)
		{
			copyRow<NI_PIXEL, MRPT_DATA>(data, dst, width, y);
		}
	}

   public:
	CDevice(
		const openni::DeviceInfo& info, openni::PixelFormat rgb,
		openni::PixelFormat depth, bool m_verbose);
	virtual ~CDevice();

	const openni::DeviceInfo& getInfo() const { return m_info; }
	std::string getLog() const { return m_log.str(); }
	void clearLog()
	{
		m_log.str("");
		m_log.clear();
	}
	bool isMirrorMode() const { return m_mirror; }
	void setMirrorMode(bool mode) { m_mirror = mode; }
	bool hasColor() const
	{
		if (!m_streams[COLOR_STREAM]) return false;
		else
			return m_streams[COLOR_STREAM]->isValid();
	}
	bool hasDepth() const
	{
		if (!m_streams[DEPTH_STREAM]) return false;
		else
			return m_streams[DEPTH_STREAM]->isValid();
	}

	bool isOpen() const;
	void close();
	bool open(int w, int h, int fps);

	bool getNextFrameRGB(
		mrpt::img::CImage& img, mrpt::system::TTimeStamp& timestamp,
		bool& there_is_obs, bool& hardware_error);
	bool getNextFrameD(
		mrpt::math::CMatrix_u16& depth_mm, mrpt::system::TTimeStamp& timestamp,
		bool& there_is_obs, bool& hardware_error);
	bool getNextFrameRGBD(
		mrpt::obs::CObservation3DRangeScan& obs, bool& there_is_obs,
		bool& hardware_error);

	bool getCameraParam(int streamType, mrpt::img::TCamera& param) const
	{
		if (streamType < 0 || streamType >= STREAM_TYPE_SIZE) { return false; }
		if (!m_streams[streamType] || m_streams[streamType]->isValid() == false)
		{ return false; }
		m_streams[streamType]->getCameraParam(param);
		return true;
	}

	bool getSerialNumber(unsigned int& sn);

	static Ptr create(
		const openni::DeviceInfo& info, openni::PixelFormat rgb,
		openni::PixelFormat depth, bool verbose);

	openni::Device& getDevicePtr() { return m_device; }

   private:
	bool getSerialNumber(std::string& sn);

#endif	// MRPT_HAS_OPENNI2
};
}  // namespace mrpt::hwdrivers
