function disparityMap = disparityBM(I1, I2, varargin)
% disparityBM Computes disparity map using block matching.
%   disparityMap = disparityBM(I1, I2) returns the disparity map for a pair
%   of stereo images, I1 and I2, using block matching algorithm. I1 and I2
%   must have the same size and must be rectified such that the
%   corresponding points are located on the same rows. This rectification
%   can be performed using the rectifyStereoImages function. The returned
%   disparity map has the same size as I1 and I2. Unreliable disparity
%   values are set to nan('single').
%
%   disparityMap = disparityBM(..., Name, Value) specifies additional
%   name-value pairs described below:
%
%   'DisparityRange'       A two-element vector, [MinDisparity
%                          MaxDisparity], defining the range of disparity.
%                          MinDisparity must be always 0. MaxDisparity must
%                          be an integer and divisible by 16. MaxDisparity
%                          value must be within the range [16 256].
%
%                          Default: [0 64]
%
%   'BlockSize'            A positive odd integer that specifies the width
%                          of each square block of pixels used for
%                          comparing I1 and I2. BlockSize must be in the
%                          range [5 51].
%
%                          Default: 15
%
%   'ContrastThreshold'    A scalar value, defining the acceptable range of
%                          contrast values. Increasing this parameter
%                          results in fewer pixels being marked as
%                          unreliable. The ContrastThreshold must be in the
%                          range (0 1].
%
%                          Default: 0.5
%
%   'TextureThreshold'     A scalar value, defining the minimum texture. If
%                          a block of pixels is less textured, the computed
%                          disparity is less reliable. Increasing this
%                          parameter results in more pixels being marked as
%                          unreliable. Set this parameter to 0 to disable
%                          it. TextureThreshold must be in the range [0 1).
%
%                          Default: 0.0002
%
%   Class Support
%   -------------
%   The input images I1 and I2 are gray scale gpuArrays with the same
%   datatypes of uint8, uint16, int16, single, or double, and they must be
%   real, finite and nonsparse. The output will be a gpuArray object of
%   type single.
%
%   Example
%   -------
%   % Load the rectified images.
%   I1 = imread('scene_left.png');
%   I2 = imread('scene_right.png');
%
%   % Show the stereo anaglyph. You can view the image in 3-D using
%   % red-cyan stereo glasses.
%   figure
%   imshow(stereoAnaglyph(I1, I2))
%   title('Red-cyan composite view of the stereo images')
%
%   % Convert images to grayscale.
%   I1 = rgb2gray(I1);
%   I2 = rgb2gray(I2);
%
%   % Move images to the GPU.
%   I1 = gpuArray(I1);
%   I2 = gpuArray(I2);
%
%   % Compute the disparity map.
%   range = [0 16];
%   disparityMap = disparityBM(I1, I2, 'DisparityRange', range);
%
%   % Show the disparity map. For better visualization use the disparity
%   % range as the display range for imshow.
%   figure
%   imshow(disparityMap, range)
%   title('Disparity Map')
%   colormap jet
%   colorbar
%
%   See also rectifyStereoImages, reconstructScene, gpuArray,
%            disparitySGM, estimateCameraParameters,
%            estimateUncalibratedRectification

%   Copyright 2017-2018 The MathWorks, Inc.
%
%   References:
%   -----------
%   [1] K. Konolige, "Small Vision Systems: Hardware and Implementation,"
%       Proceedings of the 8th International Symposium in Robotic Research,
%       pages 203-212, 1997.
%
%   [2] G. Bradski and A. Kaehler, "Learning OpenCV : Computer Vision with
%       the OpenCV Library," O'Reilly, Sebastopol, CA, 2008.

%--------------------------------------------------------------------------
% Handle the case if Name-Value pairs are gpuArray Objects
%--------------------------------------------------------------------------
if(nargin > 2)
    % Make sure all parameters are on the host.
    [varargin{:}] = gather(varargin{:});
    
    % If the data is on the CPU, dispatch to the CPU version. You don't want to
    % run on the GPU unless the data is on the GPU.
    if ~isa(I1, 'gpuArray') && ~isa(I2, 'gpuArray')
        disparityMap = disparityBM(I1,I2, varargin{:});
        return
    end
end

%--------------------------------------------------------------------------
% Parse the inputs
%--------------------------------------------------------------------------
r = parseInputs(I1, I2, varargin{:});

% BlockMatching method parameters
% -------------------------------

% preFilterCap minimum value allowed in OpenCV
if (r.ContrastThreshold < 0.016)
    r.ContrastThreshold = 0.016;
end
optBM.preFilterCap        = int32(floor(63 * r.ContrastThreshold));
optBM.SADWindowSize       = int32(r.BlockSize);
optBM.numberOfDisparities = int32(r.DisparityRange(2) - r.DisparityRange(1));

% parameters unique to block matching
optBM.textureThreshold    = int32(255 * r.TextureThreshold * r.BlockSize^2);

% OpenCV parameters that are not exposed as optional parameters
optBM.preFilterType       = int32(0);    % Fixed to CV_STEREO_BM_NORMALIZED_RESPONSE

%--------------------------------------------------------------------------
% Compute disparity
%--------------------------------------------------------------------------
I1_u8 = im2uint8(I1);
I2_u8 = im2uint8(I2);
if optBM.textureThreshold == 0
    disparityMap = single(ocvgpuDisparityBM(I1_u8, I2_u8, optBM));
