/*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 "precomp.hpp"
#include "_vm.h"
#include <stdlib.h>
#include <assert.h>


/*======================================================================================*/

CvStatus
icvDynamicCorrespond( int *first,       /* first sequence of runs */
                      /* s0|w0|s1|w1|...|s(n-1)|w(n-1)|sn */
                      int first_runs,   /* number of runs */
                      int *second,      /* second sequence of runs */
                      int second_runs, int *first_corr, /* s0'|e0'|s1'|e1'|... */
                      int *second_corr )
{

    float Pd, Fi, S;
    float Occlusion;
    float *costTable;
    uchar *matchEdges;
    int prev;
    int curr;
    int baseIndex;
    int i, j;
    int i_1, j_1;
    int n;
    int l_beg, r_beg, l_end, r_end, l_len, r_len;
    int first_curr;
    int second_curr;
    int l_color, r_color;
    int len_color;
    float cost, cost1;
    float min1, min2, min3;
    float cmin;
    uchar cpath;
    int row_size;

    /* Test arguments for errors */

    if( (first == 0) ||
        (first_runs < 1) ||
        (second == 0) || (second_runs < 1) || (first_corr == 0) || (second_corr == 0) )

        return CV_BADFACTOR_ERR;


    Pd = 0.95f;
    Fi = (float) CV_PI;
    S = 1;

    Occlusion = (float) log( Pd * Fi / ((1 - Pd) * sqrt( fabs( (CV_PI * 2) * (1. / S) ))));

    costTable = (float *)cvAlloc( (first_runs + 1) * (second_runs + 1) * sizeof( float ));

    if( costTable == 0 )
        return CV_OUTOFMEM_ERR;

    matchEdges = (uchar *)cvAlloc( (first_runs + 1) * (second_runs + 1) * sizeof( uchar ));

    if( matchEdges == 0 )
    {
        cvFree( &costTable );
        return CV_OUTOFMEM_ERR;
    }

    row_size = first_runs + 1;

    /* ============= Fill costTable ============= */

    costTable[0] = 0.0f;

    /* Fill upper line in the cost Table */

    prev = first[0];
    curr = 2;

    for( n = 0; n < first_runs; n++ )
    {

        l_end = first[curr];
        curr += 2;
        costTable[n + 1] = costTable[n] + Occlusion * (l_end - prev);
        prev = l_end;

    }                           /* for */

    /* Fill lefter line in the cost Table */

    prev = second[0];
    curr = 2;
    baseIndex = 0;

    for( n = 0; n < second_runs; n++ )
    {

        l_end = second[curr];
        curr += 2;
        costTable[baseIndex + row_size] = costTable[baseIndex] + Occlusion * (l_end - prev);
        baseIndex += row_size;
        prev = l_end;

    }                           /* for */

    /* Count costs in the all rest cells */

    first_curr = 0;
    second_curr = 0;

    for( i = 1; i <= first_runs; i++ )
    {
        for( j = 1; j <= second_runs; j++ )
        {

            first_curr = (i - 1) * 2;
            second_curr = (j - 1) * 2;

            l_beg = first[first_curr];
            first_curr++;
            l_color = first[first_curr];
            first_curr++;
            l_end = first[first_curr];
            l_len = l_end - l_beg + 1;

            r_beg = second[second_curr];
            second_curr++;
            r_color = second[second_curr];
            second_curr++;
            r_end = second[second_curr];
            r_len = r_end - r_beg + 1;

            i_1 = i - 1;
            j_1 = j - 1;

            if( r_len == l_len )
            {
                cost = 0;
            }
            else
            {

                if( r_len > l_len )
                {
                    cost = (float) (r_len * r_len - l_len * l_len) * (1 / (r_len * l_len));
                }
                else
                {
                    cost = (float) (l_len * l_len - r_len * r_len) * (1 / (r_len * l_len));
                }
            }                   /* if */

            len_color = r_color - l_color;

            cost1 = (float) ((len_color * len_color) >> 2);

            min2 = costTable[i_1 + j * row_size] + Occlusion * l_len;

            min3 = costTable[i + j_1 * row_size] + Occlusion * r_len;

            min1 = costTable[i_1 + j_1 * row_size] + cost + (float) cost1;

            if( min1 < min2 )
            {

                if( min1 < min3 )
                {
                    cmin = min1;
                    cpath = 1;
                }
                else
                {
                    cmin = min3;
                    cpath = 3;
                }               /* if */

            }
            else
            {

                if( min2 < min3 )
                {
                    cmin = min2;
                    cpath = 2;
                }
                else
                {
                    cmin = min3;
                    cpath = 3;
                }               /* if */

            }                   /* if */

            costTable[i + j * row_size] = cmin;
            matchEdges[i + j * row_size] = cpath;
        }                       /* for */
    }                           /* for */

    /* =========== Reconstruct the Path =========== */

    i = first_runs;
    j = second_runs;

    first_curr = i * 2 - 2;
    second_curr = j * 2 - 2;


    while( i > 0 && j > 0 )
    {

        /* Connect begins */
        switch (matchEdges[i + j * row_size])
        {

        case 1:                /* to diagonal */

            first_corr[first_curr] = second[second_curr];
            first_corr[first_curr + 1] = second[second_curr + 2];
            second_corr[second_curr] = first[first_curr];
            second_corr[second_curr + 1] = first[first_curr + 2];

            first_curr -= 2;
            second_curr -= 2;
            i--;
            j--;

            break;

        case 2:                /* to left */

            first_corr[first_curr] = second[second_curr + 2];
            first_corr[first_curr + 1] = second[second_curr + 2];

            first_curr -= 2;
            i--;

            break;

        case 3:                /* to up */

            second_corr[second_curr] = first[first_curr + 2];
            second_corr[second_curr + 1] = first[first_curr + 2];

            second_curr -= 2;
            j--;

            break;
        }                       /* switch */
    }                           /* while */

    /* construct rest of horisontal path if its need */
    while( i > 0 )
    {

        first_corr[first_curr] = second[second_curr + 2];       /* connect to begin */
        first_corr[first_curr + 1] = second[second_curr + 2];   /* connect to begin */

        first_curr -= 2;
        i--;

    }                           /* while */

    /* construct rest of vertical path if its need */
    while( j > 0 )
    {

        second_corr[second_curr] = first[first_curr + 2];
        second_corr[second_curr + 1] = first[first_curr + 2];

        second_curr -= 2;
        j--;

    }                           /* while */

    cvFree( &costTable );
    cvFree( &matchEdges );

    return CV_NO_ERR;
}                               /* icvDynamicCorrespond */


