#include "scanGrapher.h"
#include <opencv2/opencv.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <chrono> 

ScanGrapher::ScanGrapher(pcl::PointXYZ minp,pcl::PointXYZ maxp,double resolution)
    :bExit_(false)
{
    if(resolution<=0)
    {
        std::cout<<" resolution <= 0, change to 0.01 "<<std::endl;
        resolution=0.01;
    }
    int size_x=int((maxp.x-minp.x)/resolution);
    int size_y=int((maxp.y-minp.y)/resolution);

    if(size_x<=0 || size_y<=0)
    {
        std::cout<<"min max point set error , set map [-5  5] resolution 0.01 "<<std::endl;
        minp=pcl::PointXYZ(-5,-5,0);
        maxp=pcl::PointXYZ(5,5,0);
        resolution=0.01;
        size_x=1000;
        size_y=1000;
    }
    std::cout<<" grapher grid size : "<<size_x<<" , "<<size_y<<std::endl;
    grid_=cv::Mat::zeros(size_y,size_x,CV_8UC1);
    pixel_count_=cv::Mat::zeros(size_y,size_x,CV_32FC1);
    
    min_point_=minp;
    max_point_=maxp;
    resolution_=resolution;
    thread_=new boost::thread(boost::bind(ScanGrapher::thread_updata,this));
    if(Steger::initCuda()==false)
    {
        std::cout<<" cuda init failed..."<<std::endl;
    }
    
}
ScanGrapher::~ScanGrapher()
{
    bExit_=true;
    if(thread_)
        thread_->join();

}
 
void ScanGrapher::pushScan(pcl::PointCloud<pcl::PointXYZ>::Ptr input,pcl::PointXYZ origin)
{
    Frame frame;
    frame.cloud=input;
    frame.origin=origin;
    if(scan_queue_.size()<10)
    {
        scan_queue_.push(frame);
    }
}


void ScanGrapher::getCloud(pcl::PointCloud<pcl::PointXYZ>& cloud)
{
    cloud.clear();
    Steger steger(3.1, 0.5, 3.5);
	std::vector<cv::Point2f> points;
    cv::Mat image=getGrid();
    
        cv::Mat fimage;
	    image.convertTo(fimage, CV_32FC1);
        points.clear();
        auto start = std::chrono::system_clock::now();
        steger.detect(fimage,points);
        auto end   = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
        double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
        std::cout<<" steger time : "<<time<<"  s"<<std::endl;
        double resolution=resolution_;
        if(points.size()!=0)
        {
            std::cout<<"steger point size:"<<points.size()<<std::endl;
	        for (int i = 0; i < points.size(); ++i)
	        {
		        pcl::PointXYZ point((points[i].x-fimage.cols/2)*resolution, 
			                (points[i].y-fimage.rows/2)*resolution, 0.0);
		        cloud.push_back(point);
	        }

	        pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
	        vgfilter.setInputCloud(cloud.makeShared());
	        vgfilter.setLeafSize(resolution, resolution, resolution);
	        vgfilter.filter(cloud);
            vgfilter.filter(cloud_);
        }
}

pcl::PointCloud<pcl::PointXYZ>::Ptr ScanGrapher::getCloud()
{
    Steger steger(3.1, 0.5, 3.5);
	std::vector<cv::Point2f> points;
    cv::Mat image=getGrid();
    
    cv::Mat fimage;
	image.convertTo(fimage, CV_32FC1);
        points.clear();
        auto start = std::chrono::system_clock::now();
        steger.detect(fimage,points);
        auto end   = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
        double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
        std::cout<<" steger time : "<<time<<"  s"<<std::endl;
        double resolution=resolution_;
        if(points.size()!=0)
        {
            std::cout<<"steger point size:"<<points.size()<<std::endl;
            pcl::PointCloud<pcl::PointXYZ> cloud;
	        for (int i = 0; i < points.size(); ++i)
	        {
		        pcl::PointXYZ point((points[i].x-fimage.cols/2)*resolution, 
			                (points[i].y-fimage.rows/2)*resolution, 0.0);
		        cloud.push_back(point);
	        }

	        pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
	        vgfilter.setInputCloud(cloud.makeShared());
	        vgfilter.setLeafSize(resolution, resolution, resolution);
	        vgfilter.filter(cloud_);
        }
        return cloud_.makeShared();

}


void ScanGrapher::thread_updata(ScanGrapher* p)
{
    if(p==0) return ;
    
    while(p->bExit_==false)
    {
        usleep(100*1);
        if(!p->scan_queue_.empty())
        {
                auto start = std::chrono::system_clock::now();
                Frame frame=p->scan_queue_.front();
                //p->update(*frame.cloud,frame.origin);
                p->update(*frame.cloud,frame.origin,0.02);
                p->scan_queue_.pop();
                auto end   = std::chrono::system_clock::now();
                auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
                double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
                //std::cout<<" map updata rate : "<<1.0/(time+0.000001)<<"  Hz"<<std::endl;
        }
    }
}

