#include <stdio.h> 
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp> 
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "Autocalib.h"
#include "3dReconst.h"

using namespace std;


int main(int argc, char *argv[])
{
	vector<cv::Mat> image;

	for (int index=1; index<argc; index++) {
		image.push_back( cv::imread( argv[index], cv::IMREAD_GRAYSCALE));
	}

	cout<<image[0].size()<<endl;
	/* for(int i=0;i<image.size();i++)
	{
		int offset_x = 800;
    		int offset_y = 400;
    		cv::Rect roi;
    		roi.x = offset_x;
    		roi.y = offset_y;
    		roi.width = image[i].size().width - (offset_x*2);
		cout<<"ok"<<endl;
    		roi.height = image[i].size().height - (offset_y*3);
    		image[i] = image[i](roi);
		string name = "piece"+to_string(i)+".jpg";
                cv::imwrite(name,image[i]);

	}*/
	
	//Extract features AKAZE and estimat measuremental matrix
	Autocalib clib;	
        vector<vector<cv::Point2d> > matchVector=clib.findFeaturePoint(image,0.001);
	vector<vector<cv::Point2d> > measureVector;
	if(image.size()>2){  measureVector=clib.getMeasureVector(matchVector); }
	else{measureVector=matchVector;}
	//cout<<measureVector.size()<<endl;

	//Estimate Fundamental matrix and show epipolair lines with images (before rectification)
	vector<cv::Mat> fundMatVector =clib.estimatFundMatVector(measureVector);
	clib.showEpipolarLines(image, measureVector, fundMatVector);
	
	//optimisation
	cv::Mat measureMatrix = clib.vectorPoint2Mat(measureVector);
	vector <cv::Mat>  P;  // P : camera matrix
	P=clib.estimatParameter(measureMatrix);
        for(int i=0; i<P.size();i++)
	{cout <<"matrice de camera"<<endl<< P[i]<<endl;}
	
	
	
	//reconstruction 3d
	Reconst rec;
	vector<cv::Mat> rectifImage;
	rectifImage=rec.RectificaEpipolarLines(image, measureVector,  fundMatVector);
	rec.disparity(rectifImage);
	rec.GetPointCloud(P,image[0]);
	rec.filterPointCloud(rectifImage.size()/2);

	return 0;
}