#include "3dReconst.h"
  #include "Autocalib.h"
  
  
  //rectifier epipolar Lines and image
  vector<cv::Mat> Reconst::RectificaEpipolarLines(vector<cv::Mat> image, vector<vector<cv::Point2d> > MeasureVector, vector<cv::Mat> fundMatrix)
  {
  	vector<cv::Mat> outImage;
  	vector<cv::Mat> tempImage=image;
  	vector<vector<cv::Point2d> > tempMeasureVector=MeasureVector;
  	
  	cv::Mat Frect = (cv::Mat_<double>(3,3) << 0,0,0,0,0,-1.0,0,1.0,0);
  	cout << Frect << endl;
  	Autocalib calib;
  	
  	for (int i=0;i<tempImage.size()-1;i++)
  	{
  		RectifierAffine *rec = new RectifierAffine();
  		rec->init(&tempImage[i],&tempImage[i+1],&fundMatrix[i],&MeasureVector[i],&MeasureVector[i+1]);
  		rec->rectify();
  		
  		cv::Mat input1, input2;
  		rec->getResult(input1, input2,&MeasureVector[i],&MeasureVector[i+1]);
  		outImage.push_back(input1);
  		outImage.push_back(input2);		
  	
  		cv::Mat retifImg; 
  		retifImg=calib.plotStereoWithEpilines(outImage[2*i],outImage[2*i+1],Frect,MeasureVector[i],MeasureVector[i+1]);
  		cv::namedWindow( "Epipolar lines: rectified", cv::WINDOW_NORMAL );
  		cv::imshow("Epipolar lines: rectified", retifImg);
  
  		tempMeasureVector[i+1].swap(MeasureVector[i+1]);
  	
  		cv::waitKey(0);
  	}
  	return outImage;
  }
  
  //get disparity map and the dense point
  void Reconst::disparity(vector<cv::Mat> image)
  {
  	DenseMatcher DM;
  	for (int i=0;i<image.size()/2;i++)
  	{
  		DM.init(&image[2*i],&image[2*i+1]);
  		DM.calculateDisparityMap();
  		DM.plotDisparityMap();
  
  		cv::Mat densePt=DM.getDensePoint();
  		_densePoint.push_back(densePt);
  		
  	}
  }
  
  
  //do the triangulation and get 3D point cloud 
  void Reconst::GetPointCloud(vector<cv::Mat> P, cv::Mat image )
  {
  	
          vector<cv::Mat> rectifiedMeseureMatrice=_densePoint;
  	vector<cv::Mat> cameraMatrix=GetCameraMatrix(P);
          //cout<<rectifiedMeseureMatrice[1].cols<< endl;
  	pcl::PointCloud<pcl::PointXYZ> allPointCloud;
  	Triangulator triang;
  	for (int i=0;i<cameraMatrix.size();i++)
  	{
  		pcl::PointCloud<pcl::PointXYZ> ptCloud;
  		ptCloud=triang.triangulatePoints_LinearLS(_densePoint[i], cameraMatrix[i]);
                  
                  //ptCloud.is_dense = true;
  		int nbr=i;
  		//save 3D point cloud in a file 
  		string name = "pointCloud"+to_string(nbr)+".pcd"; 
  		pcl::io::savePCDFileASCII (name, ptCloud);
    		cerr << "Saved " << ptCloud.points.size () << " data points to pointCloud" <<i<<".pcd." << endl;
  	}
  }
  
  
  vector<cv::Mat> Reconst::GetCameraMatrix(vector<cv::Mat> P)
  {
  	vector<cv::Mat> CameraMatrix;
  	CameraMatrix.resize(P.size()-1);
  	for(int i=0;i<P.size()-1;i++) 
  	{
  		CameraMatrix[i].push_back(P[i]);
  		CameraMatrix[i].push_back(P[i+1]);
  	} 
  	return CameraMatrix;
  }
  
  
  void Reconst::filterPointCloud(int taille)
  {
       	for(int i=0;i<taille;i++ )
  	{
  		std::stringstream ss1;
          	ss1 << "pointCloud" <<std::to_string(i)<<".pcd" ; 
  		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  		
  		pcl::PCDWriter writer;
  
    		// Fill in the cloud data
    		pcl::PCDReader reader;
    		// Replace the path below with the path where you saved your file
    		reader.read<pcl::PointXYZ> (ss1.str (), *cloud);
  
    		std::cerr << "Cloud before filtering: " << std::endl;
    		std::cerr << *cloud << std::endl;
  
   		// Creating the KdTree object for the search method of the extraction
    		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    		tree->setInputCloud (cloud);
  
    		std::vector<pcl::PointIndices> cluster_indices;
    		pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    		ec.setClusterTolerance (8); // 2cm
    		ec.setMinClusterSize (30);
   		ec.setMaxClusterSize (1000000);
   		ec.setSearchMethod (tree);
    		ec.setInputCloud (cloud);
    		ec.extract (cluster_indices);
  
    		if(cluster_indices.size()==0){ std::cout<<"no data"<<std::endl; }
  
    		
    		int cloudSize=0;
    		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    		{
      			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
      			for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
        			cloud_cluster->points.push_back (cloud->points[*pit]); //*
     			cloud_cluster->width = cloud_cluster->points.size ();
     	 		cloud_cluster->height = 1;
     	 		cloud_cluster->is_dense = true;
      			if(cloud_cluster->points.size ()>cloudSize)
     			{
      				std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
      				std::stringstream ss2;
      				ss2 << "pointCloud_filtered" <<std::to_string(i)<< ".pcd";
      				writer.write<pcl::PointXYZ> (ss2.str (), *cloud_cluster, false);
      				cloudSize=cloud_cluster->points.size ();
      			} //*
      			
    		}
  	
  	}
  
  
  }