#ifndef __SCAN__GRAPHER___H
#define __SCAN__GRAPHER___H

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "Eigen/Dense"
#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include <ros/ros.h>
#include "TaskQueue/TQInterface.h"
#include "TaskQueue/TaskPool.h"
#include "TaskQueue/BaseTask.h"

#include <mutex>
#include <string>
#include <queue>
#include <boost/thread.hpp>
#include "steger/Steger.hpp"

typedef struct GRAPH_FRAME
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
    pcl::PointXYZ                   origin;
}Frame;

class ScanGrapher
{
public:
    std::mutex                      mutex_;
    pcl::PointCloud<pcl::PointXYZ>  cloud_;
    cv::Mat                         grid_;
    cv::Mat                         pixel_count_;
    pcl::PointXYZ                   min_point_;
    pcl::PointXYZ                   max_point_;
    double                          resolution_;
    boost::thread*                  thread_  ; 
    std::queue<Frame>               scan_queue_;
    bool                            bExit_;
public:
    ScanGrapher(pcl::PointXYZ min,pcl::PointXYZ max,double resolution);
    ~ScanGrapher();
    void pushScan(pcl::PointCloud<pcl::PointXYZ>::Ptr input,pcl::PointXYZ origin);
    
    cv::Mat getGrid();
    pcl::PointCloud<pcl::PointXYZ>::Ptr getCloud();
    void getCloud(pcl::PointCloud<pcl::PointXYZ>& cloud);
    void saveImage(std::string file);
    void saveCloudTxt(std::string file);
    void saveCloudPcd(std::string file);
protected:
    static bool compareAsc(const pcl::PointXYZ& pt1,const pcl::PointXYZ& pt2);
    void update(pcl::PointCloud<pcl::PointXYZ>& input,pcl::PointXYZ origin);
    void update(pcl::PointCloud<pcl::PointXYZ>& input,pcl::PointXYZ origin,double delta);
    void updateCell(int idx,int idy,double prob_occ);
    static void thread_updata(ScanGrapher* p);
    static void thread_gen_cloud(ScanGrapher* p);


};

#endif