#include "pcl/apps/3d_rec_framework/tools/openni_frame_source.h"
#include <pcl/io/pcd_io.h>
#include <pcl/memory.h>

namespace OpenNIFrameSource
{

  OpenNIFrameSource::OpenNIFrameSource (const std::string& device_id) :
    grabber_ (device_id), frame_counter_ (0), active_ (true)
  {
    std::function<void
    (const PointCloudConstPtr&)> frame_cb = [this] (const PointCloudConstPtr& cloud){ onNewFrame (cloud); };
    grabber_.registerCallback (frame_cb);
    grabber_.start ();
  }

  OpenNIFrameSource::~OpenNIFrameSource ()
  {
    // Stop the grabber when shutting down
    grabber_.stop ();
  }

  bool
  OpenNIFrameSource::isActive () const
  {
    return active_;
  }

  const PointCloudPtr
  OpenNIFrameSource::snap ()
  {
    return (most_recent_frame_);
  }

  void
  OpenNIFrameSource::onNewFrame (const PointCloudConstPtr &cloud)
  {
    mutex_.lock ();
    ++frame_counter_;
    most_recent_frame_ = pcl::make_shared<PointCloud> (*cloud); // Make a copy of the frame
    mutex_.unlock ();
  }

  void
  OpenNIFrameSource::onKeyboardEvent (const pcl::visualization::KeyboardEvent & event)
  {
    // When the spacebar is pressed, trigger a frame capture
    mutex_.lock ();
    if (event.keyDown () && event.getKeySym () == "e")
    {
      active_ = false;
    }
    mutex_.unlock ();
  }

}
