/*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*/

/*
    Partially based on Yossi Rubner code:
    =========================================================================
    emd.c

    Last update: 3/14/98

    An implementation of the Earth Movers Distance.
    Based of the solution for the Transportation problem as described in
    "Introduction to Mathematical Programming" by F. S. Hillier and 
    G. J. Lieberman, McGraw-Hill, 1990.

    Copyright (C) 1998 Yossi Rubner
    Computer Science Department, Stanford University
    E-Mail: rubner@cs.stanford.edu   URL: http://vision.stanford.edu/~rubner
    ==========================================================================
*/
#include "_cv.h"

#define MAX_ITERATIONS 500
#define CV_EMD_INF   ((float)1e20)
#define CV_EMD_EPS   ((float)1e-5)

/* CvNode1D is used for lists, representing 1D sparse array */
typedef struct CvNode1D
{
    float val;
    struct CvNode1D *next;
}
CvNode1D;

/* CvNode2D is used for lists, representing 2D sparse matrix */
typedef struct CvNode2D
{
    float val;
    struct CvNode2D *next[2];  /* next row & next column */
    int i, j;
}
CvNode2D;


typedef struct CvEMDState
{
    int ssize, dsize;

    float **cost;
    CvNode2D *_x;
    CvNode2D *end_x;
    CvNode2D *enter_x;
    char **is_x;

    CvNode2D **rows_x;
    CvNode2D **cols_x;

    CvNode1D *u;
    CvNode1D *v;

    int* idx1;
    int* idx2;

    /* find_loop buffers */
    CvNode2D **loop;
    char *is_used;

    /* russel buffers */
    float *s;
    float *d;
    float **delta;

    float weight, max_cost;
    char *buffer;
}
CvEMDState;

/* static function declaration */
static CvStatus icvInitEMD( const float *signature1, int size1,
                            const float *signature2, int size2,
                            int dims, CvDistanceFunction dist_func, void *user_param,
                            const float* cost, int cost_step,
                            CvEMDState * state, float *lower_bound,
                            char *local_buffer, int local_buffer_size );

static CvStatus icvFindBasicVariables( float **cost, char **is_x,
                                       CvNode1D * u, CvNode1D * v, int ssize, int dsize );

static float icvIsOptimal( float **cost, char **is_x,
                           CvNode1D * u, CvNode1D * v,
                           int ssize, int dsize, CvNode2D * enter_x );

static void icvRussel( CvEMDState * state );


static CvStatus icvNewSolution( CvEMDState * state );
static int icvFindLoop( CvEMDState * state );

static void icvAddBasicVariable( CvEMDState * state,
                                 int min_i, int min_j,
                                 CvNode1D * prev_u_min_i,
                                 CvNode1D * prev_v_min_j,
                                 CvNode1D * u_head );

static float icvDistL2( const float *x, const float *y, void *user_param );
static float icvDistL1( const float *x, const float *y, void *user_param );
static float icvDistC( const float *x, const float *y, void *user_param );

