#include "densematcher.h"

DenseMatcher::DenseMatcher()
{

}

DenseMatcher::DenseMatcher(int method)
{
    _method = method;
}

DenseMatcher::~DenseMatcher()
{

}

void DenseMatcher::calculateDisparityMap()
{
   std::cout <<"Disparity calculation started... " <<endl;
    if ( _lftIm == NULL || _rgtIm == NULL ){
       std::cout<< "DenseMatcher module in not initialized: use denseMantcher->init command\n"<< endl;
        return;
    }

    try{
        cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create( 16*_params.lowerBound,
                                                               16*_params.upperBound, //number of disparities
                                                                  _params.blockSize);
	
	
        sgbm->setMode(_method);

        int cn = _lftIm->channels();
        sgbm->setP1(8*cn*_params.blockSize*_params.blockSize);
        sgbm->setP2(32*cn*_params.blockSize*_params.blockSize);

        sgbm->compute( *_lftIm, *_rgtIm, _disp);

        cv::Mat_<float> temp = _disp;
        temp = temp / 16;
        
        cv::bilateralFilter(temp,_dispValues,5,30,30); //use bilateral filter ?
        //_dispValues = temp; // no filter

        cout << "Done"<<endl;
    }
    catch(...){
        cout << "DenseMatcher::Unexpected error\n"<<endl;
    }
}

void DenseMatcher::plotDisparityMap()
{
    if ( _dispValues.empty() ){
        std::cout << "Disparity was not calculated yet\n"<< endl;
        return;
    }
    cv::Mat disp8;
    cv::normalize(_dispValues, disp8, 0, 225, cv::NORM_MINMAX, CV_8U);
    //cout << _disp<<endl;

    
    cv::imshow("Disparity", disp8);
    cv::waitKey(0);
}

cv::Mat DenseMatcher::getDensePoint()
{
	int nbRow=_dispValues.rows;
	int nbCol=_dispValues.cols;
	//creat meshgrid temp1 et temp2 present reference image
	cv::Mat temp1=cv::Mat::zeros(nbRow,nbCol,CV_64F);	
	cv::Mat temp2=cv::Mat::zeros(nbRow,nbCol,CV_64F);
	for(int i=0;i<nbRow;i++)
	{
		temp1.row(i)=double(i)*cv::Mat::ones(1,nbCol,temp1.type());
	} 
	
	
	for(int j=0;j<nbCol;j++)
	{
		temp2.col(j)=double(j)*cv::Mat::ones(nbRow,1,temp2.type());
	}

	temp1=temp1.reshape(0,1);
	temp2=temp2.reshape(0,1);
	
	cv::Mat temp3;
	_disp.convertTo(temp3,CV_64F);
	
	temp3=temp3.reshape(0,1);
	temp3=temp3/16;
	//compute shift from first image to second image 
	cv::Mat temp4=temp1-temp3.reshape(0,1);

	cv::Mat densePoint_temp;
	cv::Mat densePoint;
	densePoint_temp.push_back(temp1);
	densePoint_temp.push_back(temp2);
	densePoint_temp.push_back(temp4);
	densePoint_temp.push_back(temp2);
	//std::cout<<densePoint_temp.colRange(1,20)<<std::endl;
	double min,max;
	
	cv::minMaxLoc(temp3, &min, &max,0,0);
	//cout<<"min"<< min<<endl;
        temp3=temp3.reshape(0,1);
        //cout<<temp3.at<double>(6)<<endl;
        
	densePoint_temp=densePoint_temp.t();
	//densePoint=densePoint_temp;
        
	//remove the error colum which affect final result
	for(int k=0;k<temp3.cols;k++)
	{
		if(ceil(temp3.at<double>(k))>min+1&&ceil(temp3.at<double>(k))<max-1)
		{
						 
			densePoint.push_back(densePoint_temp.row(k));
                        
		}
		
	}
        
	densePoint=densePoint.t();
	//cout<<"2"<<densePoint.size()<<endl;
	//std::cout<<densePoint.colRange(1,20)<<std::endl;
        
	return densePoint;
}




void DenseMatcher::filterDisparity(int newVal, int maxSpeckleSize, int maxDiff)
{
    _paramsFilter.newVal = newVal;
    _paramsFilter.maxSpeckleSize = maxSpeckleSize;
    _paramsFilter.maxDiff = maxDiff;

    std::cout << "Disparity filtering started... " << endl;

    if ( ! _disp.empty() ){
        _disp.copyTo(_dispFiltered);
        cv::filterSpeckles( _dispFiltered, newVal, maxSpeckleSize, maxDiff);

        cv::Mat_<float> temp = _dispFiltered;
        temp = temp / 16;

        _dispValues = temp;

    }
    else{
        std::cout <<"Done\n"<<std::endl;
        return;
    }
    std::cout <<"Done"<< std::endl;
}

cv::Mat DenseMatcher::getDisparityToDisplay() const
{
//    if ( _disp.empty() )
//        AR_Printf("Disparity was not calculated yet");

    cv::Mat disp8 = _disp;

    double minVal, maxVal;
    minMaxLoc(disp8, &minVal, &maxVal); //find minimum and maximum intensities
    cv::Mat draw;
    disp8.convertTo(draw, CV_8UC1, 255.0/(maxVal - minVal), -minVal * 255.0/(maxVal - minVal));
    //cv::imshow("image", draw);
    return draw;
}

void DenseMatcher::plotDisparityFiltered()
{
    if ( _dispFiltered.empty() ){
        std::cout << "Disparity was not filtered yet\n" << endl;
        return;
    }
    cv::Mat disp8;
    cv::normalize(_dispFiltered, disp8, 0, 255, cv::NORM_MINMAX, CV_8U);
    cv::imshow("Disparity", disp8);
}