#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 ();
    			} //*
    			
  		}
	
	}


}