/* The main function */
CV_IMPL float
cvCalcEMD2( const CvArr* signature_arr1,
            const CvArr* signature_arr2,
            int dist_type,
            CvDistanceFunction dist_func,
            const CvArr* cost_matrix,
            CvArr* flow_matrix,
            float *lower_bound,
            void *user_param )
{
    char local_buffer[16384];
    char *local_buffer_ptr = (char *)cvAlignPtr(local_buffer,16);
    CvEMDState state;
    float emd = 0;

    CV_FUNCNAME( "cvCalcEMD2" );

    memset( &state, 0, sizeof(state));

    __BEGIN__;

    double total_cost = 0;
    CvStatus result = CV_NO_ERR;
    float eps, min_delta;
    CvNode2D *xp = 0;
    CvMat sign_stub1, *signature1 = (CvMat*)signature_arr1;
    CvMat sign_stub2, *signature2 = (CvMat*)signature_arr2;
    CvMat cost_stub, *cost = &cost_stub;
    CvMat flow_stub, *flow = (CvMat*)flow_matrix;
    int dims, size1, size2;

    CV_CALL( signature1 = cvGetMat( signature1, &sign_stub1 ));
    CV_CALL( signature2 = cvGetMat( signature2, &sign_stub2 ));

    if( signature1->cols != signature2->cols )
        CV_ERROR( CV_StsUnmatchedSizes, "The arrays must have equal number of columns (which is number of dimensions but 1)" );

    dims = signature1->cols - 1;
    size1 = signature1->rows;
    size2 = signature2->rows;

    if( !CV_ARE_TYPES_EQ( signature1, signature2 ))
        CV_ERROR( CV_StsUnmatchedFormats, "The array must have equal types" );

    if( CV_MAT_TYPE( signature1->type ) != CV_32FC1 )
        CV_ERROR( CV_StsUnsupportedFormat, "The signatures must be 32fC1" );

    if( flow )
    {
        CV_CALL( flow = cvGetMat( flow, &flow_stub ));

        if( flow->rows != size1 || flow->cols != size2 )
            CV_ERROR( CV_StsUnmatchedSizes,
            "The flow matrix size does not match to the signatures' sizes" );

        if( CV_MAT_TYPE( flow->type ) != CV_32FC1 )
            CV_ERROR( CV_StsUnsupportedFormat, "The flow matrix must be 32fC1" );
    }

    cost->data.fl = 0;
    cost->step = 0;

    if( dist_type < 0 )
    {
        if( cost_matrix )
        {
            if( dist_func )
                CV_ERROR( CV_StsBadArg,
                "Only one of cost matrix or distance function should be non-NULL in case of user-defined distance" );

            if( lower_bound )
                CV_ERROR( CV_StsBadArg,
                "The lower boundary can not be calculated if the cost matrix is used" );

            CV_CALL( cost = cvGetMat( cost_matrix, &cost_stub ));
            if( cost->rows != size1 || cost->cols != size2 )
                CV_ERROR( CV_StsUnmatchedSizes,
                "The cost matrix size does not match to the signatures' sizes" );

            if( CV_MAT_TYPE( cost->type ) != CV_32FC1 )
                CV_ERROR( CV_StsUnsupportedFormat, "The cost matrix must be 32fC1" );
        }
        else if( !dist_func )
            CV_ERROR( CV_StsNullPtr, "In case of user-defined distance Distance function is undefined" );
    }
    else
    {
        if( dims == 0 )
            CV_ERROR( CV_StsBadSize,
            "Number of dimensions can be 0 only if a user-defined metric is used" );
        user_param = (void *) (size_t)dims;
        switch (dist_type)
        {
        case CV_DIST_L1:
            dist_func = icvDistL1;
            break;
        case CV_DIST_L2:
            dist_func = icvDistL2;
            break;
        case CV_DIST_C:
            dist_func = icvDistC;
            break;
        default:
            CV_ERROR( CV_StsBadFlag, "Bad or unsupported metric type" );
        }
    }

    IPPI_CALL( result = icvInitEMD( signature1->data.fl, size1,
                                    signature2->data.fl, size2,
                                    dims, dist_func, user_param,
                                    cost->data.fl, cost->step,
                                    &state, lower_bound, local_buffer_ptr,
                                    sizeof( local_buffer ) - 16 ));

    if( result > 0 && lower_bound )
    {
        emd = *lower_bound;
        EXIT;
    }

    eps = CV_EMD_EPS * state.max_cost;

    /* if ssize = 1 or dsize = 1 then we are done, else ... */
    if( state.ssize > 1 && state.dsize > 1 )
    {
        int itr;

        for( itr = 1; itr < MAX_ITERATIONS; itr++ )
        {
            /* find basic variables */
            result = icvFindBasicVariables( state.cost, state.is_x,
                                            state.u, state.v, state.ssize, state.dsize );
            if( result < 0 )
                break;

            /* check for optimality */
            min_delta = icvIsOptimal( state.cost, state.is_x,
                                      state.u, state.v,
                                      state.ssize, state.dsize, state.enter_x );

            if( min_delta == CV_EMD_INF )
            {
                CV_ERROR( CV_StsNoConv, "" );
            }

            /* if no negative deltamin, we found the optimal solution */
            if( min_delta >= -eps )
                break;

            /* improve solution */
            IPPI_CALL( icvNewSolution( &state ));
        }
    }

    /* compute the total flow */
    for( xp = state._x; xp < state.end_x; xp++ )
    {
        float val = xp->val;
        int i = xp->i;
        int j = xp->j;
        int ci = state.idx1[i];
        int cj = state.idx2[j];

        if( xp != state.enter_x && ci >= 0 && cj >= 0 )
        {
            total_cost += (double)val * state.cost[i][j];
            if( flow )
                ((float*)(flow->data.ptr + flow->step*ci))[cj] = val;
        }
    }

    emd = (float) (total_cost / state.weight);

    __END__;

    if( state.buffer && state.buffer != local_buffer_ptr )
        cvFree( &state.buffer );

    return emd;
}


