/*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 "SubdivMorpher.h"
#include <malloc.h>
#include <assert.h>
#include <memory.h>
#include <stdio.h>
#include <math.h>

#define MAX_DIFFERENCE 20
#define COEFF_OCC 30
#define COEFF_MATCH 30
#define COEFF_MATCH_2 64
#define END_OF_PATH 1000000000

#define MAX3(xxx1,xxx2,xxx3) ( ( ( (xxx1) > (xxx2) ) && ( (xxx1) > xxx3 ) ) ? (xxx1) : ( ( (xxx2) > (xxx3) ) ? (xxx2) : (xxx3) ) )
#define MAX2(xxx1,xxx2) ( ( (xxx1) > (xxx2) ) ? (xxx1) : (xxx2) )
#define MIN3(xxx1,xxx2,xxx3) ( ( ( (xxx1) < (xxx2) ) && ( (xxx1) < xxx3 ) ) ? (xxx1) : ( ( (xxx2) < (xxx3) ) ? (xxx2) : (xxx3) ) )
#define MIN2(xxx1,xxx2) ( ( (xxx1) < (xxx2) ) ? (xxx1) : (xxx2) )

// This function allocates memory for FindFullCorr function
//      width               - width of image
//      height              - height of image
//      type                - type of memory for allocate ( the size of memory depends on it )
//      maxPixelDifference  - maximum value of pixel difference on two images
int* CCvPixelToPixelMorpher::corrAlloc( int width, int height, corrArrayType type, int maxPixelDifference )
{
    int* pointer;
    switch( type )
    {
    case CORR_ARRAY:
        {
            pointer = ( int* ) malloc( width * height * 2 * sizeof( int ) );
        }
        break;
    case NUM_CORR_ARRAY:
        {
            pointer = ( int* ) malloc( height * sizeof( int ) );
        }
        break;
    case CORR_EDGES:
        {
            int i;
            int width_1 = width - 1;

            pointer = ( int* ) malloc( width * 2 * sizeof( int ) );
            for( i = 0; i < width; i ++ ) {
                pointer[ i * 2 ]      = i - maxPixelDifference;
                pointer[ i * 2 + 1 ]  = i + maxPixelDifference;
            } // for( i = 0; i < width; i ++ )
            for( i = 0; i < width; i ++ ) {
                if( pointer[ i * 2 ] < 0 ) {
                    pointer[ i * 2 ]      = 0;
                }
                if( pointer[ i * 2 + 1 ] >= width ) {
                    pointer[ i * 2 + 1 ]  = width_1;
                }
            } // for( i = 0; i < width; i ++ )

        } // case CORR_EDGES:
        break;
    case CORR_TEMP:
        {
            pointer = ( int* ) malloc( width * width * 4 * sizeof( int ) );
        }
        break;
    default:
        {
            pointer = 0;
        }

    } // switch( type )

    return pointer;
} // corrAlloc

// This function searches correspondences for full image
//      _leftImage              - pointer to 3-channel left image
//      _leftLineStep           - size of one line on the left image in bytes
//      _rightImage             - pointer to 3-channel right image
//      _rightLineStep          - size of one line on the right image in bytes
//      _corrArray              - output integer array of correspondences ( size (width)X(height)X(2) ).
//                                each element of its array consists of two integer values, one of
//                                which represents number of pixel on the left image and the other
//                                corresponding to it point on the right image
//      _numCorrArray           - output array with numbers of finded correspondences on each line
//      width                   - width of images
//      height                  - height of images
//      maxPixelDifference      - maximum value of pixel difference on two images
void CCvPixelToPixelMorpher::FindFullCorr( unsigned char* _leftImage,
                   int _leftLineStep,
                   unsigned char* _rightImage,
                   int _rightLineStep,
                   int* _corrArray,
                   int* _numCorrArray,
                   int width,
                   int height,
                   int maxPixelDifference
                 )
{
    //int     width2 = width * width;
    //int     tempMemorySize = width2 * 4 * sizeof( int );
    int*    tempMemory;

    tempMemory  = corrAlloc( width, height, CORR_TEMP );

    int*    edges   = corrAlloc( width, height, CORR_EDGES, maxPixelDifference );

    int j;

    // lines cycle - processes each line
    for( j = 0; j < height; j ++ )
    {
        FindCorr( _leftImage + _leftLineStep * j,
                  _rightImage + _rightLineStep * j,
                  _corrArray + width * 2 * j,
                  _numCorrArray + j,
                  width,
                  edges,
                  tempMemory
                );
    } // for( j = 0; j < height; j ++ )

    free( edges );
    free( tempMemory );

} // FindFullCorr

// This function searches correspondence for one line only
//      _leftLine
void CCvPixelToPixelMorpher::FindCorr( unsigned char* _leftLine,
               unsigned char* _rightLine,
               int* _corrArray,
               int* numCorrArray,
               int width,
               int* edges,
               int* tempArray
             )
{
    int     width2      = width * width;
    int     width_1     = width - 1;
    int*    xArray      = tempArray;                // array, which shows the best previous point in path
    int*    yArray      = tempArray + width2;       // array, which shows the best previous point in path
    int*    costArray   = tempArray + width2 * 2;   // array of costs table
    int*    distArray   = tempArray + width2 * 3;   // array of distances between points on two images

    int blockSize = width2 * sizeof( int );
    memset( ( void* ) xArray, 0, blockSize );               // filling by zero
    memset( ( void* ) yArray, 0, blockSize );               // filling by zero
    memset( ( void* ) distArray, 0, blockSize );            // filling by zero

    int i;
    int j;
    int iTemp;
    int jTemp;

    int currCost;
    int bestHorCost;

    int srcOffset;
    int destOffset;
    int futureCost;

    // filling cost table by very big value ( initialization )
    for( j = 0; j < width; j ++ )
    {
        for( i = edges[ j * 2 ]; i <= edges[ j * 2 + 1 ]; i ++ )
        {
            costArray[ j * width + i ] = 2000000000;    // filling by very big value

        } // for( i = edges[ j * 2 ]; i < edges[ j * 2 + 1 ]; i ++ )

    } // for( j = 0; j < width; j ++ )

    // computing distances for all pairs of points ( between points on left image and on right image )
    for( j = 0; j < width; j ++ )
    {
        for( i = edges[ j * 2 ]; i <= edges[ j * 2 + 1 ]; i ++ )
        {
            distArray[ j * width + i ] = distance( _leftLine, _rightLine, width, j, i );

        } // for( i = edges[ j * 2 ]; i < edges[ j * 2 + 1 ]; i ++ )

    } // for( j = 0; j < width; j ++ )

    // filling left upper corner
    for( i = edges[ 0 ]; i <= edges[ 1 ]; i ++ )
    {
        // horizontal line
        xArray[ i ]             = END_OF_PATH;
        costArray[ i ]          = *( distArray + i );
        // vertical line
        xArray[ i * width ]     = END_OF_PATH;
        costArray[ i * width ]  = *( distArray + i * width );
    } // for( i = edges[ 0 ]; i <= edges[ 1 ]; i ++ )

    // normal dynamic programming
    for( j = 0; j < width_1; j ++ )
    {
        bestHorCost = costArray[ j * width + edges[ j * 2 ] ];
        //bestHorCost = 2000000000;
        for( i = edges[ j * 2 ]; i <= edges[ j * 2 + 1 ]; i ++ )
        {
            srcOffset   = j * width + i;
            currCost    = costArray[ srcOffset ];
            if( currCost - COEFF_MATCH < bestHorCost + COEFF_OCC )
            {
                bestHorCost = currCost;

                // filling the next horizontal line

                // first point
                if( i != width - 1 )
                {
                    destOffset  = j * width + ( i + width + 1 );
                    futureCost  = currCost + distArray[ destOffset ];
                    if( ( xArray[ srcOffset ] == i - 1 ) && ( yArray[ srcOffset ] == j - 1 ) ) {
                        futureCost -= COEFF_MATCH;
                    }
                    if( ( xArray[ srcOffset ] != i - 1 ) || ( yArray[ srcOffset ] != j - 1 ) ) {
                        futureCost -= COEFF_MATCH_2;
                    }
                    if( costArray[ destOffset ] >= futureCost ) {
                        costArray[ destOffset ]     = futureCost;
                        xArray[ destOffset ]        = i;
                        yArray[ destOffset ]        = j;
                    }
                }
                // residuary points ( if they exist )
                for( iTemp = i + 2; iTemp <= edges[ j * 2 + 3 ]; iTemp ++ )
                {
                    destOffset  = j * width + ( iTemp + width );
                    futureCost  = currCost + COEFF_OCC + distArray[ destOffset ];
                    if( costArray[ destOffset ] >= futureCost ) {
                        costArray[ destOffset ]     = futureCost;
                        xArray[ destOffset ]        = i;
                        yArray[ destOffset ]        = j;
                    }

                } // for( iTemp = i + 2; iTemp <= edges[ j * 2 + 3 ]; iTemp ++ )

            } // if( currCost - COEFF_MATCH < bestHorCost + COEFF_OCC )

            // filling the next vertical line
            if( i != width_1 )
            {
                for( jTemp = j + 2; jTemp <= edges[ i * 2 + 3 ]; jTemp ++ )
                {
                    destOffset  = jTemp * width + ( i + 1 );
                    futureCost  = currCost + COEFF_OCC + distArray[ destOffset ];