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

void
matSetRect(matrix_p data,
	u32_t left,u32_t top,u32_t right,u32_t bottom,
	vec_t value)
{
	u32_t r, c;
	vec_t *row;
	for(r = top;r <= bottom;r++)
	{
		row = data->cell[r];
		for(c = left;c <= right;c++)
			row[c] = value;
	}
}

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;
	min = max = data->cell[top][left];
	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];
		}
	}
	if(min_ret)
		*min_ret = min;
	if(max_ret)
		*max_ret = max;
}

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);
	for(r = row_start;r < row_end;r++)
		memZero(m->cell[r],row_len);
}

void
matZeroCols(matrix_p m,u32_t col_start,u32_t col_end)
{
	u32_t c, r;
	V_ASSERT(m);
	if(col_start > col_end || col_start >= m->cols)
		return;
	if(col_end >= m->cols)
		col_end = m->cols - 1;
	for(c = col_start;c <= col_end;c++)
		for(r = 0;r < m->rows;r++)
			m->cell[r][c] = 0;
}

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;
}

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;
	matRVMFTWalkCartesian(rvmft,col_start,0,col_end,rvmft->rows >> 1,
		zero_col_cb,&zs);
}

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;
	matRVMFTWalkCartesian(rvmft,0,row_start,rvmft->cols >> 1,row_end,
		zero_row_cb,&zs);
}

void
matBandReject(matrix_p m,u32_t lo_bound,u32_t hi_bound)
{
	matZeroCols(m,lo_bound,hi_bound);
	matZeroRows(m,lo_bound,hi_bound);
}

void
matBandPass(matrix_p m,u32_t lo_bound,u32_t hi_bound)
{
	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);
}

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;
}

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

void
vecLinSpaced(vector_p dest,vec_t min,vec_t max)
{
	u32_t i, n_divs;
    vec_t delta;
    n_divs = dest->size;
    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;
}

void
matLinSpacedRows(matrix_p dest,vec_t min,vec_t max)
{
	u32_t i, n_divs, row_bytes;
    vec_t delta;
    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);
}

void
matLinSpacedCols(matrix_p dest,vec_t min,vec_t max)
{
	u32_t i, j, n_divs;
    vec_t delta;
    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->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->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()
 */
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(!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;
	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)
            	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;
}

vboolean
matHistogram(vector_p dest,matrix_p src,vboolean normalize,
	vector_p histogram_space)
{
    vec_t min, max;
    matSubmatrixMinMax(src,0,0,src->cols - 1,src->rows - 1,&min,&max);
	return matHistogramRange(dest,src,normalize,histogram_space,
		min,max);
}

/*
 * vecHistogram()
 */
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(!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;
	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;
}

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

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->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()
 */
vboolean
matAdjustContrast(matrix_p dest,matrix_p src,vec_t min,vec_t max)
{
	CALLER_NAME(matAdjustContrast)
	vec_t rscale;
    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()
 */
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;
	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;
}

V_LOCAL void
vecScaleAdd(vector_p dest,vector_p src,vec_t scale,vec_t offset)
{
	u32_t c;
	V_ASSERT(dest->size == src->size);
	for(c = 0;c < dest->size;c++)
		dest->elements[c] = src->elements[c] * scale + offset;
}

/*
 * vecAdjustContrast()
 */
vboolean
vecAdjustContrast(vector_p dest,vector_p src,vec_t min,vec_t max)
{
	CALLER_NAME(vecAdjustContrast)
	vec_t rscale;
    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()
 */
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;
    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()
 */
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;
    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->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;
}

matrix_p
matExpand(matrix_p src,u32_t new_rows,u32_t new_cols)
{
	u32_t d_row, d_col, s_row, s_col, col_height, row_len, row_limit, 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(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(new_rows > src->rows || new_cols > src->cols)
		matZero(dst);
	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] + d_col;
		dptr_end = dptr + row_len;
		while(dptr < dptr_end)
			*dptr++ = *sptr++;
	}
	return dst;
}

matrix_p
matExpandPowers2(matrix_p src)
{
	u32_t new_rows, new_cols;
	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);
}

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 = 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;
}

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;
	}
}

void
vecMirror(vector_p v)
{
	if(!v || !v->size)
		return;
	vec_mirror(v->elements,v->size);
}

void
matMirror(matrix_p m,vboolean mirror_x,vboolean mirror_y)
{
	vec_p tmp;
	u32_t r, br, row_bytes;
	if(!m || !m->rows || !m->cols)
		return;
	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);
	}
		
}

/* fixme: move to vector.c */

V_LOCAL void
vec_copy_elements_pwr2(vec_p dst,vec_p src,u32_t dest_size,
	s32_t dest_idx,u32_t src_left,u32_t src_right)
{
	u32_t sc, dc;
	u32_t dest_mask = dest_size - 1;
	dc = dest_idx;
	for(sc = src_left;sc <= src_right;sc++,dc++)
		dst[dc & dest_mask] = src[sc];
}

V_LOCAL void
vec_copy_elements_generic(vec_p dst,vec_p src,u32_t dest_size,
	s32_t dest_idx,u32_t src_left,u32_t src_right)
{
	u32_t sc, dc;
	if(dest_idx < 0)
		dest_idx += ((-dest_idx % dest_size) + 1) * dest_size;
	dc = dest_idx;
	for(sc = src_left;sc <= src_right;sc++,dc++)
		dst[dc % dest_size] = src[sc];
}

V_LOCAL void
vecCopyElements(vector_p dest,vector_p source,
	s32_t dest_idx,u32_t src_left,u32_t src_right)
{
	if(src_right >= source->size)
		src_right = source->size - 1;
	if(src_left > src_right)
		return;
	if(IS_POWER_TWO(dest->size))
		vec_copy_elements_pwr2(dest->elements,source->elements,dest->size,
			dest_idx,src_left,src_right);
	else
		vec_copy_elements_generic(dest->elements,source->elements,dest->size,
			dest_idx,src_left,src_right);
}
		
vboolean
vecShift(vector_p v,s32_t amount)
{
	vector_p v2 = vecDuplicate(v);
	if(v2)
		return false;
	vecCopyElements(v,v2,amount,0,v->size - 1);
	memFree(v2);
	return true;
}

