#include <v_lib.h>
#include "iplmath.h"
#include "messages.h"
#include "istrings.h"

/*
 * matSubmatrixMinMax()
 * Computes minimal and maximal element in given submatrix.
 */
void
matSubmatrixMinMax(matrix_p data,
    u32_t left,u32_t top,u32_t right,u32_t bottom,
    vec_t *min_ret,
    vec_t *max_ret)
{
    u32_t r, c;
    vec_t min, max, *row;
    V_ASSERT(data && left <= right && top <= bottom &&
    	right < data->cols && bottom < data->rows);
    min = max = data->cell[top][left];
    if(min_ret && max_ret)
    {
        for(r = top;r <= bottom;r++)
        {
            row = data->cell[r];
            for(c = left;c <= right;c++)
            {
                if(min > row[c])
                    min = row[c];
                else if(max < row[c])
                    max = row[c];
            }
        }
        *min_ret = min;
        *max_ret = max;
    } else if(min_ret) {
        for(r = top;r <= bottom;r++)
        {
            row = data->cell[r];
            for(c = left;c <= right;c++)
            {
                if(min > row[c])
                    min = row[c];
            }
        }
        *min_ret = min;
    } else if(max_ret) {
        for(r = top;r <= bottom;r++)
        {
            row = data->cell[r];
            for(c = left;c <= right;c++)
            {
                if(max < row[c])
                    max = row[c];
            }
        }
        *max_ret = max;
    }
}

/*
 * matZeroRows()
 * Clears rows in range [row_start;row_end]
 */
void
matZeroRows(matrix_p m,u32_t row_start,u32_t row_end)
{
    u32_t r, row_len;
    V_ASSERT(m);
    if(row_start > row_end || row_start > m->rows)
        return;
    if(row_end > m->rows)
        row_end = m->rows;
    row_len = m->cols * sizeof(vec_t);
    if(row_len & 3) /* it will _never_ be true */
    {
        for(r = row_start;r < row_end;r++)
            memZero(m->cell[r],row_len);
    }
    else
    {
        row_len = row_len >> 2;
        for(r = row_start;r < row_end;r++)
            memZero32(m->cell[r],row_len);
    }
}

typedef struct zero_struct_s
{
    u32_t   start;
    u32_t   end;
}   zero_struct_t, *zero_struct_p;

#include "argsused.h"
V_LOCAL vboolean V_CALL
zero_col_cb(void_p arg,u32_t row,u32_t col,vboolean is_neg,vec_t reim[])
{
    zero_struct_p zs = (zero_struct_p)arg;
    if(col >= zs->start && col <= zs->end)
    {
        reim[0] = 0;
        reim[1] = 0;
    }
    return true;
}

#include "argsused.h"
V_LOCAL vboolean V_CALL
zero_row_cb(void_p arg,u32_t row,u32_t col,vboolean is_neg,vec_t reim[])
{
    zero_struct_p zs = (zero_struct_p)arg;
    if(row >= zs->start && row <= zs->end)
    {
        reim[0] = 0;
        reim[1] = 0;
    }
    return true;
}

/*
 * matRVMFTZeroCols()
 * Clears columns of 2D Fourier transform in range [row_start;row_end]
 */
void
matRVMFTZeroCols(matrix_p rvmft,u32_t col_start,u32_t col_end)
{
    zero_struct_t zs;
    zs.start = col_start;
    zs.end = col_end;
    V_ASSERT(rvmft);
    matRVMFTWalkCartesian(rvmft,col_start,0,col_end,rvmft->rows >> 1,
        zero_col_cb,&zs);
}

/*
 * matRVMFTZeroRows()
 * Clears rows of 2D Fourier transform in range [row_start;row_end]
 */
void
matRVMFTZeroRows(matrix_p rvmft,u32_t row_start,u32_t row_end)
{
    zero_struct_t zs;
    zs.start = row_start;
    zs.end = row_end;
    V_ASSERT(rvmft);
    matRVMFTWalkCartesian(rvmft,0,row_start,rvmft->cols >> 1,row_end,
        zero_row_cb,&zs);
}

/*
 * matBandReject()
 * Clears both rows and columns in range [lo_bound;hi_bound]
 */
void
matBandReject(matrix_p m,u32_t lo_bound,u32_t hi_bound)
{
    V_ASSERT(m);
    matZeroCols(m,lo_bound,hi_bound);
    matZeroRows(m,lo_bound,hi_bound);
}

