/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "_cvaux.h"

/****************************************************************************************\
    The code below is some modification of Stan Birchfield's algorithm described in:

    Depth Discontinuities by Pixel-to-Pixel Stereo
    Stan Birchfield and Carlo Tomasi
    International Journal of Computer Vision,
    35(3): 269-293, December 1999.
    
    This implementation uses different cost function that results in
    O(pixPerRow*maxDisparity) complexity of dynamic programming stage versus
    O(pixPerRow*log(pixPerRow)*maxDisparity) in the above paper.
\****************************************************************************************/

/****************************************************************************************\
*       Find stereo correspondence by dynamic programming algorithm                      *
\****************************************************************************************/
#define ICV_DP_STEP_LEFT  0
#define ICV_DP_STEP_UP    1
#define ICV_DP_STEP_DIAG  2

#define ICV_BIRCH_DIFF_LUM 5

#define ICV_MAX_DP_SUM_VAL (INT_MAX/4)

typedef struct _CvDPCell
{
    uchar  step; //local-optimal step
    int    sum;  //current sum  
}_CvDPCell;

typedef struct _CvRightImData
{
    uchar min_val, max_val;
} _CvRightImData;

// When CV_IMAX3 and CV_IMIN3 are used in the same expression, the evulation
// order is undefined, and they should not assign to the same temporary variable.
#define CV_IMAX3(a,b,c) ((max3 = (a) >= (b) ? (a) : (b)),(max3 >= (c) ?  max3 : (c)))
#define CV_IMIN3(a,b,c) ((min3 = (a) <= (b) ? (a) : (b)),(min3 <= (c) ?  min3 : (c)))

