/*======================================================*/
/*                                                      */
/*    matching.cpp                                      */
/*                              (c) HAL 2010-           */
/*                                                      */
/*   void calculateMatcing( double* T, double* H,       */
/*               double* src, double* dst, int length );*/
/*                                                      */
/*    Calculate matching function between src & dst;    */
/*  where T is the destination for writing result,      */
/*  H is difference between src & 'stretched' dst.      */
/*                                                      */
/*======================================================*/

#include "matching.h"
#include <QDebug>

double interpolateArray( double x, const double *p )
{
    int t = (int)x;
    double r = x - (double)t;
    return ( p[t] * ( 1.0 - r ) + p[t+1] * r );
}

void applyStretching( double *T, double *target, int length, double *temp_spectrum )
{
    int i;
    for( i = 0; i < length; i++ )
        temp_spectrum[i] = interpolateArray( T[i], target );
    memcpy( target, temp_spectrum, sizeof(double) * length );
}

void getGraduation( double* src, double* dst,
                    double* d1s, double *d2s,
                    double* d1d, double *d2d, int length)
{
    /* 導関数の数値を計算 */
    double *temp_spectrum = (double*)malloc(sizeof(double)*length);
    memset( temp_spectrum,0,sizeof(double)*length );
    memset( d1s,0,sizeof(double)*length );
    memset( d2s,0,sizeof(double)*length );
    memset( d1d,0,sizeof(double)*length );
    memset( d2d,0,sizeof(double)*length );
    for( int i = 1; i < length; i++ )
        temp_spectrum[i] = fabs( dst[i] - dst[i-1] );
    for( int i = 0; i < length - 1; i++ )
        d1d[i] = 0.5 * ( temp_spectrum[i] + temp_spectrum[i+1] );

    for( int i = 1; i < length; i++ )
        temp_spectrum[i] = fabs( d1d[i] - d1d[i-1] );
    for( int i = 0; i < length - 1; i++ )
        d2d[i] = 0.5 * ( temp_spectrum[i] + temp_spectrum[i+1] );


    for( int i = 1; i < length; i++ )
        temp_spectrum[i] = fabs( src[i] - src[i-1] );
    for( int i = 0; i < length - 1; i++ )
        d1s[i] = 0.5 * ( temp_spectrum[i] + temp_spectrum[i+1] );

    for( int i = 1; i < length; i++ )
        temp_spectrum[i] = fabs( d1s[i] - d1s[i-1] );
    for( int i = 0; i < length - 1; i++ )
        d2s[i] = 0.5 * ( temp_spectrum[i] + temp_spectrum[i+1] );
}

void logNormalizeInfinite( double* target, int length )
{
    /* 正規化の結果は０～１の範囲になる． */
    int i;
    double min_v = DBL_MAX, max_v = DBL_MIN;
    for( i = 0; i < length; i++ ){
        if( target[i] < min_v )
            min_v = target[i];
        if( target[i] > max_v )
            max_v = target[i];
    }
    min_v += 1.0e-17;
    for( i = 0; i < length; i++ )
        target[i] = log( (target[i] + 1.0e-17) / min_v ) / log( max_v + 1.0e-17 );
}

double getCost( double* src, double* dst, int i, int j, int n, int m,
                double* d1s, double *d2s, double *d1d, double *d2d)
{
    int k;
    double temp, rate, sum;

    if( n > m * GRAD || m > n * GRAD )
        return DBL_MAX;

    rate = temp = (double)n / (double)m;
    sum = 0.0;

    if( rate > 1.0 )
        rate = 1.0 / rate;
    if( m >= n ){
        for( k = 1; k <= m; k++ ){
            sum += fabs( interpolateArray( (double)i + (double)k * temp, src ) - dst[j+k] );
            sum += fabs( interpolateArray( (double)i + (double)k * temp, d1s ) / temp - d1d[j+k] );
            sum += 0.5 * fabs( interpolateArray( (double)i + (double)k * temp, d2s ) / pow( temp, 2.0 ) - d2d[j+k] );
        }
        temp = m;
        temp = sqrt( (double)( m*m + n*n ) ) / temp;
        sum *= temp;
    }else{
        for( k = 1; k <= n; k++ ){
            sum += fabs( src[i+k] - interpolateArray( (double)j + (double)k / temp, dst ) );
            sum += fabs( d1s[i+k] - interpolateArray( (double)j + (double)k / temp, d1d ) * temp );
            sum += 0.5 * fabs( d2s[i+k] - interpolateArray( (double)j + (double)k / temp, d2d ) * pow( temp, 2.0 ) );
        }
        temp = n;
        temp = sqrt( (double)( m*m + n*n ) ) / temp;
        sum *= temp;
    }
    rate = 1.0 + MAGIC_K * pow( 1.0 - rate, 2.0 );

    return sum * rate;
}