/*
 * matBandPass()
 * Clears both rows and columns outside the range [lo_bound;hi_bound]
 */
void
matBandPass(matrix_p m,u32_t lo_bound,u32_t hi_bound)
{
    V_ASSERT(m);
    if(lo_bound)
        matBandReject(m,0,lo_bound-1);
    if(hi_bound < m->rows-1)
        matZeroRows(m,hi_bound+1,m->rows-1);
    if(hi_bound < m->cols-1)
        matZeroCols(m,hi_bound+1,m->cols-1);
}


/*
 * matOffset()
 * Adds given 'offset' to all elements
 */
void
matOffset(matrix_p m,vec_t offset)
{
    u32_t r, c;
    V_ASSERT(m);
    for(r = 0;r < m->rows;r++)
        for(c = 0;c < m->cols;c++)
            m->cell[r][c] += offset;
}

/*
 * vecRunningSum()
 * Computes discrete integral
 */
void
vecRunningSum(vector_p dest,vector_p src)
{
    u32_t i;
    vec_p d, s;
    V_ASSERT(dest && src && src->size == dest->size);
    d = dest->elements;
    s = src->elements;
    d[0] = s[0];
    for(i = 1;i < dest->size;i++)
        d[i] = s[i] + d[i-1];
}

/*
 * vecLinSpaced()
 * Stores result of linear interpolation from 'min' to 'max' to 'dest'
 */
void
vecLinSpaced(vector_p dest,vec_t min,vec_t max)
{
    u32_t i, n_divs;
    vec_t delta;
    n_divs = dest->size;
    V_ASSERT(dest);
    if(n_divs > 1)
    {
        dest->elements[0] = min;
        delta = (max - min) / (n_divs - 1);
        for(i = 1;i < n_divs;i++)
            dest->elements[i] = dest->elements[i - 1] + delta;
    }
    else
        dest->elements[0] = (min + max) / 2;
}

/*
 * matLinSpacedRows()
 * Stores result of linear interpolation from 'min' to 'max' to every row
 */
void
matLinSpacedRows(matrix_p dest,vec_t min,vec_t max)
{
    u32_t i, n_divs, row_bytes;
    vec_t delta;
    V_ASSERT(dest);
    n_divs = dest->cols;
    if(n_divs > 1)
    {
        dest->cell[0][0] = min;
        delta = (max - min) / (n_divs - 1);
        for(i = 1;i < dest->cols;i++)
            dest->cell[0][i] = dest->cell[0][i-1] + delta;
    }
    else
        dest->cell[0][0] = (min + max) / 2;
    row_bytes = n_divs * sizeof(vec_t);
    for(i = 1;i < dest->rows;i++)
        memCopy(&(dest->cell[i][0]),&(dest->cell[0][0]),row_bytes);
}

/*
 * matLinSpacedCols()
 * Stores result of linear interpolation from 'min' to 'max' to every col
 */
void
matLinSpacedCols(matrix_p dest,vec_t min,vec_t max)
{
    u32_t i, j, n_divs;
    vec_t delta;
    V_ASSERT(dest);
    n_divs = dest->rows;
    if(n_divs > 1)
    {
        dest->cell[0][0] = min;
        delta = (max - min) / (n_divs - 1);
        for(i = 1;i < dest->rows;i++)
            dest->cell[i][0] = dest->cell[i-1][0] + delta;
    }
    else
        dest->cell[0][0] = (min + max) / 2;
    for(i = 1;i < dest->cols;i++)
    {
        for(j = 0;j < dest->rows;j++)
            dest->cell[j][i] = dest->cell[j][0];
    }
}

/*
 * matScaleM()
 * Per - element multiplication
 */
void
matScaleM(matrix_p dest,matrix_p src,matrix_p scale)
{
    u32_t r, c;
    vec_p dp, sp, scp;
    V_ASSERT(dest && src && scale);
    V_ASSERT(dest->rows == src->rows);
    V_ASSERT(dest->cols == src->cols);
    V_ASSERT(dest->rows == scale->rows);
    V_ASSERT(dest->cols == scale->cols);
    for(r = 0;r < dest->rows;r++)
    {
        dp = dest->cell[r];
        sp = src->cell[r];
        scp = scale->cell[r];
        for(c = 0;c < dest->cols;c++)
            dp[c] = sp[c] * scp[c];
    }
}