/************************************************************************************\
*          initialize structure, allocate buffers and generate initial golution      *
\************************************************************************************/
static CvStatus
icvInitEMD( const float* signature1, int size1,
            const float* signature2, int size2,
            int dims, CvDistanceFunction dist_func, void* user_param,
            const float* cost, int cost_step,
            CvEMDState* state, float* lower_bound,
            char* local_buffer, int local_buffer_size )
{
    float s_sum = 0, d_sum = 0, diff;
    int i, j;
    int ssize = 0, dsize = 0;
    int equal_sums = 1;
    int buffer_size;
    float max_cost = 0;
    char *buffer, *buffer_end;

    memset( state, 0, sizeof( *state ));
    assert( cost_step % sizeof(float) == 0 );
    cost_step /= sizeof(float);

    /* calculate buffer size */
    buffer_size = (size1+1) * (size2+1) * (sizeof( float ) +    /* cost */
                                   sizeof( char ) +     /* is_x */
                                   sizeof( float )) +   /* delta matrix */
        (size1 + size2 + 2) * (sizeof( CvNode2D ) + /* _x */
                           sizeof( CvNode2D * ) +  /* cols_x & rows_x */
                           sizeof( CvNode1D ) + /* u & v */
                           sizeof( float ) + /* s & d */
                           sizeof( int ) + sizeof(CvNode2D*)) +  /* idx1 & idx2 */ 
        (size1+1) * (sizeof( float * ) + sizeof( char * ) + /* rows pointers for */
                 sizeof( float * )) + 256;      /*  cost, is_x and delta */

    if( buffer_size < (int) (dims * 2 * sizeof( float )))
    {
        buffer_size = dims * 2 * sizeof( float );
    }

    /* allocate buffers */
    if( local_buffer != 0 && local_buffer_size >= buffer_size )
    {
        buffer = local_buffer;
    }
    else
    {
        buffer = (char*)cvAlloc( buffer_size );
        if( !buffer )
            return CV_OUTOFMEM_ERR;
    }

    state->buffer = buffer;
    buffer_end = buffer + buffer_size;

    state->idx1 = (int*) buffer;
    buffer += (size1 + 1) * sizeof( int );

    state->idx2 = (int*) buffer;
    buffer += (size2 + 1) * sizeof( int );

    state->s = (float *) buffer;
    buffer += (size1 + 1) * sizeof( float );

    state->d = (float *) buffer;
    buffer += (size2 + 1) * sizeof( float );

    /* sum up the supply and demand */
    for( i = 0; i < size1; i++ )
    {
        float weight = signature1[i * (dims + 1)];

        if( weight > 0 )
        {
            s_sum += weight;
            state->s[ssize] = weight;
            state->idx1[ssize++] = i;
            
        }
        else if( weight < 0 )
            return CV_BADRANGE_ERR;
    }

    for( i = 0; i < size2; i++ )
    {
        float weight = signature2[i * (dims + 1)];

        if( weight > 0 )
        {
            d_sum += weight;
            state->d[dsize] = weight;
            state->idx2[dsize++] = i;
        }
        else if( weight < 0 )
            return CV_BADRANGE_ERR;
    }

    if( ssize == 0 || dsize == 0 )
        return CV_BADRANGE_ERR;

    /* if supply different than the demand, add a zero-cost dummy cluster */
    diff = s_sum - d_sum;
    if( fabs( diff ) >= CV_EMD_EPS * s_sum )
    {
        equal_sums = 0;
        if( diff < 0 )
        {
            state->s[ssize] = -diff;
            state->idx1[ssize++] = -1;
        }    
        else
        {
            state->d[dsize] = diff;
            state->idx2[dsize++] = -1;
        }
    }

    state->ssize = ssize;
    state->dsize = dsize;
    state->weight = s_sum > d_sum ? s_sum : d_sum;

    if( lower_bound && equal_sums )     /* check lower bound */
    {
        int sz1 = size1 * (dims + 1), sz2 = size2 * (dims + 1);
        float lb = 0;

        float* xs = (float *) buffer;
        float* xd = xs + dims;

        memset( xs, 0, dims*sizeof(xs[0]));
        memset( xd, 0, dims*sizeof(xd[0]));

        for( j = 0; j < sz1; j += dims + 1 )
        {
            float weight = signature1[j];
            for( i = 0; i < dims; i++ )
                xs[i] += signature1[j + i + 1] * weight;
        }

        for( j = 0; j < sz2; j += dims + 1 )
        {
            float weight = signature2[j];
            for( i = 0; i < dims; i++ )
                xd[i] += signature2[j + i + 1] * weight;
        }

        lb = dist_func( xs, xd, user_param ) / state->weight;
        i = *lower_bound <= lb;
        *lower_bound = lb;
        if( i )
            return ( CvStatus ) 1;
    }

    /* assign pointers */
    state->is_used = (char *) buffer;
    /* init delta matrix */
    state->delta = (float **) buffer;
    buffer += ssize * sizeof( float * );

    for( i = 0; i < ssize; i++ )
    {
        state->delta[i] = (float *) buffer;
        buffer += dsize * sizeof( float );
    }

    state->loop = (CvNode2D **) buffer;
    buffer += (ssize + dsize + 1) * sizeof(CvNode2D*);

    state->_x = state->end_x = (CvNode2D *) buffer;
    buffer += (ssize + dsize) * sizeof( CvNode2D );

    /* init cost matrix */
    state->cost = (float **) buffer;
    buffer += ssize * sizeof( float * );

    /* compute the distance matrix */
    for( i = 0; i < ssize; i++ )
    {
        int ci = state->idx1[i];

        state->cost[i] = (float *) buffer;
        buffer += dsize * sizeof( float );

        if( ci >= 0 )
        {
            for( j = 0; j < dsize; j++ )
            {
                int cj = state->idx2[j];
                if( cj < 0 )
                    state->cost[i][j] = 0;
                else
                {
                    float val;
                    if( dist_func )
                    {
                        val = dist_func( signature1 + ci * (dims + 1) + 1,
                                         signature2 + cj * (dims + 1) + 1,
                                         user_param );
                    }
                    else
                    {
                        assert( cost );
                        val = cost[cost_step*ci + cj];
                    }
                    state->cost[i][j] = val;
                    if( max_cost < val )
                        max_cost = val;
                }
            }
        }
        else
        {
            for( j = 0; j < dsize; j++ )
                state->cost[i][j] = 0;
        }
    }

    state->max_cost = max_cost;
    
    memset( buffer, 0, buffer_end - buffer );

    state->rows_x = (CvNode2D **) buffer;
    buffer += ssize * sizeof( CvNode2D * );

    state->cols_x = (CvNode2D **) buffer;
    buffer += dsize * sizeof( CvNode2D * );

    state->u = (CvNode1D *) buffer;
    buffer += ssize * sizeof( CvNode1D );

    state->v = (CvNode1D *) buffer;
    buffer += dsize * sizeof( CvNode1D );

    /* init is_x matrix */
    state->is_x = (char **) buffer;
    buffer += ssize * sizeof( char * );

    for( i = 0; i < ssize; i++ )
    {
        state->is_x[i] = buffer;
        buffer += dsize;
    }

    assert( buffer <= buffer_end );

    icvRussel( state );

    state->enter_x = (state->end_x)++;
    return CV_NO_ERR;
}