void icvFindStereoCorrespondenceByBirchfieldDP( uchar* src1, uchar* src2,
                                                uchar* disparities,
                                                CvSize size, int widthStep,
                                                int    maxDisparity, 
                                                float  _param1, float _param2, 
                                                float  _param3, float _param4,
                                                float  _param5 )
{
    int     x, y, i, j, max3, min3;
    int     d, s;
    int     dispH =  maxDisparity + 3; 
    uchar  *dispdata;
    int     imgW = size.width;
    int     imgH = size.height;
    uchar   val, prevval, prev, curr;
    int     min_val;
    uchar*  dest = disparities;
    int param1 = cvRound(_param1);
    int param2 = cvRound(_param2);
    int param3 = cvRound(_param3);
    int param4 = cvRound(_param4);
    int param5 = cvRound(_param5);

    #define CELL(d,x)   cells[(d)+(x)*dispH]
    
    uchar*              dsi = (uchar*)cvAlloc(sizeof(uchar)*imgW*dispH);
    uchar*              edges = (uchar*)cvAlloc(sizeof(uchar)*imgW*imgH);
    _CvDPCell*          cells = (_CvDPCell*)cvAlloc(sizeof(_CvDPCell)*imgW*MAX(dispH,(imgH+1)/2));
    _CvRightImData*     rData = (_CvRightImData*)cvAlloc(sizeof(_CvRightImData)*imgW);
    int*                reliabilities = (int*)cells;
    
    for( y = 0; y < imgH; y++ ) 
    { 
        uchar* srcdata1 = src1 + widthStep * y;
        uchar* srcdata2 = src2 + widthStep * y;        

        //init rData
        prevval = prev = srcdata2[0];
        for( j = 1; j < imgW; j++ )
        {             
            curr = srcdata2[j];
            val = (uchar)((curr + prev)>>1);
            rData[j-1].max_val = (uchar)CV_IMAX3( val, prevval, prev );
            rData[j-1].min_val = (uchar)CV_IMIN3( val, prevval, prev );
            prevval = val;
            prev = curr;
        }
        rData[j-1] = rData[j-2];//last elem

        // fill dissimularity space image
        for( i = 1; i <= maxDisparity + 1; i++ )
        {               
            dsi += imgW;
            rData--;
            for( j = i - 1; j < imgW - 1; j++ )
            {                
                int t; 
                if( (t = srcdata1[j] - rData[j+1].max_val) >= 0 )
                {
                    dsi[j] = (uchar)t;
                }
                else if( (t = rData[j+1].min_val - srcdata1[j]) >= 0 )
                {
                    dsi[j] = (uchar)t;
                }
                else
                {
                    dsi[j] = 0;
                }
            }
        }
        dsi -= (maxDisparity+1)*imgW;
        rData += maxDisparity+1;

        //intensity gradients image construction
        //left row
        edges[y*imgW] = edges[y*imgW+1] = edges[y*imgW+2] = 2;
        edges[y*imgW+imgW-1] = edges[y*imgW+imgW-2] = edges[y*imgW+imgW-3] = 1;
        for( j = 3; j < imgW-4; j++ )
        {
            edges[y*imgW+j] = 0;
            
            if( ( CV_IMAX3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) - 
                  CV_IMIN3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) ) >= ICV_BIRCH_DIFF_LUM )
            {
                edges[y*imgW+j] |= 1;
            }
            if( ( CV_IMAX3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) - 
                  CV_IMIN3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) ) >= ICV_BIRCH_DIFF_LUM )
            {
                edges[y*imgW+j] |= 2;
            }            
        }        

        //find correspondence using dynamical programming
        //init DP table
        for( x = 0; x < imgW; x++ ) 
        {
            CELL(0,x).sum = CELL(dispH-1,x).sum = ICV_MAX_DP_SUM_VAL;
            CELL(0,x).step = CELL(dispH-1,x).step = ICV_DP_STEP_LEFT;
        }
        for( d = 2; d < dispH; d++ ) 
        {
            CELL(d,d-2).sum = ICV_MAX_DP_SUM_VAL;
            CELL(d,d-2).step = ICV_DP_STEP_UP;
        }    
        CELL(1,0).sum  = 0;
        CELL(1,0).step = ICV_DP_STEP_LEFT;

        for( x = 1; x < imgW; x++ )
        {        
            int d = MIN( x + 1, maxDisparity + 1);
            uchar* _edges = edges + y*imgW + x;
            int e0 = _edges[0] & 1;
            _CvDPCell* _cell = cells + x*dispH;

            do
            {
                int s = dsi[d*imgW+x];
                int sum[3];

                //check left step
                sum[0] = _cell[d-dispH].sum - param2;                

                //check up step
                if( _cell[d+1].step != ICV_DP_STEP_DIAG && e0 )
                {
                    sum[1] = _cell[d+1].sum + param1;

                    if( _cell[d-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-d] & 2) ) 
                    {
                        int t;
                        
                        sum[2] = _cell[d-1-dispH].sum + param1;

                        t = sum[1] < sum[0];

                        //choose local-optimal pass
                        if( sum[t] <= sum[2] )
                        {
                            _cell[d].step = (uchar)t;
                            _cell[d].sum = sum[t] + s;
                        }
                        else
                        {                
                            _cell[d].step = ICV_DP_STEP_DIAG;
                            _cell[d].sum = sum[2] + s;
                        }
                    }
                    else
                    {
                        if( sum[0] <= sum[1] )
                        {
                            _cell[d].step = ICV_DP_STEP_LEFT;
                            _cell[d].sum = sum[0] + s;
                        }
                        else
                        {
                            _cell[d].step = ICV_DP_STEP_UP;
                            _cell[d].sum = sum[1] + s;
                        }
                    }
                }
                else if( _cell[d-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-d] & 2) ) 
                {
                    sum[2] = _cell[d-1-dispH].sum + param1;
                    if( sum[0] <= sum[2] )
                    {
                        _cell[d].step = ICV_DP_STEP_LEFT;
                        _cell[d].sum = sum[0] + s;
                    }
                    else
                    {
                        _cell[d].step = ICV_DP_STEP_DIAG;
                        _cell[d].sum = sum[2] + s;
                    }
                }
                else
                {
                    _cell[d].step = ICV_DP_STEP_LEFT;
                    _cell[d].sum = sum[0] + s;
                }
            }
            while( --d );
        }// for x

        //extract optimal way and fill disparity image
        dispdata = dest + widthStep * y;

        //find min_val
        min_val = ICV_MAX_DP_SUM_VAL;
        for( i = 1; i <= maxDisparity + 1; i++ )
        {
            if( min_val > CELL(i,imgW-1).sum )
            {
                d = i;
                min_val = CELL(i,imgW-1).sum;
            }
        }
        
        //track optimal pass
        for( x = imgW - 1; x > 0; x-- )
        {        
            dispdata[x] = (uchar)(d - 1);
            while( CELL(d,x).step == ICV_DP_STEP_UP ) d++;
            if ( CELL(d,x).step == ICV_DP_STEP_DIAG )
            {
                s = x;
                while( CELL(d,x).step == ICV_DP_STEP_DIAG ) 
                {
                    d--; 
                    x--;                    
                }
                for( i = x; i < s; i++ )
                {
                    dispdata[i] = (uchar)(d-1);
                }            
            }        
        }//for x
    }// for y

    //Postprocessing the Disparity Map

    //remove obvious errors in the disparity map
    for( x = 0; x < imgW; x++ )
    {
        for( y = 1; y < imgH - 1; y++ )
        {
            if( dest[(y-1)*widthStep+x] == dest[(y+1)*widthStep+x] )
            {
                dest[y*widthStep+x] = dest[(y-1)*widthStep+x];
            }
        }
    }

    //compute intensity Y-gradients
    for( x = 0; x < imgW; x++ )
    {
        for( y = 1; y < imgH - 1; y++ )
        {
            if( ( CV_IMAX3( src1[(y-1)*widthStep+x], src1[y*widthStep+x], 
                        src1[(y+1)*widthStep+x] ) - 
                  CV_IMIN3( src1[(y-1)*widthStep+x], src1[y*widthStep+x], 
                        src1[(y+1)*widthStep+x] ) ) >= ICV_BIRCH_DIFF_LUM )
            {
                edges[y*imgW+x] |= 4;
                edges[(y+1)*imgW+x] |= 4;
                edges[(y-1)*imgW+x] |= 4;
                y++;
            }
        }
    }

    //remove along any particular row, every gradient 
    //for which two adjacent columns do not agree.
    for( y = 0; y < imgH; y++ )
    {
        prev = edges[y*imgW];
        for( x = 1; x < imgW - 1; x++ )
        {
            curr = edges[y*imgW+x];            
            if( (curr & 4) &&
                ( !( prev & 4 ) ||
                  !( edges[y*imgW+x+1] & 4 ) ) )
            {
                edges[y*imgW+x] -= 4;
            }
            prev = curr;
        }
    }

    // define reliability
    for( x = 0; x < imgW; x++ )
    {
        for( y = 1; y < imgH; y++ )
        {
            i = y - 1;
            for( ; y < imgH && dest[y*widthStep+x] == dest[(y-1)*widthStep+x]; y++ )
                ;
            s = y - i;
            for( ; i < y; i++ )
            {                
                reliabilities[i*imgW+x] = s;
            }            
        }
    }   
    
    //Y - propagate reliable regions 
    for( x = 0; x < imgW; x++ )
    {        
        for( y = 0; y < imgH; y++ )
        {   
            d = dest[y*widthStep+x];
            if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 4) &&
                d > 0 )//highly || moderately
            {   
                disparities[y*widthStep+x] = (uchar)d;
                //up propagation
                for( i = y - 1; i >= 0; i-- )
                {
                    if(  ( edges[i*imgW+x] & 4 ) ||
                         ( dest[i*widthStep+x] < d && 
                           reliabilities[i*imgW+x] >= param3 ) ||
                         ( reliabilities[y*imgW+x] < param5 && 
                           dest[i*widthStep+x] - 1 == d ) ) break;

                    disparities[i*widthStep+x] = (uchar)d;                    
                }                     
                                
                //down propagation
                for( i = y + 1; i < imgH; i++ )
                {
                    if(  ( edges[i*imgW+x] & 4 ) ||
                         ( dest[i*widthStep+x] < d && 
                           reliabilities[i*imgW+x] >= param3 ) ||
                         ( reliabilities[y*imgW+x] < param5 && 
                           dest[i*widthStep+x] - 1 == d ) ) break;

                    disparities[i*widthStep+x] = (uchar)d;
                }
                y = i - 1;
            }
            else
            {
                disparities[y*widthStep+x] = (uchar)d;
            }
        }
    }

    // define reliability along X
    for( y = 0; y < imgH; y++ )
    {
        for( x = 1; x < imgW; x++ )
        {
            i = x - 1;
            for( ; x < imgW && dest[y*widthStep+x] == dest[y*widthStep+x-1]; x++ );
            s = x - i;
            for( ; i < x; i++ )
            {                
                reliabilities[y*imgW+i] = s;
            }            
        }
    }   
    
    //X - propagate reliable regions 
    for( y = 0; y < imgH; y++ )    
    {        
        for( x = 0; x < imgW; x++ )
        {   
            d = dest[y*widthStep+x];
            if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 1) &&
                d > 0 )//highly || moderately
            {   
                disparities[y*widthStep+x] = (uchar)d;
                //up propagation
                for( i = x - 1; i >= 0; i-- )
                {
                    if(  (edges[y*imgW+i] & 1) ||
                         ( dest[y*widthStep+i] < d && 
                           reliabilities[y*imgW+i] >= param3 ) ||
                         ( reliabilities[y*imgW+x] < param5 && 
                           dest[y*widthStep+i] - 1 == d ) ) break;

                    disparities[y*widthStep+i] = (uchar)d;
                }                     
                                
                //down propagation
                for( i = x + 1; i < imgW; i++ )
                {
                    if(  (edges[y*imgW+i] & 1) ||
                         ( dest[y*widthStep+i] < d && 
                           reliabilities[y*imgW+i] >= param3 ) ||
                         ( reliabilities[y*imgW+x] < param5 && 
                           dest[y*widthStep+i] - 1 == d ) ) break;

                    disparities[y*widthStep+i] = (uchar)d;
                }
                x = i - 1;
            }
            else
            {
                disparities[y*widthStep+x] = (uchar)d;
            }
        }
    }

    //release resources
    cvFree( &dsi );    
    cvFree( &edges );    
    cvFree( &cells );        
    cvFree( &rData );        
}