/*
 * matUnscaleM()
 * Per - element division
 */
void
matUnscaleM(matrix_p dest,matrix_p src,matrix_p divisor)
{
    u32_t r, c;
    vec_p dp, sp, divp;
    V_ASSERT(dest && src && divisor);
    V_ASSERT(dest->rows == src->rows);
    V_ASSERT(dest->cols == src->cols);
    V_ASSERT(dest->rows == divisor->rows);
    V_ASSERT(dest->cols == divisor->cols);
    for(r = 0;r < dest->rows;r++)
    {
        dp = dest->cell[r];
        sp = src->cell[r];
        divp = divisor->cell[r];
        for(c = 0;c < dest->cols;c++)
        {
            if(IsZero(divp[c]))
                continue;
            dp[c] = sp[c] / divp[c];
        }
    }
}

/*
 * matHistogramRange()
 * Computes histogram of given 'src'. Number of bins equal to size
 * of vector 'dest'. Lower bound of 1st bin equal to 'min', upper
 * bound of last bin equal to 'max'. If 'histogram_space' is not NULL,
 * stores there centers of each bin. If 'normalize', than computes
 * relative frequencies.
 */
vboolean
matHistogramRange(vector_p dest,matrix_p src,vboolean normalize,
    vector_p histogram_space,vec_t min,vec_t max)
{
    u32_t sum, i, r, c, n_divs = dest->size;
    vec_t lower, upper, delta, *row;
    delta = (max - min) / n_divs;
    V_ASSERT(dest && src);
    V_ASSERT(!histogram_space || (histogram_space->size == dest->size));
    if(IsZero(delta))
    {
        if(histogram_space)
            for(i = 0;i < histogram_space->size;i++)
                histogram_space->elements[i] = min;
        vecZero(dest);
        if(normalize)
            dest->elements[0] = 1;
        else
            dest->elements[0] = (vec_t)(src->rows * src->cols);
        return true;
    }
    n_divs--;
    for(i = 0;i < n_divs;i++)
    {
        sum = 0;
        lower = min + i * delta;
        upper = lower + delta;
        if(histogram_space)
            histogram_space->elements[i] = (lower + upper) / 2;
        for(r = 0;r < src->rows;r++)
        {
            row = src->cell[r];
            for(c = 0;c < src->cols;c++)
            {
                if(row[c] >= lower && row[c] < upper)
                    sum++;
            }
        }
        dest->elements[i] = (vec_t)sum;
    }
    /* last bin */
    sum = 0;
    lower = min + i * delta;
    upper = lower + delta;
    if(histogram_space)
        histogram_space->elements[i] = (2 * lower + delta) / 2;
    for(r = 0;r < src->rows;r++)
    {
        row = src->cell[r];
        for(c = 0;c < src->cols;c++)
        {
            if(row[c] >= lower && row[c] <= upper)
                sum++;
        }
    }
    dest->elements[i] = (vec_t)sum;
    n_divs++;
    if(normalize)
    {
        vec_t vsum = dest->elements[0];
        for(i = 1;i < n_divs;i++)
            vsum += dest->elements[i];
        if(vsum > 0)
        {
            vsum = 1 / vsum;
            vecScale(dest,dest,vsum);
        }
        else
            return false;
    }
    return true;
}

/*
 * matHistogram()
 * Same as above, but 'min' and 'max' deterimed to be minimal and
 * maximal elements of 'src'.
 */
vboolean
matHistogram(vector_p dest,matrix_p src,vboolean normalize,
    vector_p histogram_space)
{
    vec_t min, max;
    V_ASSERT(dest && src);
    matSubmatrixMinMax(src,0,0,src->cols - 1,src->rows - 1,&min,&max);
    return matHistogramRange(dest,src,normalize,histogram_space,
        min,max);
}

/*
 * vecHistogramRange()
 * Same as matHistogramRange(), but uses vector as source.
 */