/****************************************************************************************\
*                              icvFindBasicVariables                                   *
\****************************************************************************************/
static CvStatus
icvFindBasicVariables( float **cost, char **is_x,
                       CvNode1D * u, CvNode1D * v, int ssize, int dsize )
{
    int i, j, found;
    int u_cfound, v_cfound;
    CvNode1D u0_head, u1_head, *cur_u, *prev_u;
    CvNode1D v0_head, v1_head, *cur_v, *prev_v;

    /* initialize the rows list (u) and the columns list (v) */
    u0_head.next = u;
    for( i = 0; i < ssize; i++ )
    {
        u[i].next = u + i + 1;
    }
    u[ssize - 1].next = 0;
    u1_head.next = 0;

    v0_head.next = ssize > 1 ? v + 1 : 0;
    for( i = 1; i < dsize; i++ )
    {
        v[i].next = v + i + 1;
    }
    v[dsize - 1].next = 0;
    v1_head.next = 0;

    /* there are ssize+dsize variables but only ssize+dsize-1 independent equations,
       so set v[0]=0 */
    v[0].val = 0;
    v1_head.next = v;
    v1_head.next->next = 0;

    /* loop until all variables are found */
    u_cfound = v_cfound = 0;
    while( u_cfound < ssize || v_cfound < dsize )
    {
        found = 0;
        if( v_cfound < dsize )
        {
            /* loop over all marked columns */
            prev_v = &v1_head;

            for( found |= (cur_v = v1_head.next) != 0; cur_v != 0; cur_v = cur_v->next )
            {
                float cur_v_val = cur_v->val;

                j = (int)(cur_v - v);
                /* find the variables in column j */
                prev_u = &u0_head;
                for( cur_u = u0_head.next; cur_u != 0; )
                {
                    i = (int)(cur_u - u);
                    if( is_x[i][j] )
                    {
                        /* compute u[i] */
                        cur_u->val = cost[i][j] - cur_v_val;
                        /* ...and add it to the marked list */
                        prev_u->next = cur_u->next;
                        cur_u->next = u1_head.next;
                        u1_head.next = cur_u;
                        cur_u = prev_u->next;
                    }
                    else
                    {
                        prev_u = cur_u;
                        cur_u = cur_u->next;
                    }
                }
                prev_v->next = cur_v->next;
                v_cfound++;
            }
        }

        if( u_cfound < ssize )
        {
            /* loop over all marked rows */
            prev_u = &u1_head;
            for( found |= (cur_u = u1_head.next) != 0; cur_u != 0; cur_u = cur_u->next )
            {
                float cur_u_val = cur_u->val;
                float *_cost;
                char *_is_x;

                i = (int)(cur_u - u);
                _cost = cost[i];
                _is_x = is_x[i];
                /* find the variables in rows i */
                prev_v = &v0_head;
                for( cur_v = v0_head.next; cur_v != 0; )
                {
                    j = (int)(cur_v - v);
                    if( _is_x[j] )
                    {
                        /* compute v[j] */
                        cur_v->val = _cost[j] - cur_u_val;
                        /* ...and add it to the marked list */
                        prev_v->next = cur_v->next;
                        cur_v->next = v1_head.next;
                        v1_head.next = cur_v;
                        cur_v = prev_v->next;
                    }
                    else
                    {
                        prev_v = cur_v;
                        cur_v = cur_v->next;
                    }
                }
                prev_u->next = cur_u->next;
                u_cfound++;
            }
        }

        if( !found )
        {
            return CV_NOTDEFINED_ERR;
        }
    }

    return CV_NO_ERR;
}