void calculateMatching( double* T, double* H, double* src_s, double* dst_s, int length, double *inverse )
{
    int i, j, k, l, n, m, tx, ty;
    double **dpMap;
    double tempd, g1, g2;
    int    **pathX, **pathY;

    double* src = (double*)malloc( sizeof(double)*length );
    double* dst = (double*)malloc( sizeof(double)*length );
    double* d1d = (double*)malloc( sizeof(double)*length );
    double* d2d = (double*)malloc( sizeof(double)*length );
    double* d1s = (double*)malloc( sizeof(double)*length );
    double* d2s = (double*)malloc( sizeof(double)*length );
    double* temporaryBuffer = (double*)malloc(sizeof(double)*length);

    memcpy( src, src_s, sizeof(double)*length );
    memcpy( dst, dst_s, sizeof(double)*length );

    /* 無限大ノルムで正規化して対数をとっておこう */
    logNormalizeInfinite( src, length );
    logNormalizeInfinite( dst, length );

    /* メモリの確保と値の初期化 */
    dpMap = (double **)malloc( sizeof(double *) * length );
    pathX = (int **)malloc( sizeof(int *) * length );
    pathY = (int **)malloc( sizeof(int *) * length );
    for( i = 0; i < length; i++ ){
        dpMap[i] = (double *)malloc( sizeof(double) * length );
        for( j = 0; j < length; j++ )
            dpMap[i][j] = 10000000.0;
        pathX[i] = (int *)malloc( sizeof(int) * length );
        pathY[i] = (int *)malloc( sizeof(int) * length );
        memset( pathX[i], -1, sizeof(int) * length );
        memset( pathY[i], -1, sizeof(int) * length );
    }
    pathX[0][0] = pathY[0][0] = 0;
    dpMap[0][0] = fabs( dst[0] - src[0] );


    /* 導関数を算出 */
    getGraduation( src, dst, d1s, d2s, d1d, d2d, length );

    /* 最短経路を求めてマッチング */
    for( i = 0; i < length; i++ ){
        for( j = 0; j < length; j++ ){
            if( abs(i - j) > 256 )  /* 領域を直指定するなんて... */
                continue;
            /* 現在点における傾き */
            tx = pathX[i][j]; ty = pathY[i][j];
            if( i == tx || j == ty )
                g1 = 1.0;
            else
                g1 = (double)( j - ty ) / (double)( i - tx );
            /* 分割はあまり増やしても意味が無いみたい */
            for( n = 1; n < 8; n++ ){
                if( i + n >= length )
                    break;
                for( m = 1; m < 8; m++ ){
                    if( j + m >= length )
                        break;
                    /* 目標点における傾き */
                    g2 = (double)m / (double)n;
                    if( g2 > g1 )
                        tempd = dpMap[i][j] + getCost( src, dst, i, j, n, m, d1s, d2s, d1d, d2d ) * g2 / g1;
                    else
                        tempd = dpMap[i][j] + getCost( src, dst, i, j, n, m, d1s, d2s, d1d, d2d ) * g1 / g2;
                    /* 最大値の更新 */
                    if( i + n < length && j + m < length && dpMap[i+n][j+m] > tempd ){
                        dpMap[i+n][j+m] = tempd;
                        pathX[i+n][j+m] = i;
                        pathY[i+n][j+m] = j;
                    }
                }
            }
        }
    }

    T[0] = 0.0;
    l = j = length - 1;

    while( l > 0 && j > 0 ){
        tx = pathX[l][j]; ty = pathY[l][j];
        for( k = l; k > tx; k-- )
            T[k] = (double)j - (double)( l - k ) * (double)( j - ty ) / (double)( l - tx );
        l = tx; j = ty;
    }

    l = j = length - 1;
    while( l > 0 && j > 0 ){
        tx = pathX[l][j]; ty = pathY[l][j];
        for( k = j; k > ty; k-- ){
            inverse[k] = (double)l - (double)( j - k ) * (double)( l - tx ) / (double)( j - ty );
        }
        l = tx; j = ty;
    }

    //
    if(H){
        applyStretching( T, dst, length, temporaryBuffer );
        for( i = 0; i < length; i++ )
            H[i] = src[i] - dst[i];
    }

    /* メモリのお掃除 */
    for( i = 0; i < length; i++ ){
        free( dpMap[i] );
        free( pathX[i] );
        free( pathY[i] );
    }
    free( dpMap );
    free( pathX );
    free( pathY );
    free( temporaryBuffer );
    free( d1d );
    free( d2d );
    free( d1s );
    free( d2s );
    free( src );
    free( dst );

}