vboolean
vecHistogramRange(vector_p dest,vector_p src,vboolean normalize,
    vector_p histogram_space,vec_t min,vec_t max)
{
    u32_t sum, i, j, n_divs = dest->size;
    vec_t lower, upper, delta, *ptr;
    ptr = src->elements;
    delta = (max - min) / n_divs;
    V_ASSERT(dest && src);
    V_ASSERT(!histogram_space || (histogram_space->size == n_divs));
    if(IsZero(delta))
    {
        if(histogram_space)
            for(i = 0;i < histogram_space->size;i++)
                histogram_space->elements[i] = min;
        vecZero(dest);
        if(normalize)
            dest->elements[0] = 1;
        else
            dest->elements[0] = (vec_t)src->size;
        return true;
    }
    n_divs--;
    for(i = 0;i < n_divs;i++)
    {
        sum = 0;
        lower = min + i * delta;
        upper = lower + delta;
        if(histogram_space)
            histogram_space->elements[i] = (lower + upper) / 2;
        for(j = 0;j < src->size;j++)
        {
            if(ptr[j] >= lower && ptr[j] < upper)
                sum++;
        }
        dest->elements[i] = (vec_t)sum;
    }
    /* last bin */
    sum = 0;
    lower = min + i * delta;
    upper = lower + delta;
    if(histogram_space)
        histogram_space->elements[i] = (2 * lower + delta) / 2;
    for(j = 0;j < src->size;j++)
    {
        if(ptr[j] >= lower)
            sum++;
    }
    dest->elements[i] = (vec_t)sum;
    n_divs++;
    if(normalize)
    {
        vec_t vsum = dest->elements[0];
        for(i = 1;i < n_divs;i++)
            vsum += dest->elements[i];
        if(vsum > 0)
        {
            vsum = 1 / vsum;
            vecScale(dest,dest,vsum);
        }
        else
            return false;
    }
    return true;
}

/*
 * vecHistogram()
 */
vboolean
vecHistogram(vector_p dest,vector_p src,vboolean normalize,
    vector_p space)
{
    vec_t min, max;
    V_ASSERT(dest && src);
    min = src->elements[vecMin(src)];
    max = src->elements[vecMax(src)];
    return vecHistogramRange(dest,src,normalize,space,min,max);
}

/*
 * matScaleAdd()
 * Multiplies every element by 'scale' and than adds 'offset'.
 */
V_LOCAL void
matScaleAdd(matrix_p dest,matrix_p src,vec_t scale,vec_t offset)
{
    u32_t r, c;
    vec_p dp, sp;
    V_ASSERT(dest && src);
    V_ASSERT(dest->rows == src->rows);
    V_ASSERT(dest->cols == src->cols);
    for(r = 0;r < dest->rows;r++)
    {
        dp = dest->cell[r];
        sp = src->cell[r];
        for(c = 0;c < dest->cols;c++)
            dp[c] = offset + sp[c] * scale;
    }
}

/*
 * matAdjustContrast()
 * Transforms range [min;max] to [0;1]
 */
vboolean
matAdjustContrast(matrix_p dest,matrix_p src,vec_t min,vec_t max)
{
    CALLER_NAME(matAdjustContrast)
    vec_t rscale;
    V_ASSERT(dest && src);
    rscale = max - min;
    if(!IsZero(rscale))
    {
        rscale = 1 / rscale;
        matScaleAdd(dest,src,rscale,- min * rscale);
        return true;
    }
    WARNING_GENERIC(ipl_constant_matrix);
    return false;
}

/*
 * matNormalize()
 * Transforms range [min of 'src';max of 'src'] to [new_min;mew_max]
 */
vboolean
matNormalize(matrix_p dest,matrix_p src,vec_t new_min,vec_t new_max)
{
    CALLER_NAME(matNormalize)
    vec_t min, max, old_range, new_range, scale, offset;
    V_ASSERT(dest && src);
    matSubmatrixMinMax(src,0,0,src->cols - 1,src->rows - 1,&min,&max);
    old_range = max - min;
    if(IsZero(old_range))
    {
        WARNING_GENERIC(ipl_constant_matrix);
        matSetRect(dest,0,0,dest->cols - 1,dest->rows - 1,
            (new_max + new_min) / 2);
        return true;
    }
    new_range = new_max - new_min;
    scale = new_range / old_range;
    offset = new_min - min * scale;
    matScaleAdd(dest,src,scale,offset);
    return true;
}

/*
 * vecScaleAdd()
 * Same as matScaleAdd(), but operates on vector
 */