/****************************************************************************************\
*                                   icvIsOptimal                                       *
\****************************************************************************************/
static float
icvIsOptimal( float **cost, char **is_x,
              CvNode1D * u, CvNode1D * v, int ssize, int dsize, CvNode2D * enter_x )
{
    float delta, min_delta = CV_EMD_INF;
    int i, j, min_i = 0, min_j = 0;

    /* find the minimal cij-ui-vj over all i,j */
    for( i = 0; i < ssize; i++ )
    {
        float u_val = u[i].val;
        float *_cost = cost[i];
        char *_is_x = is_x[i];

        for( j = 0; j < dsize; j++ )
        {
            if( !_is_x[j] )
            {
                delta = _cost[j] - u_val - v[j].val;
                if( min_delta > delta )
                {
                    min_delta = delta;
                    min_i = i;
                    min_j = j;
                }
            }
        }
    }

    enter_x->i = min_i;
    enter_x->j = min_j;

    return min_delta;
}

/****************************************************************************************\
*                                   icvNewSolution                                     *
\****************************************************************************************/
static CvStatus
icvNewSolution( CvEMDState * state )
{
    int i, j;
    float min_val = CV_EMD_INF;
    int steps;
    CvNode2D head, *cur_x, *next_x, *leave_x = 0;
    CvNode2D *enter_x = state->enter_x;
    CvNode2D **loop = state->loop;

    /* enter the new basic variable */
    i = enter_x->i;
    j = enter_x->j;
    state->is_x[i][j] = 1;
    enter_x->next[0] = state->rows_x[i];
    enter_x->next[1] = state->cols_x[j];
    enter_x->val = 0;
    state->rows_x[i] = enter_x;
    state->cols_x[j] = enter_x;

    /* find a chain reaction */
    steps = icvFindLoop( state );

    if( steps == 0 )
        return CV_NOTDEFINED_ERR;

    /* find the largest value in the loop */
    for( i = 1; i < steps; i += 2 )
    {
        float temp = loop[i]->val;

        if( min_val > temp )
        {
            leave_x = loop[i];
            min_val = temp;
        }
    }

    /* update the loop */
    for( i = 0; i < steps; i += 2 )
    {
        float temp0 = loop[i]->val + min_val;
        float temp1 = loop[i + 1]->val - min_val;

        loop[i]->val = temp0;
        loop[i + 1]->val = temp1;
    }

    /* remove the leaving basic variable */
    i = leave_x->i;
    j = leave_x->j;
    state->is_x[i][j] = 0;

    head.next[0] = state->rows_x[i];
    cur_x = &head;
    while( (next_x = cur_x->next[0]) != leave_x )
    {
        cur_x = next_x;
        assert( cur_x );
    }
    cur_x->next[0] = next_x->next[0];
    state->rows_x[i] = head.next[0];

    head.next[1] = state->cols_x[j];
    cur_x = &head;
    while( (next_x = cur_x->next[1]) != leave_x )
    {
        cur_x = next_x;
        assert( cur_x );
    }
    cur_x->next[1] = next_x->next[1];
    state->cols_x[j] = head.next[1];

    /* set enter_x to be the new empty slot */
    state->enter_x = leave_x;

    return CV_NO_ERR;
}


