#include "stdafx.h"
#include <array>
#include <string>

// Disable Error C4996 that occur when using Boost.Signals2.
#ifdef _DEBUG
#define _SCL_SECURE_NO_WARNINGS
#endif

#include "kinect2_grabber.h"
#include <pcl/visualization/pcl_visualizer.h>


boost::mutex mutex;

int _tmain( int argc, _TCHAR* argv[] )
{
	std::array<std::shared_ptr<pcl::visualization::PCLVisualizer>, 2> viewer;
	for( int count = 0; count < viewer.size(); count++ ){
		viewer[count] = std::make_shared<pcl::visualization::PCLVisualizer>();
		std::string name = "Viewer" + std::to_string( count + 1 );
		viewer[count]->setWindowName( name.c_str() );
	}

	pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud;
	boost::function<void( const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& )> function =
		[&cloud]( const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &ptr ){
		boost::mutex::scoped_lock lock( mutex );
		cloud = ptr;
	};

	pcl::Grabber* grabber = new pcl::Kinect2Grabber();

	boost::signals2::connection connection = grabber->registerCallback( function );

	grabber->start();

	bool stop = false;
	while( 1 ){
		for( int count = 0; count < viewer.size(); count++ ){
			if( viewer[count]->wasStopped() ){
				stop = true;
				break;
			}

			viewer[count]->spinOnce();

			if( cloud ){
				if( !viewer[count]->updatePointCloud( cloud, "cloud" ) ){
					viewer[count]->addPointCloud( cloud, "cloud" );
					viewer[count]->resetCameraViewpoint( "cloud" );
				}
			}
		}

		if( GetKeyState( VK_ESCAPE ) < 0 || stop ){
			break;
		}
	}

	grabber->stop();

	return 0;
}