else
    disparityMap = (ocvgpuDisparityBM(I1_u8, I2_u8, optBM))';
end

%==========================================================================
% Parse and check inputs
%==========================================================================
function r = parseInputs(I1, I2, varargin)
% Error out if logical images are inputs
if( islogical(I1) || islogical(I2) )
    error( message('vision:disparity:logicalImagesNotSupported') );
end

vision.internal.inputValidation.validateImagePairForDisparity(I1, I2, 'I1', 'I2', 'grayscale');
imageSize = size(I1);
r = parseOptionalInputs(imageSize, varargin{:});

%==========================================================================
function r = parseOptionalInputs(imageSize, varargin)
r = parseOptionalInputs_sim(imageSize, getDefaultParameters(),...
    varargin{:});

%==========================================================================
function r = parseOptionalInputs_sim(imageSize, defaults, varargin)
if(nargin == 2)
    r = defaults;
    checkContrastThreshold(r.ContrastThreshold);
    checkBlockSize(r.BlockSize, imageSize);
    checkDisparityRange(r.DisparityRange, imageSize);
    checkTextureThreshold(r.TextureThreshold);
    crossCheckBlockSizeDisparityRange(imageSize, r.BlockSize, ...
        r.DisparityRange(2));
else
    %--------------------------------------------------------------------------
    % Specify the optional parameter value pairs
    %--------------------------------------------------------------------------
    parser = inputParser;
    parser.FunctionName = mfilename;
    parser.addParameter('ContrastThreshold', defaults.ContrastThreshold, ...
        @(x)(checkContrastThreshold(x)));
    parser.addParameter('BlockSize', defaults.BlockSize, ...
        @(x)(checkBlockSize(x, imageSize)));
    parser.addParameter('DisparityRange', defaults.DisparityRange, ...
        @(x)(checkDisparityRange(x, imageSize)));
    parser.addParameter('TextureThreshold', defaults.TextureThreshold, ...
        @(x)(checkTextureThreshold(x)));
    %--------------------------------------------------------------------------
    % Parse the optional parameters
    %--------------------------------------------------------------------------
    parser.parse(varargin{:});
    
    crossCheckBlockSizeDisparityRange(imageSize, parser.Results.BlockSize, ...
        parser.Results.DisparityRange(2));
    
    r = parser.Results;
end

%==========================================================================
function defaults = getDefaultParameters()
defaults = struct(...
    'ContrastThreshold', 0.5, ...
    'BlockSize',   15, ...
    'DisparityRange',   [0 64], ...
    'TextureThreshold',   0.0002);
%==========================================================================
function r = checkContrastThreshold(value)
validateattributes(value, {'numeric'}, ...
    {'real', 'nonsparse', 'scalar','nonnan', 'finite', 'nonnegative', '>', 0, '<=', 1},...
    mfilename, 'ContrastThreshold');
r = 1;

%==========================================================================
function r = checkBlockSize(value, imageSize)
maxBlockSize = min([imageSize, 51]); % 51 is the maximum limit allowed by the OpenCV GPU code
validateattributes(value, {'numeric'}, ...
    {'real', 'nonsparse', 'scalar','nonnan', 'finite', 'integer',...
    'nonnegative', 'odd', '>=', 5, '<=', maxBlockSize}, mfilename, 'BlockSize');
r = 1;

%==========================================================================
function r = checkDisparityRange(value,imageSize)
imgWidth = imageSize(2);
% Max disparity supported is 256 and it cannot exceed the No. of columns of
% the image
validateattributes(value, {'numeric'}, ...
    {'real', 'nonsparse', 'nonnan', 'finite', 'integer', 'size', [1,2]},...
    mfilename, 'DisparityRange');
if( value(2) > 256 )
    error(message('vision:disparity:DisparityRangeExceedLimitBMGPU'));
elseif(value(1) ~= 0)
    error(message('vision:disparity:nonZeroMinDisparityRangeBMGPU'));
elseif(value(2) >= imgWidth )
    error(message('vision:disparity:DisparityRangeExceedImageWidth',...
        imgWidth) );
elseif(value(2) <= value(1))
    error(message('vision:disparity:invalidMaxDisparityRange'));
elseif(mod(value(2) - value(1), 16) ~= 0)
    error(message('vision:disparity:invalidDisparityRangeBM'));
end
r = 1;

%==========================================================================
function r = checkTextureThreshold(value)
validateattributes(value, {'numeric'}, ...
    {'real', 'nonsparse', 'scalar','nonnan', 'finite', 'nonnegative', '<', 1},...
    mfilename, 'TextureThreshold');
r = 1;

%==========================================================================
function r = crossCheckBlockSizeDisparityRange(imageSize,BlockSize,DisparityRange)
% DisparityRange and BlockSize should satisfy all the following conditions
% 1. 0 < DisparityRange <= 256
% 2. 3 < BlockSize <= 51
% 3. imageSize(2) - DisparityRange - BlockSize >= 0
imgWidth = imageSize(2);
if(imgWidth < (BlockSize+DisparityRange))
    error(message('vision:disparity:invalidImageWidthBMGPU'));
end
r = 1;