/****************************************************************************************\
*                                    icvFindLoop                                       *
\****************************************************************************************/
static int
icvFindLoop( CvEMDState * state )
{
    int i, steps = 1;
    CvNode2D *new_x;
    CvNode2D **loop = state->loop;
    CvNode2D *enter_x = state->enter_x, *_x = state->_x;
    char *is_used = state->is_used;

    memset( is_used, 0, state->ssize + state->dsize );

    new_x = loop[0] = enter_x;
    is_used[enter_x - _x] = 1;
    steps = 1;

    do
    {
        if( (steps & 1) == 1 )
        {
            /* find an unused x in the row */
            new_x = state->rows_x[new_x->i];
            while( new_x != 0 && is_used[new_x - _x] )
                new_x = new_x->next[0];
        }
        else
        {
            /* find an unused x in the column, or the entering x */
            new_x = state->cols_x[new_x->j];
            while( new_x != 0 && is_used[new_x - _x] && new_x != enter_x )
                new_x = new_x->next[1];
            if( new_x == enter_x )
                break;
        }

        if( new_x != 0 )        /* found the next x */
        {
            /* add x to the loop */
            loop[steps++] = new_x;
            is_used[new_x - _x] = 1;
        }
        else                    /* didn't find the next x */
        {
            /* backtrack */
            do
            {
                i = steps & 1;
                new_x = loop[steps - 1];
                do
                {
                    new_x = new_x->next[i];
                }
                while( new_x != 0 && is_used[new_x - _x] );

                if( new_x == 0 )
                {
                    is_used[loop[--steps] - _x] = 0;
                }
            }
            while( new_x == 0 && steps > 0 );

            is_used[loop[steps - 1] - _x] = 0;
            loop[steps - 1] = new_x;
            is_used[new_x - _x] = 1;
        }
    }
    while( steps > 0 );

    return steps;
}


