/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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