V_LOCAL void
vecScaleAdd(vector_p dest,vector_p src,vec_t scale,vec_t offset)
{
    u32_t c;
    V_ASSERT(dest && src);
    V_ASSERT(dest->size == src->size);
    for(c = 0;c < dest->size;c++)
        dest->elements[c] = src->elements[c] * scale + offset;
}

/*
 * vecAdjustContrast()
 * Same as matAdjustContrast()
 */
vboolean
vecAdjustContrast(vector_p dest,vector_p src,vec_t min,vec_t max)
{
    CALLER_NAME(vecAdjustContrast)
    vec_t rscale;
    V_ASSERT(dest && src);
    rscale = max - min;
    if(!IsZero(rscale))
    {
        rscale = 1 / rscale;
        vecScaleAdd(dest,src,rscale,- min * rscale);
        return true;
    }
    WARNING_GENERIC(ipl_constant_matrix);
    return false;
}

/*
 * vecNormalize()
 * Same as matNormalize()
 */
vboolean
vecNormalize(vector_p dest,vector_p src,vec_t new_min,vec_t new_max)
{
    CALLER_NAME(vecNormalize)
    u32_t i;
    vec_t min, max, old_range, new_range, scale, offset;
    V_ASSERT(dest && src);
    min = src->elements[vecMin(src)];
    max = src->elements[vecMax(src)];
    old_range = max - min;
    if(IsZero(old_range))
    {
        WARNING_GENERIC(ipl_constant_vector);
        offset = (new_max + new_min) / 2;
        for(i = 0;i < dest->size;i++)
            dest->elements[i] = offset;
        return true;
    }
    new_range = new_max - new_min;
    scale = new_range / old_range;
    offset = new_min - min * scale;
    vecScaleAdd(dest,src,scale,offset);
    return true;
}

/*
 * matGammaCorrection()
 * Arbitrary transform of elements using 'gamma' as transform curve.
 */
vboolean
matGammaCorrection(matrix_p dest,matrix_p src,vector_p gamma,
    vboolean unit_input)
{
    u32_t i, r, c, n_divs = gamma->size;
    vec_t min, max, t, delta, *rs, *rd;
    V_ASSERT(dest && src);
    if(unit_input)
    {
        min = 0;
        max = 1;
    }
    else
        matSubmatrixMinMax(src,0,0,src->cols - 1,src->rows - 1,
            &min, &max);
    delta = (max - min) / n_divs;
    if(IsZero(delta))
        return false;
    for(r = 0;r < dest->rows;r++)
    {
        rd = dest->cell[r];
        rs = src->cell[r];
        for(c = 0;c < dest->cols;c++)
        {
            t = (rs[c] - min) / delta; /* 0 <= t < n_divs */
            if(t < 0)
                t = 0;
            i = (u32_t)t;
            if(i >= n_divs)
                i = n_divs - 1;
            rd[c] = gamma->elements[i];
        }
    }
    return true;
}

#include "argsused.h"
V_LOCAL vboolean V_CALL
rvmft_pse_cb(void_p arg,u32_t col,u32_t row,vboolean negf,vec_t reim[])
{
    matrix_p pse = (matrix_p)arg;
    /* flip highest frequencies back to zero */
    if(row >= pse->rows)
        row = 0;
    if(col >= pse->cols)
        col = 0;
    pse->cell[row][col] += (vec_t)hypot(reim[0],reim[1]);
    return true;
}

/*
 * matRVMFTSpectrum()
 * Stores power spectrum estimation of the 'rvmft' in 'dest'.
 */
vboolean
matRVMFTSpectrum(matrix_p dest,matrix_p rvmft)
{
    V_ASSERT(dest && rvmft);
    V_ASSERT(dest->rows == (rvmft->rows >> 1) &&
        dest->cols == (rvmft->cols >> 1));
    matZero(dest);
    matRVMFTWalkCartesian(rvmft,0,0,rvmft->cols >> 1,rvmft->rows >> 1,
        rvmft_pse_cb,dest);
    return true;
}

/*
 * matExpand()
 * Expands or crops the matrix. While expanding, pads the new areas
 * with 'pad_value'
 */
