/*!
 *  \brief     Stereo image dense DenseMatcher
 *  \details   This class is used to demonstrate a number of section commands.
 *  \author    Andrey Kudryavtsev
 *  \version   0.1
 *  \date      03/06/2016
 */

#ifndef DenseMatcher_H
#define DenseMatcher_H

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d.hpp>

using namespace std;


class DenseMatcher
{

public:
    DenseMatcher();
    DenseMatcher(int method);
    ~DenseMatcher();

    void init(cv::Mat * lftIm, cv::Mat * rgtIm) { _lftIm = lftIm; _rgtIm = rgtIm;}
    void setMethod( int method ) { _method = method; }
    void setBounds( int lowerBound, int upperBound) {_params.lowerBound = lowerBound; _params.upperBound = upperBound;}
    void setBlockSize( int blockSize) {_params.blockSize = blockSize;}

    void calculateDisparityMap();
    void plotDisparityMap();

    void filterDisparity(int newVal, int maxSpeckleSize, int maxDiff);
    void plotDisparityFiltered();

    cv::Mat getDisparityToDisplay() const;

    enum methods{
        MODE_HH        = cv::StereoSGBM::MODE_HH,          ///< Perform linear interpolation on the table
        MODE_SGBM      = cv::StereoSGBM::MODE_SGBM,          ///< Perform parabolic interpolation on the table
        MODE_SGBM_3WAY = cv::StereoSGBM::MODE_SGBM_3WAY           ///< Perform parabolic interpolation on the table
    };

    bool isInitialized()        { return ! (_lftIm == NULL || _rgtIm == NULL) && ! _lftIm->empty() && ! _rgtIm->empty() ; } // _lftIm and _rgtIm are initialized
    bool isDisparityEmpty()     { return _disp.empty(); }
    bool isDisparityFiltered()  { return _dispFiltered.empty(); }

    cv::Mat _dispFiltered;
    cv::Mat _disp;
    cv::Mat _dispValues;
    cv::Mat getDensePoint();

private:

    int _method = MODE_SGBM; // Default method

    cv::Mat * _lftIm = NULL;
    cv::Mat * _rgtIm = NULL;

    struct Params{
        int lowerBound = -1;
        int upperBound = 2;
        int blockSize  = 9;
    };
    Params _params;

    struct ParamsFilter{
        int newVal = 0; // The disparity value used to paint-off the speckles
        int maxSpeckleSize = 260; // The maximum speckle size to consider it a speckle. Larger blobs are not affected by the algorithm
        int maxDiff = 10; // Maximum difference between neighbor disparity pixels to put them into the same blob. Note that since StereoBM,
        //StereoSGBM and may be other algorithms return a fixed-point disparity map, where disparity values are multiplied by 16,
        //this scale factor should be taken into account when specifying this parameter value.
    };
    ParamsFilter _paramsFilter;
};



/**************************
 *
 * CONTROL PART
 *
 * */



#endif // DenseMatcher_H