/*======================================================================================*/

static CvStatus
icvDynamicCorrespondMulti( int lines,   /* number of scanlines */
                           int *first,  /* s0|w0|s1|w1|...s(n-1)|w(n-1)|sn */
                           int *first_runs,     /* numbers of runs */
                           int *second, int *second_runs, int *first_corr,      /* s0'|e0'|s1'|e1'|... */
                           int *second_corr )
{
    CvStatus error;

    int currFirst;
    int currSecond;
    int currFirstCorr;
    int currSecondCorr;
    int n;

    /* Test errors */

    if( (lines < 1) ||
        (first == 0) ||
        (first_runs == 0) ||
        (second == 0) || (second_runs == 0) || (first_corr == 0) || (second_corr == 0) )
        return CV_BADFACTOR_ERR;

    currFirst = 0;
    currSecond = 0;
    currFirstCorr = 0;
    currSecondCorr = 0;

    for( n = 0; n < lines; n++ )
    {

        error = icvDynamicCorrespond( &(first[currFirst]),
                                      first_runs[n],
                                      &(second[currSecond]),
                                      second_runs[n],
                                      &(first_corr[currFirstCorr]),
                                      &(second_corr[currSecondCorr]) );

        if( error != CV_NO_ERR )
            return error;

        currFirst += first_runs[n] * 2 + 1;
        currSecond += second_runs[n] * 2 + 1;
        currFirstCorr += first_runs[n] * 2;
        currSecondCorr += second_runs[n] * 2;

    }

    return CV_NO_ERR;

}                               /* icvDynamicCorrespondMulti */


/*======================================================================================*/

/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: cvDynamicCorrespondMulti
//    Purpose: The functions 
//    Context:
//    Parameters:  
//
//    Notes:  
//F*/
CV_IMPL void
cvDynamicCorrespondMulti( int lines,    /* number of scanlines */
                          int *first,   /* s0|w0|s1|w1|...s(n-1)|w(n-1)|sn */
                          int *first_runs,      /* numbers of runs */
                          int *second, int *second_runs, int *first_corr,       /* s0'|e0'|s1'|e1'|... */
                          int *second_corr )
{
    IPPI_CALL( icvDynamicCorrespondMulti( lines,        /* number of scanlines */
                                          first,        /* s0|w0|s1|w1|...s(n-1)|w(n-1)|sn */
                                          first_runs,   /* numbers of runs */
                                          second, second_runs, first_corr,      /* s0'|e0'|s1'|e1'|... */
                                          second_corr ));
}