/*F///////////////////////////////////////////////////////////////////////////
//
//    Name:    cvFindStereoCorrespondence
//    Purpose: find stereo correspondence on stereo-pair
//    Context:
//    Parameters:
//      leftImage - left image of stereo-pair (format 8uC1).
//      rightImage - right image of stereo-pair (format 8uC1).
//      mode -mode of correspondance retrieval (now CV_RETR_DP_BIRCHFIELD only)
//      dispImage - destination disparity image
//      maxDisparity - maximal disparity 
//      param1, param2, param3, param4, param5 - parameters of algorithm
//    Returns:
//    Notes:
//      Images must be rectified.
//      All images must have format 8uC1.
//F*/
CV_IMPL void
cvFindStereoCorrespondence( 
                   const  CvArr* leftImage, const  CvArr* rightImage,
                   int     mode,
                   CvArr*  depthImage,
                   int     maxDisparity,                                
                   double  param1, double  param2, double  param3, 
                   double  param4, double  param5  )
{       
    CV_FUNCNAME( "cvFindStereoCorrespondence" );

    __BEGIN__;

    CvMat  *src1, *src2;    
    CvMat  *dst;
    CvMat  src1_stub, src2_stub, dst_stub;
    int    coi;    

    CV_CALL( src1 = cvGetMat( leftImage, &src1_stub, &coi ));
    if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" );
    CV_CALL( src2 = cvGetMat( rightImage, &src2_stub, &coi ));
    if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" );    
    CV_CALL( dst = cvGetMat( depthImage, &dst_stub, &coi ));
    if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" );

    // check args 
    if( CV_MAT_TYPE( src1->type ) != CV_8UC1 || 
        CV_MAT_TYPE( src2->type ) != CV_8UC1 ||        
        CV_MAT_TYPE( dst->type ) != CV_8UC1) CV_ERROR(CV_StsUnsupportedFormat,
                        "All images must be single-channel and have 8u" );    

    if( !CV_ARE_SIZES_EQ( src1, src2 ) || !CV_ARE_SIZES_EQ( src1, dst ) )
            CV_ERROR( CV_StsUnmatchedSizes, "" );
    
    if( maxDisparity <= 0 || maxDisparity >= src1->width || maxDisparity > 255 )
        CV_ERROR(CV_StsOutOfRange, 
                 "parameter /maxDisparity/ is out of range");
    
    if( mode == CV_DISPARITY_BIRCHFIELD )
    {
        if( param1 == CV_UNDEF_SC_PARAM ) param1 = CV_IDP_BIRCHFIELD_PARAM1;
        if( param2 == CV_UNDEF_SC_PARAM ) param2 = CV_IDP_BIRCHFIELD_PARAM2;
        if( param3 == CV_UNDEF_SC_PARAM ) param3 = CV_IDP_BIRCHFIELD_PARAM3;
        if( param4 == CV_UNDEF_SC_PARAM ) param4 = CV_IDP_BIRCHFIELD_PARAM4;
        if( param5 == CV_UNDEF_SC_PARAM ) param5 = CV_IDP_BIRCHFIELD_PARAM5;

        CV_CALL( icvFindStereoCorrespondenceByBirchfieldDP( src1->data.ptr, 
            src2->data.ptr, dst->data.ptr, 
            cvGetMatSize( src1 ), src1->step,
            maxDisparity, (float)param1, (float)param2, (float)param3, 
            (float)param4, (float)param5 ) );
    }
    else
    {
        CV_ERROR( CV_StsBadArg, "Unsupported mode of function" );
    }

    __END__; 
}

/* End of file. */