void ScanGrapher::update(pcl::PointCloud<pcl::PointXYZ>& input,pcl::PointXYZ origin)
{
    std::lock_guard<std::mutex> lock(mutex_); 
    int size_w=int((max_point_.x-min_point_.x)/resolution_);
    int idx_ori=int((origin.x-min_point_.x)/resolution_);
    int idy_ori=int((origin.y-min_point_.y)/resolution_);
    double prob_occ=0.7;
    double prob_free=0.3;
    double prob_prior=0.5;
    for(int i=0;i<input.size();++i)
    {
        pcl::PointXYZ point=input.points[i];
        if(point.x<min_point_.x||point.x>=max_point_.x
            ||point.y<min_point_.y||point.y>=max_point_.y)
            continue;
        
        double distance=sqrt(pow(point.x-origin.x,2.0)+pow(point.y-origin.y,2.0));
        double angle=atan2(point.y-origin.y,point.x-origin.y);
        double sangle=sin(angle);
        double cangle=cos(angle);

        for(double r=resolution_;r<distance;r+=resolution_)
        {
            double world_x=origin.x+r*cangle;
            double world_y=origin.y+r*sangle;
            int idx=int((world_x-min_point_.x)/resolution_);
            int idy=int((world_y-min_point_.y)/resolution_);
            double prob=prob_occ;
            if(r<distance-resolution_/2.0)
                prob=prob_free;
            else if(r>distance+resolution_/2.0)
                prob=prob_prior;
            else
                prob=prob_occ;
            updateCell(idx,idy,prob);
        }
        
    }
}

void ScanGrapher::update(pcl::PointCloud<pcl::PointXYZ>& input,pcl::PointXYZ origin,double delta)
{
    if(delta<=0)
    {
        std::cout<<" gauss delta invalid (>0) : "<<delta<<std::endl;
        return ;
    }
    //std::cout<<input.points[0].x<<", "<<input.points[0].y<<"   o : "<<origin.x<<" , "<<origin.y<<std::endl;
    std::lock_guard<std::mutex> lock(mutex_); 
    int size_w=int((max_point_.x-min_point_.x)/resolution_);
    int idx_ori=int((origin.x-min_point_.x)/resolution_);
    int idy_ori=int((origin.y-min_point_.y)/resolution_);
    
    double sqrt2pi_inv=1.0/(sqrt(2.0*M_PI)*delta);
    for(int i=0;i<input.size();++i)
    {
        pcl::PointXYZ point=input.points[i];
        if(point.x<min_point_.x||point.x>=max_point_.x
            ||point.y<min_point_.y||point.y>=max_point_.y)
            continue;
        
        double distance=sqrt(pow(point.x-origin.x,2.0)+pow(point.y-origin.y,2.0));
        double angle=atan2(point.y-origin.y,point.x-origin.x);
        double sangle=sin(angle);
        double cangle=cos(angle);

        //求直线方程  a b

        
        for(double r=resolution_;r<distance+1.0+delta*3.0;r+=resolution_)
        {
            double world_x=origin.x+r*cangle;
            double world_y=origin.y+r*sangle;
            int idx=int((world_x-min_point_.x)/resolution_);
            int idy=int((world_y-min_point_.y)/resolution_);
            if(idx<0||idx>=size_w|| idy<0 || idy>=grid_.rows)
                continue;
            if(r<distance-delta*3.0 )
            { 
                grid_.at<uchar>(idy,idx)=1;
                pixel_count_.at<float>(idy,idx)=1.0;
            }
            else if(r<distance+delta*3.0)
            {   
                double old_prob=grid_.at<uchar>(idy,idx)/255.0;
                double prob=exp(-pow(r-distance,2.0)/(2.0*delta*delta));

                //std::cout<<"  exp :"<<prob<<" , r= "<<r-distance<<std::endl;

                float count=pixel_count_.at<float>(idy,idx);
                double prob_mean=(old_prob*double(count)+prob)/(double(count)+1.0);
                
                int gray=int(prob_mean*255);
                gray=gray>255?255:gray;
                gray=gray<1?1:gray;
                grid_.at<uchar>(idy,idx)=gray;
                pixel_count_.at<float>(idy,idx)+=1.0;
            }
            else
            {
                grid_.at<uchar>(idy,idx)=0;
                pixel_count_.at<float>(idy,idx)=1.0;
            }
            
            
        }
        
    }

}

void ScanGrapher::updateCell(int idx,int idy,double prob_occ)
{
    int size_w=int((max_point_.x-min_point_.x)/resolution_);
    int size_h=int((max_point_.y-min_point_.y)/resolution_);
    if(idx<0 || idx>=size_w || idy<0 || idy>=size_h) 
        return ;
    int gray=grid_.at<uchar>(idy,idx)+int(log(prob_occ/(1.0-prob_occ))*255.0);
    gray=gray>255?255:gray;
    gray=gray<0?0:gray;
    grid_.at<uchar>(idy,idx)=gray;

}


cv::Mat ScanGrapher::getGrid()
{
    
    return grid_.clone();
}
bool ScanGrapher::compareAsc(const pcl::PointXYZ& pt1,const pcl::PointXYZ& pt2)
{
	return pt1.z < pt2.z;
}
void ScanGrapher::saveImage(std::string file)
{
    cv::imwrite(file.c_str(),getGrid());
}

void ScanGrapher::saveCloudTxt(std::string file)
{
    getCloud();
    if(cloud_.size()>0)
    {
        FILE* p=fopen(file.c_str(),"w");
        for(int i=0;i<cloud_.size();++i)
        {
            pcl::PointXYZ point=cloud_.points[i];
            fprintf(p,"%.4f %.4f %.4f\n",point.x,point.y,point.z);
        }
        fclose(p);
    }
    
}
void ScanGrapher::saveCloudPcd(std::string file)
{
    getCloud();
    if(cloud_.size()>0)
    {
        pcl::PCDWriter writer;
        writer.write(file.c_str(),cloud_);
    }
    
}