/****************************************************************************************\
*                                        icvRussel                                     *
\****************************************************************************************/
static void
icvRussel( CvEMDState * state )
{
    int i, j, min_i = -1, min_j = -1;
    float min_delta, diff;
    CvNode1D u_head, *cur_u, *prev_u;
    CvNode1D v_head, *cur_v, *prev_v;
    CvNode1D *prev_u_min_i = 0, *prev_v_min_j = 0, *remember;
    CvNode1D *u = state->u, *v = state->v;
    int ssize = state->ssize, dsize = state->dsize;
    float eps = CV_EMD_EPS * state->max_cost;
    float **cost = state->cost;
    float **delta = state->delta;

    /* initialize the rows list (ur), and the columns list (vr) */
    u_head.next = u;
    for( i = 0; i < ssize; i++ )
    {
        u[i].next = u + i + 1;
    }
    u[ssize - 1].next = 0;

    v_head.next = v;
    for( i = 0; i < dsize; i++ )
    {
        v[i].val = -CV_EMD_INF;
        v[i].next = v + i + 1;
    }
    v[dsize - 1].next = 0;

    /* find the maximum row and column values (ur[i] and vr[j]) */
    for( i = 0; i < ssize; i++ )
    {
        float u_val = -CV_EMD_INF;
        float *cost_row = cost[i];

        for( j = 0; j < dsize; j++ )
        {
            float temp = cost_row[j];

            if( u_val < temp )
                u_val = temp;
            if( v[j].val < temp )
                v[j].val = temp;
        }
        u[i].val = u_val;
    }

    /* compute the delta matrix */
    for( i = 0; i < ssize; i++ )
    {
        float u_val = u[i].val;
        float *delta_row = delta[i];
        float *cost_row = cost[i];

        for( j = 0; j < dsize; j++ )
        {
            delta_row[j] = cost_row[j] - u_val - v[j].val;
        }
    }

    /* find the basic variables */
    do
    {
        /* find the smallest delta[i][j] */
        min_i = -1;
        min_delta = CV_EMD_INF;
        prev_u = &u_head;
        for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
        {
            i = (int)(cur_u - u);
            float *delta_row = delta[i];

            prev_v = &v_head;
            for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
            {
                j = (int)(cur_v - v);
                if( min_delta > delta_row[j] )
                {
                    min_delta = delta_row[j];
                    min_i = i;
                    min_j = j;
                    prev_u_min_i = prev_u;
                    prev_v_min_j = prev_v;
                }
                prev_v = cur_v;
            }
            prev_u = cur_u;
        }

        if( min_i < 0 )
            break;

        /* add x[min_i][min_j] to the basis, and adjust supplies and cost */
        remember = prev_u_min_i->next;
        icvAddBasicVariable( state, min_i, min_j, prev_u_min_i, prev_v_min_j, &u_head );

        /* update the necessary delta[][] */
        if( remember == prev_u_min_i->next )    /* line min_i was deleted */
        {
            for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
            {
                j = (int)(cur_v - v);
                if( cur_v->val == cost[min_i][j] )      /* column j needs updating */
                {
                    float max_val = -CV_EMD_INF;

                    /* find the new maximum value in the column */
                    for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
                    {
                        float temp = cost[cur_u - u][j];

                        if( max_val < temp )
                            max_val = temp;
                    }

                    /* if needed, adjust the relevant delta[*][j] */
                    diff = max_val - cur_v->val;
                    cur_v->val = max_val;
                    if( fabs( diff ) < eps )
                    {
                        for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
                            delta[cur_u - u][j] += diff;
                    }
                }
            }
        }
        else                    /* column min_j was deleted */
        {
            for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
            {
                i = (int)(cur_u - u);
                if( cur_u->val == cost[i][min_j] )      /* row i needs updating */
                {
                    float max_val = -CV_EMD_INF;

                    /* find the new maximum value in the row */
                    for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
                    {
                        float temp = cost[i][cur_v - v];

                        if( max_val < temp )
                            max_val = temp;
                    }

                    /* if needed, adjust the relevant delta[i][*] */
                    diff = max_val - cur_u->val;
                    cur_u->val = max_val;

                    if( fabs( diff ) < eps )
                    {
                        for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
                            delta[i][cur_v - v] += diff;
                    }
                }
            }
        }
    }
    while( u_head.next != 0 || v_head.next != 0 );
}


