#include	"BSprivate.h"

/* insert a nonzero into a row */
#define	INSERT(len,row,val,pos) \
{ \
int	t99, found; \
pos = -1; \
found = FALSE; \
for (t99=0;t99<len;t99++) { \
	if (row[t99] == val) { found = TRUE; break;} \
	if (row[t99] > val) { \
		found = TRUE; \
		pos = t99; \
		len++; \
		for (t99=len-1;t99>pos;t99--) { \
			row[t99] = row[t99-1]; \
		} \
		row[pos] = val; \
		break; \
	} \
} \
if (! found) {pos = len; row[len] = val; len++;} \
}

/*+ BSdo_contract - extract a lower dimension matrix from a matrix

    Input Parameters:
.   A - The sparse matrix
.   numbering - a numbering of the nodes of A
.   permutation - a permutation of the nodes of A that puts 
                  the numbering in sorted order
.   procinfo - the usual processor info
.   ident - indicates whether nodes with the same number have an
            identical graph structure (TRUE = same)

    Returns:
    the new, lower dimensional sparse matrix

+*/
BSspmat	*BSdo_contract(BSspmat *A, BSnumbering *numbering,
	BSpermutation *permutation, BSprocinfo  *procinfo, int ident)
{
	int	num_nz;
	BSspmat	*sA;
	BSpermutation	*iperm;
	BSnumbering	*pnumbering;
	BSbb *inode_contract_bb;
	int	work;
	int	pos;
	int	*addrs, *answs;
	int	count;
	void	(*map)(int,int *,int *,BSprocinfo *,BSmapping *);
	int	i, j;
	int	*colptr;
	int	ptr, max, tptr;
	int	*offset;
	BSsprow	*row;
	int	*trow, row_len;
	int	ival;
	int	first, last;
	int	max_row_len;
	int	*map_work;

	/* find number of nonlocal nonzeros */
	/* perhaps this should be stored somewhere? */
	num_nz = BSnonlocalnz(A,&max_row_len,procinfo); CHKERRN(0);
	MY_MALLOCN(map_work,(int *),sizeof(int)*max_row_len,1);
	
	/* create inverse permutation */
	iperm = BSalloc_permutation(A->num_rows); CHKERRN(0);
	BSperm2iperm(permutation,iperm); CHKERRN(0);

	/* permute numbering */
	pnumbering = BSalloc_numbering((A->num_rows+1)); CHKERRN(0);
	BSperm_ivec(numbering->numbers,pnumbering->numbers,permutation); CHKERRN(0);
	pnumbering->numbers[A->num_rows] = pnumbering->numbers[A->num_rows-1]+1;
	
	/* need my offset */
	BSoffset(1,&(pnumbering->numbers[A->num_rows]),&offset,procinfo);
	CHKERRN(0);

	/* initialize BB */
	inode_contract_bb = BSinit_bb(A->num_rows,A->map); CHKERRN(0);

	/* post info for new numbering to BB */
	for (i=0;i<A->num_rows;i++) {
		ival = (*offset) + numbering->numbers[i];
		BSpost_bb(inode_contract_bb,1,&i,&ival); CHKERRN(0);
	}

	/* create new matrix */
	MY_MALLOCN(sA,(BSspmat *),sizeof(BSspmat),1);
	sA->num_rows = pnumbering->numbers[A->num_rows];
	sA->symmetric = A->symmetric;
	sA->icc_storage = A->icc_storage;
	MY_MALLOCN(sA->rows,(BSsprow **),sizeof(BSsprow *)*sA->num_rows,2);

	/* get global number of rows */
	sA->global_num_rows = sA->num_rows;
	GISUM(&(sA->global_num_rows),1,&work,procinfo->procset); CHKERRN(0);

	/* allocate and build query */
	MY_MALLOCN(addrs,(int *),sizeof(int)*num_nz,1);
	MY_MALLOCN(answs,(int *),sizeof(int)*num_nz,2);
	count = 0;
	map = A->map->fglobal2proc;
	last = -1;
	for (i=0;i<A->num_rows;i++) {
		if ((ident) && (pnumbering->numbers[i] == last)) {
			continue;
		} else {
			last = pnumbering->numbers[i];
		}
		row = A->rows[iperm->perm[i]];
		colptr = row->col;
     	(*map)(row->length,colptr,map_work,procinfo,A->map); CHKERRN(0);
		for (j=0;j<row->length;j++) {
			if (map_work[j] != procinfo->my_id) {
				addrs[count] = colptr[j];
				count++;
			}
		}
	}

	/* query BB */
	BSquery_match_bb(inode_contract_bb,count,addrs,answs,procinfo); CHKERRN(0);
	MY_FREEN(addrs);
	BSfree_bb(inode_contract_bb); CHKERRN(0);

	/* now build contracted matrix */
	ptr = 0;
	count = 0;
	map = A->map->fglobal2local;
	for (i=0;i<sA->num_rows;i++) {
		first = TRUE;
		/* find the maximum possible length of the row */
		max = 0;
		tptr = ptr;
		while (pnumbering->numbers[tptr] == i) {
			max += A->rows[iperm->perm[tptr]]->length;
			tptr++;
			if ((ident) && (!first)) break;
			first = FALSE;
		}
		/* now add nonzero positions into the new row */
		row_len = 0;
		MY_MALLOCN(sA->rows[i],(BSsprow *),sizeof(BSsprow),i+3);
		MY_MALLOCN(trow,(int *),sizeof(int)*max,i+3);
		first = TRUE;
		while (pnumbering->numbers[ptr] == i) {
			if ((ident) && (!first)) {
				ptr++;
				continue;
			}
			first = FALSE;
			row = A->rows[iperm->perm[ptr]];
			colptr = row->col;
     		(*map)(row->length,colptr,map_work,procinfo,A->map); CHKERRN(0);
			for (j=0;j<row->length;j++) {
				if (map_work[j] < 0) {
					INSERT(row_len,trow,answs[count],pos);
					count++;
				} else {
					INSERT(row_len,trow,(*offset)+numbering->numbers[map_work[j]],
						pos);
					if (numbering->numbers[map_work[j]] == i) {
						sA->rows[i]->diag_ind = pos;
					}
				}
			}
			ptr++;
		}
		/* copy new row into position */
		sA->rows[i]->length = row_len;
		sA->rows[i]->nz = NULL;
		if (row_len == max) {
			sA->rows[i]->col = trow;
		} else {
			MY_MALLOCN(sA->rows[i]->col,(int *),sizeof(int)*row_len,i+3);
			for (j=0;j<row_len;j++) sA->rows[i]->col[j] = trow[j];
			MY_FREEN(trow);
		}
		/* set the diagonal position */
		for (j=0;j<row_len;j++) {
			if (sA->rows[i]->col[j] == (*offset)+i) {
				sA->rows[i]->diag_ind = j;
				break;
			}
		}
	}
	MY_FREEN(answs);

	BSfree_numbering(pnumbering); CHKERRN(0);
	BSfree_permutation(iperm); CHKERRN(0);

	/* set up the mapping for sA */
	MY_MALLOCN(sA->map,(BSmapping *),sizeof(BSmapping),-1);

	MY_MALLOCN(sA->map->vlocal2global,(int *),sizeof(int),-2);
	*((int *) sA->map->vlocal2global) = (*offset);
	sA->map->flocal2global = BSloc2glob;
	sA->map->free_l2g = BSfreel2g;

	MY_MALLOCN(sA->map->vglobal2local,(int *),sizeof(int),-3);
	*((int *) sA->map->vglobal2local) = (*offset);
	sA->map->fglobal2local = BSglob2loc;
	sA->map->free_g2l = BSfreeg2l;

	sA->map->vglobal2proc = 
		(void *) BSmake_off_map((*offset),procinfo,sA->global_num_rows); 
	CHKERRN(0);
	sA->map->fglobal2proc = BSglob2proc;
	sA->map->free_g2p = BSfree_off_map;

	MY_FREEN(offset);
	MY_FREEN(map_work);
	return(sA);
}