matrix_p
matExpand(matrix_p src,u32_t new_rows,u32_t new_cols,
    vec_t pad_value)
{
    u32_t d_row; /* row in dst to place 1st row of src */
    u32_t d_col; /* col in dst to place 1st col of src */
    u32_t s_row; /* row in src from which to extract to 1st row of dst */
    u32_t s_col; /* col in src from which to extract to 1st col of dst */
    u32_t col_height; /* number of rows to copy */
    u32_t row_len; /* number of columns to copy */
    u32_t row_limit, lower_row, right_col, r;
    vec_p sptr, dptr, dptr_end;
    matrix_p dst;
    V_ASSERT(new_rows && new_cols);
    dst = matAlloc(new_rows,new_cols);
    V_ASSERT(dst);
    if(!src)
    {
        matSetRect(dst,0,0,dst->cols - 1,dst->rows - 1,pad_value);
        return dst;
    }
    if(new_rows >= src->rows)
    {
        d_row = (new_rows - src->rows) >> 1;
        s_row = 0;
        col_height = src->rows;
    }
    else
    {
        d_row = 0;
        s_row = (src->rows - new_rows) >> 1;
        col_height = new_rows;
    }
    if(new_cols >= src->cols)
    {
        d_col = (new_cols - src->cols) >> 1;
        s_col = 0;
        row_len = src->cols;
    }
    else
    {
        d_col  = 0;
        s_col = (src->cols - new_cols) >> 1;
        row_len = new_cols;
    }
    if(d_row)
        matSetRect(dst,0,0,dst->cols - 1,d_row - 1,pad_value);
    lower_row = d_row + col_height; /* 1st row that remains intact */
    if(lower_row < dst->rows)
        matSetRect(dst,0,lower_row,dst->cols - 1,dst->rows - 1,pad_value);
    if(d_col)
        matSetRect(dst,0,d_row,d_col - 1,lower_row - 1,pad_value);
    right_col = d_col + row_len;
    if(right_col < dst->cols)
        matSetRect(dst,right_col,d_row,dst->cols-1,lower_row-1,pad_value);
    row_limit = s_row + col_height;
    for(r = s_row;r < row_limit;r++)
    {
        sptr = src->cell[r] + s_col;
        dptr = dst->cell[d_row + r - s_row] + d_col;
        dptr_end = dptr + row_len;
        while(dptr < dptr_end)
            *dptr++ = *sptr++;
    }
    return dst;
}

/*
 * matExpandPowers2()
 * Same as matExpand(), expands to nearest non-less powers of 2
 */
matrix_p
matExpandPowers2(matrix_p src,vec_t pad_value)
{
    u32_t new_rows, new_cols;
    if(!src)
        return matExpand(NULL,1,1,pad_value);
    NOT_LESS_POWER_TWO(new_rows,src->rows,u32_t);
    NOT_LESS_POWER_TWO(new_cols,src->cols,u32_t);
    return matExpand(src,new_rows,new_cols,pad_value);
}

/*
 * matConvolve3x3()
 * Convolves 'src' with 3x3 kernel stored in 3 given 3-element vectors
 */