/****************************************************************************************\
*                                   icvAddBasicVariable                                *
\****************************************************************************************/
static void
icvAddBasicVariable( CvEMDState * state,
                     int min_i, int min_j,
                     CvNode1D * prev_u_min_i, CvNode1D * prev_v_min_j, CvNode1D * u_head )
{
    float temp;
    CvNode2D *end_x = state->end_x;

    if( state->s[min_i] < state->d[min_j] + state->weight * CV_EMD_EPS )
    {                           /* supply exhausted */
        temp = state->s[min_i];
        state->s[min_i] = 0;
        state->d[min_j] -= temp;
    }
    else                        /* demand exhausted */
    {
        temp = state->d[min_j];
        state->d[min_j] = 0;
        state->s[min_i] -= temp;
    }

    /* x(min_i,min_j) is a basic variable */
    state->is_x[min_i][min_j] = 1;

    end_x->val = temp;
    end_x->i = min_i;
    end_x->j = min_j;
    end_x->next[0] = state->rows_x[min_i];
    end_x->next[1] = state->cols_x[min_j];
    state->rows_x[min_i] = end_x;
    state->cols_x[min_j] = end_x;
    state->end_x = end_x + 1;

    /* delete supply row only if the empty, and if not last row */
    if( state->s[min_i] == 0 && u_head->next->next != 0 )
        prev_u_min_i->next = prev_u_min_i->next->next;  /* remove row from list */
    else
        prev_v_min_j->next = prev_v_min_j->next->next;  /* remove column from list */
}


/****************************************************************************************\
*                                  standard  metrics                                     *
\****************************************************************************************/
static float
icvDistL1( const float *x, const float *y, void *user_param )
{
    int i, dims = (int)(size_t)user_param;
    double s = 0;

    for( i = 0; i < dims; i++ )
    {
        double t = x[i] - y[i];

        s += fabs( t );
    }
    return (float)s;
}

static float
icvDistL2( const float *x, const float *y, void *user_param )
{
    int i, dims = (int)(size_t)user_param;
    double s = 0;

    for( i = 0; i < dims; i++ )
    {
        double t = x[i] - y[i];

        s += t * t;
    }
    return cvSqrt( (float)s );
}

static float
icvDistC( const float *x, const float *y, void *user_param )
{
    int i, dims = (int)(size_t)user_param;
    double s = 0;

    for( i = 0; i < dims; i++ )
    {
        double t = fabs( x[i] - y[i] );

        if( s < t )
            s = t;
    }
    return (float)s;
}

/* End of file. */