#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/common/time.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/people/ground_based_people_detection_app.h>

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;


class Algorithm
{
    pcl::visualization::PCLVisualizer *viewer;
    boost::mutex cloud_mutex;
    enum { COLS = 640, ROWS = 480 };

    void cloud_cb_(const PointCloudT::ConstPtr &callback_cloud, PointCloudT::Ptr& cloud,
        bool* new_cloud_available_flag)
    {
        cloud_mutex.lock();    // for not overwriting the point cloud from another thread
        *cloud = *callback_cloud;
        *new_cloud_available_flag = true;
        cloud_mutex.unlock();
    }

    struct callback_args{
        // structure used to pass arguments to the callback function
        PointCloudT::Ptr clicked_points_3d;
        pcl::visualization::PCLVisualizer::Ptr viewerPtr;
    };

    void
        pp_callback(const pcl::visualization::PointPickingEvent& event, void* args)
    {
            struct callback_args* data = (struct callback_args *)args;
            if (event.getPointIndex() == -1)
                return;
            PointT current_point;
            event.getPoint(current_point.x, current_point.y, current_point.z);
            data->clicked_points_3d->points.push_back(current_point);
            // Draw clicked points in red:
            pcl::visualization::PointCloudColorHandlerCustom<PointT> red(data->clicked_points_3d, 255, 0, 0);
            data->viewerPtr->removePointCloud("clicked_points");
            data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
            data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
            std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;
    }

    public:
    Algorithm(pcl::visualization::PCLVisualizer *viewer2){
        viewer = viewer2;
    };

    void play()
    {
        std::string svm_filename = "trainedLinearSVMForPeopleDetectionWithHOG.yaml";
        float min_confidence = -1.5;
        float min_height = 1.3;
        float max_height = 2.3;
        float voxel_size = 0.06;
        Eigen::Matrix3f rgb_intrinsics_matrix;
        rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics

        PointCloudT::Ptr cloud(new PointCloudT);
        bool new_cloud_available_flag = false;
        pcl::Grabber* interface = new pcl::OpenNIGrabber("sample-pokoj.oni");
        boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind(&cloud_cb_, _1, cloud, &new_cloud_available_flag);
        //interface->registerCallback(f);
        //interface->start();
    }
};

int main(int argc, char** argv)
{
    int a;
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    Algorithm obiekt(&viewer);
    obiekt.play();
    cin >> a;
    return 0;
}