matrix_p
matConvolve3x3(matrix_p src,
    vec3_t filt_row0,vec3_t filt_row1,vec3_t filt_row2)
{
    vec_t r0c0,r0c1,r0c2;
    vec_t r1c0,r1c2;
    vec_t r2c0,r2c1,r2c2;
    matrix_p dst;
    vec_p *dcell, *scell;
    u32_t r, c, rmax, cmax;
    V_ASSERT(src);
    rmax = src->rows - 1;
    cmax = src->cols - 1;
    V_ASSERT(rmax >= 1 && cmax >= 1);
    dst = matAlloc(src->rows,src->cols);
    matScale(dst,src,filt_row1[1]);
    r0c0 = filt_row2[2]; r0c1 = filt_row2[1]; r0c2 = filt_row2[0];
    r1c0 = filt_row1[2]; r1c2 = filt_row1[0];
    r2c0 = filt_row0[2]; r2c1 = filt_row0[1]; r2c2 = filt_row0[0];
    dcell = dst->cell; scell = src->cell;
    /* corners */
    dcell[0][0] += /* left, top */
        scell[0][1] * r1c2 +
        scell[1][0] * r2c1 +
        scell[1][1] * r2c2;
    dcell[0][cmax] += /* right, top */
        scell[0][cmax - 1] * r1c0 +
        scell[1][cmax] * r2c1 +
        scell[1][cmax - 1] * r2c0;
    dcell[rmax][0] += /* left, bottom */
        scell[rmax][1] * r1c2 +
        scell[rmax - 1][0] * r0c1 +
        scell[rmax - 1][1] * r0c2;
    dcell[rmax][cmax] += /* right, bottom */
        scell[rmax][cmax - 1] * r1c0 +
        scell[rmax - 1][cmax] * r0c1 +
        scell[rmax - 1][cmax - 1] * r0c0;
    /* boundaries */
    for(r = 1;r < rmax;r++) /* left */
    {
        dcell[r][0] +=
            scell[r + 1][0] * r2c1 +
            scell[r + 1][1] * r2c2 +
            scell[r][1] * r1c2 +
            scell[r - 1][1] * r0c2 +
            scell[r - 1][0] * r0c1;
    }
    for(c = 1;c < cmax;c++) /* top */
    {
        dcell[0][c] +=
            scell[0][c - 1] * r1c0 +
            scell[1][c - 1] * r2c0 +
            scell[1][c] * r2c1 +
            scell[1][c + 1] * r2c2 +
            scell[0][c + 1] * r1c2;
    }
    for(r = 1;r < rmax;r++) /* right */
    {
        dcell[r][cmax] +=
            scell[r - 1][cmax] * r0c1 +
            scell[r - 1][cmax - 1] * r0c0 +
            scell[r][cmax - 1] * r1c0 +
            scell[r + 1][cmax - 1] * r2c0 +
            scell[r + 1][cmax] * r2c1;
    }
    for(c = 1;c < cmax;c++) /* bottom */
    {
        dcell[rmax][c] +=
            scell[rmax][c + 1] * r1c2 +
            scell[rmax - 1][c + 1] * r0c2 +
            scell[rmax - 1][c] * r0c1 +
            scell[rmax - 1][c - 1] * r0c0 +
            scell[rmax][c - 1] * r1c0;
    }
    /* mid-range */
    for(r = 1;r < rmax;r++)
    {
        for(c = 1;c < cmax;c++)
        {
            dcell[r][c] +=
                scell[r][c + 1] * r1c2 +
                scell[r - 1][c + 1] * r0c2 +
                scell[r - 1][c] * r0c1 +
                scell[r - 1][c - 1] * r0c0 +
                scell[r][c - 1] * r1c0 +
                scell[r + 1][c - 1] * r2c0 +
                scell[r + 1][c] * r2c1 +
                scell[r + 1][c + 1] * r2c2;
        }
    }
    return dst;
}

/*
 * vec_mirror()
 * Reverses the order of elements in array
 */
V_LOCAL void
vec_mirror(vec_p fptr,u32_t len)
{
    vec_t tmp, *bptr;
    bptr = fptr + len - 1;
    for(;fptr < bptr;fptr++,bptr--)
    {
        tmp = *fptr;
        *fptr = *bptr;
        *bptr = tmp;
    }
}

/*
 * vecMirror()
 * Reverses the order of elements in vector
 */
void
vecMirror(vector_p v)
{
    V_ASSERT(v);
    vec_mirror(v->elements,v->size);
}

/*
 * matMirror()
 * If 'mirror_x', reverses the order of elements in every row;
 * if 'mirror_y', reverses the order of elements in every column
 */
void
matMirror(matrix_p m,vboolean mirror_x,vboolean mirror_y)
{
    vec_p tmp;
    u32_t r, br, row_bytes;
    V_ASSERT(m);
    if(mirror_x)
        for(r = 0;r < m->rows;r++)
            vec_mirror(m->cell[r],m->cols);
    if(mirror_y)
    {
        row_bytes = m->cols * sizeof(vec_t);
        tmp = (vec_p)memAlloc(row_bytes);
        for(r = 0,br = m->rows - 1;r < br;r++,br--)
        {
            memCopy(tmp,m->cell[r],row_bytes);
            memCopy(m->cell[r],m->cell[br],row_bytes);
            memCopy(m->cell[br],tmp,row_bytes);
        }
        memFree(tmp);
    }
        
}

/*
 * vecShift()
 * Circular shift of vector's elements
 */
vboolean
vecShift(vector_p v,s32_t amount)
{
    vector_p v2 = vecDuplicate(v);
    V_ASSERT(v2);
    vecCopyElements(v,v2,amount,0,v->size - 1);
    memFree(v2);
    return true;
}
