#include <v_lib.h>
#include "math.h"
#include "messages.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;

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

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

/*
 * matHistogram()
 * The implementation is quite slow.
 */
vboolean
matHistogram(vector_p dest,matrix_p src,vboolean normalize,
	vector_p histogram_space)
{
	u32_t rn, cn, rx, cx;
	u32_t sum, i, r, c, n_divs = dest->size;
    vec_t lower, upper, min, max, delta, *row;
    matMin(src,&rn,&cn);
    matMax(src,&rx,&cx);
    min = src->cell[rn][cn];
    max = src->cell[rx][cx];
    delta = (max - min) / n_divs;
    n_divs--;
	V_ASSERT(!histogram_space || histogram_space->size == dest->size);
    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] = 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] = 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()
 * The implementation is quite slow.
 */
vboolean
vecHistogram(vector_p dest,vector_p src,vboolean normalize,
	vector_p space)
{
	u32_t sum, i, j, n_divs = dest->size;
    vec_t lower, upper, min, max, delta, *ptr;
    ptr = src->elements;
    min = ptr[vecMin(src)];
    max = ptr[vecMax(src)];
    delta = (max - min) / n_divs;
    V_ASSERT(!space || (space->size = n_divs));
    n_divs--;
    for(i = 0;i < n_divs;i++)
    {
    	sum = 0;
        lower = min + i * delta;
        upper = lower + delta;
    	if(space)
    		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(space)
		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;
}


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

/*
 * matMaximizeContrast()
 */
vboolean
matMaximizeContrast(matrix_p dest,matrix_p src)
{
	u32_t r, c;
	vec_t min, max;
    matMin(src,&r,&c);
    min = src->cell[r][c];
    matMax(src,&r,&c);
    max = src->cell[r][c];
    return matAdjustContrast(dest,src,min,max);
}

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] = offset + src->elements[c] * scale;
}

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

/*
 * vecMaximizeContrast()
 */
vboolean
vecMaximizeContrast(vector_p dest,vector_p src)
{
	vec_t min, max;
    min = src->elements[vecMin(src)];
    max = src->elements[vecMax(src)];
    return vecAdjustContrast(dest,src,min,max);
}

/*
 * 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
    {
		u32_t rn, cn, rx, cx;
    	matMin(src,&rn,&cn);
    	matMax(src,&rx,&cx);
    	min = src->cell[rn][cn];
    	max = src->cell[rx][cx];
	}
    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
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;
}
