#include "btd.h"

//------------------------------------------------------------------------
// Create a new/blank BTD Matrix
//------------------------------------------------------------------------
BTD* createBTDMatrix() {
	BTD* btd = (BTD*)malloc(sizeof(BTD));
	if(btd==NULL) {
		perror("createBTDMatrix(): btd"); exit(-1);
	}
	btd->pv      = NULL;
	btd->pvMap   = NULL;
   btd->pvInvMap= NULL;
	btd->btm     = NULL;
	btd->texData = NULL;
	btd->numSArea = 0;
	btd->sArea   = NULL;
	btd->areaStarted = 0;
	return btd;
}

//------------------------------------------------------------------------
// Free space by destroying the BTD structure
//------------------------------------------------------------------------
void deleteBTDMatrix(BTD* btd) {
   if(btd!=NULL) {
      if(btd->pv      != NULL) free(btd->pv);
      if(btd->pvMap   != NULL) free(btd->pvMap);
      if(btd->pvInvMap!= NULL) free(btd->pvInvMap);
      if(btd->btm     != NULL) free(btd->btm);
      if(btd->texData != NULL) free(btd->texData);
      if(btd->sArea   != NULL) free(btd->sArea);
      free(btd);
   }
}

//------------------------------------------------------------------------
// Graph's 2D LTM short int adjacency matrix in 1D unsigned char format
//   Note: uses absolute value of negative edges to preserve precision
//------------------------------------------------------------------------
unsigned char* getBTDMatrix(BTD* btd, graph* g, float dataThresh) {
	int i, btdColorChrom = -1;
   int colorFeature = -1;
	int row,col,idx, chromNum=1;
	short int value;
	float featureValue,min,max,maxFeatureValue=20.f;
	unsigned char* retVals;
	retVals = (unsigned char*)malloc(g->numVerts*g->numVerts*sizeof(unsigned char));
	if(retVals==NULL) {
		perror("  getBTDMatrix(): malloc retVals"); exit(-1);
	}

   if (colorFeature!=-1) {

      //Get min and max of the feature number
     	min=FLT_MAX; max=FLT_MIN;
      if(g->vertData==NULL) return NULL;
	   for(i=0;i<g->numVerts;i++) {
         featureValue=g->vertData[i*(g->numVertFeatures+1)+colorFeature];
		   if(featureValue>max)
			   max=featureValue;
		   if(featureValue<min)
			   min=featureValue;
	   }

      //Color map the requested feature evenly throughout the LUT
      idx=0;
     	for(row=0;row<g->numVerts;row++) {
         featureValue = g->vertData[row*(g->numVertFeatures+1)+colorFeature];
         Normalize1f(min,max, 0.f,255.f, &featureValue);
         //featureValue = 255.f*row/g->numVerts; //TEST - Order before permute
   		for(col=0;col<g->numVerts;col++) {
            retVals[idx++] = (unsigned char)featureValue;
         }
       }

      /*//TEST - just for query-by-individual-paraclique
      idx=0;
      maxFeatureValue=-1.f;
     	for(row=0;row<g->numVerts;row++) {
         featureValue = g->vertData[row*(g->numVertFeatures+1)+colorFeature];
         if( (featureValue>=(maxFeatureValue-(1e-6))) && (featureValue<=(maxFeatureValue+(1e-6))) )
            featureValue=255.f;
         else featureValue=0.f;
   		for(col=0;col<g->numVerts;col++) {
            retVals[idx++] = (unsigned char)featureValue;
         }
       }*/
      return retVals;
   }

	idx=0;
	printf("color-mapping...");
	for(row=0;row<g->numVerts;row++) {
		for(col=0;col<g->numVerts;col++) {
			
			if(row>col) value=g->mat2d[row][col];
			else if (row<col) value=g->mat2d[col][row];
			else value=INAN;

         //Skip any non-existent edges
			if(value==INAN) retVals[idx++] = (unsigned char)0;
			else {
				//Color-code existent edges
				if(btdColorChrom==-1 && colorFeature==-1) {
					//Color same as edges
               //value=(short int)(value*value/DENOM_THRESH);
               retVals[idx++] = (unsigned char)getColorIndex(value,dataThresh,1,1);
            } else if (btdColorChrom==1) {
					//Color based on first chromosome
					featureValue = g->vertData[row*(g->numVertFeatures+1)+chromNum];
//KI: Need to verify proper chromosome
//memcpy(&key,&(g->vertData[row*(g->numVertFeatures+1)+g->numVertFeatures]),sizeof(int));
//printf(" %s %f\n",ll_getLabel(g->edgeLabels, row),featureValue);
					if(featureValue>INAN-(1e-6) && featureValue<INAN+(1e-6)) {
						featureValue=0.f;
					} else if (featureValue>maxFeatureValue) {
						featureValue=1.f;
					}
					retVals[idx++] = (unsigned char)(255.f*featureValue/maxFeatureValue);
				} else if (btdColorChrom==2) {
					//Color based on first chromosome
					featureValue = g->vertData[col*(g->numVertFeatures+1)+chromNum];
					if(featureValue>INAN-(1e-6) && featureValue<INAN+(1e-6)) {
						featureValue=0.f;
					} else if (featureValue>maxFeatureValue) {
						featureValue=1.f;
					}
					retVals[idx++] = (unsigned char)(255.f*featureValue/maxFeatureValue);
				} else { printf("Bad btdColorChrom\n"); exit(-1); }
			}

		}
	}
	return retVals;
}

//------------------------------------------------------------------------
// Rearrange original data from permutation vector to recreate matrix
//------------------------------------------------------------------------
void reconstructBTDMatrix(BTD* btd, graph* g, float dataThresh) {
	int j,k, idx, row, col, size, numRows;
	int *loc, *tag;
	unsigned char* temp;

	printf("  Reconstruct matrix...");
	// Get the matrix
	if(btd->pv==NULL) {
		printf("  reconstructBTDMatrix(): pv doesn't exist"); exit(-1);
	}
	if(btd->btm!=NULL) free(btd->btm);
	btd->btm = getBTDMatrix(btd,g,dataThresh);
	numRows = g->numVerts;

	// Allocate bookkeeping
	loc = (int*) malloc(numRows*sizeof(int));
	tag = (int*) malloc(numRows*sizeof(int));
	if(loc==NULL || tag==NULL) {
		perror("  reconstructBTDMatrix(): loc/tag"); exit(-1);
	}

	// Extra space for row swapping
	idx=0;
	size = numRows*sizeof(unsigned char);
	temp=(unsigned char*)malloc(size);
	if(temp==NULL) {
		perror("  reconstructBTDMatrix(): temp"); exit(-1);
	}
	for(row=0;row<numRows;row++) {
		loc[row]=row;
		tag[row]=row;
	}

	// permute the rows
	printf("swapping rows...");
	for(row=0;row<numRows;row++) {
		j = loc[row];
		k = btd->pv[row];

		if(j!=k) {
			//swap the rows (assumes square: numRows=numCols)
			memcpy(temp,&(btd->btm[j*numRows]),size);
			memcpy(&(btd->btm[j*numRows]),&(btd->btm[k*numRows]),size);
			memcpy(&(btd->btm[k*numRows]),temp,size);
			//swap the indices
			tag[j]   = tag[k];
			tag[k]   = row;
			loc[row] = loc[tag[j]];
			loc[tag[j]] = j;
		}
	}

	for(row=0;row<numRows;row++) {
		loc[row]=row;
		tag[row]=row;
	}

	// permute the columns
	printf("swapping columns...");
	for(row=0;row<numRows;row++) {
		j = loc[row];
		k = btd->pv[row];

		if(j!=k) {
			//swap the columns
			for(col=0;col<numRows;col++) {
				temp[col]=btd->btm[col*numRows+j];
				btd->btm[col*numRows+j]=btd->btm[col*numRows+k];
				btd->btm[col*numRows+k]=temp[col];
			}
			//swap the indices
			tag[j]      = tag[k];
			tag[k]      = row;
			loc[row]    = loc[tag[j]];
			loc[tag[j]] = j;
		}
	}

	printf("done\n");
	calcMaxBlockSize(btd);

	//HACK: hard-coded for D3
   if(strstr(g->name,"demo3")!=NULL) btd->maxBlockSize=3000;

	//Copy mapping information from permute col/vert to real col/vert
	if(btd->pvMap!=NULL) free(btd->pvMap);
	btd->pvMap = loc;

   //Build inverse map so no search time with BTD selections
   btd->pvInvMap = (int*) malloc((numRows+1)*sizeof(int));//base 1
	if(btd->pvInvMap==NULL) {
		perror("  reconstructBTDMatrix(): btd->pvInvMap"); exit(-1);
	}
   btd->pvInvMap[0]=-1; //should never ask for 0?
   for(idx=0;idx<numRows;idx++) {
      btd->pvInvMap[btd->pvMap[idx]] = idx;
   }

	free(temp);
	free(tag);
}

//------------------------------------------------------------------------
// Gives color index [0,255] for a given weight based upon parameters
//------------------------------------------------------------------------
int getColorIndex(short int weight, float dataThresh, int normalize, int separate) {
	int colorIndex;
	float numer, denom, normThresh;

	if(weight==INAN) {
		colorIndex = 0;
	}
	else if(normalize==0 && separate==0) {
		colorIndex=(int)(255.f*((float)weight/DENOM_THRESH));
		if(weight<0) colorIndex*=-1;
	}
	else if (normalize==0 && separate==1) {
		colorIndex=(int)(255.f*((float)weight/DENOM_THRESH));
		//Now in [-255,255]; going to [0,255]=255*([0,.15]+[.85,1])
		colorIndex=(int)((colorIndex+255)/2);
	}
	else if (normalize==1 && separate==0) {
		if(dataThresh<(0.0-(1e-6))) normThresh=dataThresh*-1.f;
		else normThresh=dataThresh;
		//Going to range [0,1500]
		if(weight>0) numer = (float)weight-normThresh*DENOM_THRESH;
		else numer = -1*(float)weight-normThresh*DENOM_THRESH;
		//Going to range[0,1]
		denom = (1.f-normThresh)*DENOM_THRESH;
		//Going to range[0,255]
		colorIndex=(int)(255.f*numer/denom);
	}
	else {
		if(dataThresh<(0.0-(1e-6))) normThresh=dataThresh*-1.f;
		else normThresh=dataThresh;
		//To [-1500,0]<0 & [0,1500]>0
		if(weight>0) numer = (float)weight-normThresh*DENOM_THRESH;
		else numer = -1.f * (-1*((float)weight)-normThresh*DENOM_THRESH);
		denom = (1.f-normThresh)*DENOM_THRESH;
		//Going to range [0,127]<0 & [127,255]>0
		colorIndex=(int)(255.f*(denom+numer)/(denom*2));
	}
   /*else {
		if(dataThresh<(0.0-(1e-6))) normThresh=dataThresh*-1.f;
		else normThresh=dataThresh;
		//To [-1500,0]<0 & [0,1500]>0
      if(weight>0) {
         numer = weight/DENOM_THRESH; //[.8,1]
         numer = (float)pow(2,numer); //[1.74,2]; [0,.2]=>[1,1.15] (other)
         Normalize1f((float)pow(2,normThresh),2.f, 0.f,1.f, &numer);
         if(numer<0.f || numer>1.f) printf("ERROR!\n");
      }
      else {
         numer = -1.f * ((-1.f*weight/DENOM_THRESH)-normThresh);
         numer = -1.f*(float)(pow(2,-1.f*numer)/pow(2,1.f-normThresh));//w=-.8=>0,w=-.9=>-.933,w=-1=>-1
      }
		//Going to range [0,127]<0 & [127,255]>0
		colorIndex=(int)(127.f*numer)+127;
	}*/

	return colorIndex;
}

//------------------------------------------------------------------------
// Loads the permuted matrix into texUnit, assumes tlut[256][3]
//------------------------------------------------------------------------
void bindBTDTexture(BTD* btd, float** tlut, int texUnit) {
	int i,j,numOrig,lutID,maxTexSize,texX,texY,idx=0;

	//HACK - change first voxel to encode white
	float tlut0[3];
	tlut0[0]=0.93f;	tlut0[1]=0.93f;	tlut0[2]=0.93f;
	SWAPF(tlut0[0],tlut[0][0]);
	SWAPF(tlut0[1],tlut[0][1]);
	SWAPF(tlut0[2],tlut[0][2]);
	numOrig = btd->width;

	maxTexSize = 2048; //HACK - guessing largest system texture size (doesn't work with 4096 if supported)
	btd->texSubsample=0;
	while(btd->width>maxTexSize) {
		btd->width = btd->width/2;
		btd->height = btd->height/2;
		btd->texSubsample++;
	}
	btd->texSubsample=(int)pow(2,btd->texSubsample);

	btd->texData = (float*)malloc(btd->width*btd->height*3*sizeof(float));
	if(btd->texData==NULL) {
		perror("bindBTDTexture() btd->texData"); exit(-1);
	}
	if(tlut==NULL) {
		perror("bindBTDTexture() tlut not initialized"); exit(-1);
	}

	idx=0;
   //Dumb nearest-neighbor approach, averaging would be better
	for(j=0;j<btd->height;j++) {
		for(i=0;i<btd->width;i++) {
			texX = i*btd->texSubsample;
			texY = j*btd->texSubsample;
			lutID = (int)btd->btm[texY*numOrig+texX];
			//lutID = (int)btd->btm[idx*btd->texSubsample];
			if(tlut[lutID]==NULL) {
				perror("bindBTDTexture() tlut bad idx"); exit(-1);
			}
			btd->texData[idx*3]  =tlut[lutID][0];
			btd->texData[idx*3+1]=tlut[lutID][1];
			btd->texData[idx*3+2]=tlut[lutID][2];
         //To see ColorBar
         /*btd->texData[idx*3]=tlut[(int)(255.f*i/btd->width)][0];
         btd->texData[idx*3+1]=tlut[(int)(255.f*i/btd->width)][1];
			btd->texData[idx*3+2]=tlut[(int)(255.f*i/btd->width)][2];*/
			idx++;
		}
	}

	if(btd->btm!=NULL) {
		printf("  BTD Texture (%d,%d)\n",btd->width,btd->height);
		btd->texUnit= texUnit;
		btd->texObj = vcbBindOGLTexture2D(btd->texUnit, GL_NEAREST, GL_RGB, GL_RGB, GL_FLOAT,
										  btd->width, btd->height, btd->texData, GL_REPLACE, NULL);

		glDisable(GL_TEXTURE_2D); //don't apply this texture to the main scene
	}

	//HACK - swap back to original tlut
	SWAPF(tlut0[0],tlut[0][0]); 
	SWAPF(tlut0[1],tlut[0][1]); 
	SWAPF(tlut0[2],tlut[0][2]);
}

//------------------------------------------------------------------------
// Loads the permuted matrix into texUnit with tlut mapping
//------------------------------------------------------------------------
/*void bindBTDTextureMap(BTD* btd, float** tlut, int texUnit,
					graph* g, float dataThresh, int normalize, int separate) {
	int row,col,v1,v2,idx,lutID;

	//Quick hack - change first voxel to encode white
	float tlut0[3];
	tlut0[0]=1.f;	tlut0[1]=1.f;	tlut0[2]=1.f;
	SWAPF(tlut0[0],tlut[0][0]); 
	SWAPF(tlut0[1],tlut[0][1]); 
	SWAPF(tlut0[2],tlut[0][2]); 

	btd->texData = (float*)malloc(btd->width*btd->height*3*sizeof(float));
	if(btd->texData==NULL) {
		perror("bindBTDTexture() btd->texData"); exit(-1);
	}
	if(tlut==NULL) {
		perror("bindBTDTexture() tlut not initialized"); exit(-1);
	}

// CONVERT THIS TO USE SEPARATE AND NORMALIZEEDGECOLORS
//PERHAPS FUNCTION TO ALTER tlut IDX and use in edge colors too

	idx=0;
	for(row=0;row<btd->width;row++) {
		for(col=0;col<btd->height;col++) {
			//lutID = (int)btd->btm[i];
			v1=row; v2=col;
			if(v1<v2) SWAP(v1,v2);
			if(v1!=0 && v2!=0) {
				lutID = getColorIndex(g->mat2d[v1][v2],dataThresh,normalize,separate);
			} else lutID=0;
			if(lutID<0 || lutID>255 || tlut[lutID]==NULL) {
				perror("bindBTDTexture() tlut bad idx"); exit(-1);
			}
			btd->texData[idx*3]  =tlut[lutID][0];
			btd->texData[idx*3+1]=tlut[lutID][1];
			btd->texData[idx*3+2]=tlut[lutID][2];
			idx++;
		}
	}

	if(btd->btm!=NULL) {
		printf("  BTD Texture (%d,%d)\n",btd->width,btd->height);
		btd->texUnit= texUnit;
		btd->texObj = vcbBindOGLTexture2D(btd->texUnit, GL_NEAREST, GL_RGB, GL_RGB, GL_FLOAT,
										  btd->width, btd->height, btd->texData, GL_REPLACE, NULL);

		glDisable(GL_TEXTURE_2D); //don't apply this texture to the main scene
	}

	//Quick hack - swap back to original tlut
	SWAPF(tlut0[0],tlut[0][0]); 
	SWAPF(tlut0[1],tlut[0][1]); 
	SWAPF(tlut0[2],tlut[0][2]); 
}*/

//------------------------------------------------------------------------
// Find the maximum block size for the current btm (usually after permute)
//------------------------------------------------------------------------
int calcMaxBlockSize(BTD* btd) {
	int row,col,min,max,size;
	unsigned char val;

	size=INT_MIN;
	//For each row, find the block size
	for(row=0;row<btd->height;row++) {
		min=INT_MAX;
		max=INT_MIN;
		for(col=0;col<btd->height;col++) {
			val = btd->btm[row*btd->height + col];
			if(val!=(unsigned char)0) {
				if(col<min) min=col;
				if (col>max) max=col;
			}
		}
		//if block size is biggest so far, record it
		if(max-min>size) size=max-min;
	}
	btd->maxBlockSize=size;
	return size;
}

//------------------------------------------------------------------------
// Determine if a click is on the interactive BTD
//------------------------------------------------------------------------
int btdClicked(BTD* btd, int x, int y) {
	if(x>=btd->drawX && x<=btd->drawX+btd->drawWidth &&
		y>=btd->drawY && y<=btd->drawY+btd->drawHeight) {
		return 1;
	}
	return 0;
}

//------------------------------------------------------------------------
// Process clicks to determine the start of selected blocks
//------------------------------------------------------------------------
void btdBeginPickBlocks(BTD* btd, int x, int y) {
	int* oldList;

	btd->areaStarted=1;
	if(btd->sArea!=NULL) {
		oldList=btd->sArea;
		btd->sArea=(int*)malloc(4*(btd->numSArea+1)*sizeof(int));
		if(btd->sArea==NULL) {
			perror(" btdBeginPickBlocks(): sArea"); exit(-1);
		}
		memcpy(btd->sArea,oldList,4*btd->numSArea*sizeof(int));
		free(oldList);
	} else {
		btd->sArea=(int*)malloc(4*(btd->numSArea+1)*sizeof(int));
		if(btd->sArea==NULL) {
			perror(" btdBeginPickBlocks(): sArea"); exit(-1);
		}
	}

	btd->sArea[btd->numSArea*4]=x;
	btd->sArea[btd->numSArea*4+1]=y;
   printf(" Start (%d,%d)\t",x,y);
}

//------------------------------------------------------------------------
// Process click releases to now select the appropriate blocks
//------------------------------------------------------------------------
int btdEndPickBlocks(BTD* btd, int x, int y) {
	int temp;
	if(btd->areaStarted==0) return -1;
	
	btd->sArea[btd->numSArea*4+2]=x;
	btd->sArea[btd->numSArea*4+3]=y;
	//Check to make sure 1st coordinate bottom-left, 2nd top right
	if(btd->sArea[btd->numSArea*4+2]<btd->sArea[btd->numSArea*4]) {
		temp=btd->sArea[btd->numSArea*4+2];
		btd->sArea[btd->numSArea*4+2]=btd->sArea[btd->numSArea*4];
		btd->sArea[btd->numSArea*4]=temp;
	}
	if(btd->sArea[btd->numSArea*4+3]<btd->sArea[btd->numSArea*4+1]) {
		temp=btd->sArea[btd->numSArea*4+3];
		btd->sArea[btd->numSArea*4+3]=btd->sArea[btd->numSArea*4+1];
		btd->sArea[btd->numSArea*4+1]=temp;
	}
/*printf("Received new block [(%d,%d) to (%d,%d)]\n",
	   btd->sArea[btd->numSArea*4],btd->sArea[btd->numSArea*4+1],
	   btd->sArea[btd->numSArea*4+2],btd->sArea[btd->numSArea*4+3]);*/

	btd->numSArea++;
	btd->areaStarted=0;
   printf(" End block (%d,%d)\n",x,y);
	return 0;
}

//------------------------------------------------------------------------
// Process click releases to now select the appropriate diamond regions
//------------------------------------------------------------------------
int btdEndPickDiamonds(BTD* btd, int x, int y) {
	int dist, x1,y1;

	if(btd->areaStarted==0) return -1;
	
	x1=btd->sArea[btd->numSArea*4];
	y1=btd->sArea[btd->numSArea*4+1];

   //Make first point center of diamond
	btd->sArea[btd->numSArea*4]=(x1+x)/2;
	btd->sArea[btd->numSArea*4+1]=(y1+y)/2;

	x1=btd->sArea[btd->numSArea*4];
	y1=btd->sArea[btd->numSArea*4+1];
   dist=(int)sqrt((x-x1)*(x-x1)+(y-y1)*(y-y1));
   
   //Make 2nd point the distance to diamond points with -1 flag
   btd->sArea[btd->numSArea*4+2]=dist;
	btd->sArea[btd->numSArea*4+3]=-1;

/*printf("Received new diamond [center (%d,%d), offset (%d)]\n",
	   btd->sArea[btd->numSArea*4],btd->sArea[btd->numSArea*4+1],
	   btd->sArea[btd->numSArea*4+2]);*/

	btd->numSArea++;
	btd->areaStarted=0;
   printf(" End diamond (%d,%d)\n",x,y);
	return 0;
}

//------------------------------------------------------------------------
// Convert screen/pixel space to row/col of btd space [0,btd.height]
//------------------------------------------------------------------------
void screen2btd(BTD* btd, int* x, int* y) {
	int newX[1], newY[1];
	if(btd==NULL || x==NULL || y==NULL) {
		printf("  screen2btd(): Not performing conversion\n");
		return;
	}
	
	newX[0] = (int)((x[0]-btd->drawX)/(float)btd->drawWidth*btd->height);
	newY[0] = (int)((y[0]-btd->drawY)/(float)btd->drawHeight*btd->height);
	x[0] = newX[0];	y[0] = newY[0];
}

//------------------------------------------------------------------------
// Convert screen/pixel space to row/col of belt space [0,beltWid/Height]
//------------------------------------------------------------------------
void screen2belt(BTD* btd, int* x, int* y) {
	int newX[1], newY[1];
	if(btd==NULL || x==NULL || y==NULL) {
		printf("  screen2belt(): Not performing conversion\n");
		return;
	}
	
	newX[0] = x[0]-btd->drawX;
	newY[0] = y[0]-btd->drawY;
	x[0] = newX[0];	y[0] = newY[0];
}

//------------------------------------------------------------------------
// Convert row/col of belt space diagonal to row/col of matrix
//------------------------------------------------------------------------
int belt2btd(BTD* btd, int* x, int* y) {
	int newX, newY;
	float normX, normY, scaleY;

	if(btd==NULL || x==NULL || y==NULL) {
		printf("  belt2btd(): Not performing conversion\n");
		return -1;
	}
	//Undefined on the corners of the belt
	if( x[0]<abs(y[0]-btd->drawHeight/2) ||
		btd->drawWidth-x[0]<abs(y[0]-btd->drawHeight/2) ) {
		return -1;
	}
	
	//Following equation derived from 3x3 translation+rotation matrix
	//  rot*scale*trans*coord where rot(45degrees) and trans(-0.5 y)
	//  and scale(sqrt(2) x, maxBlockSize/(height*sqrt(2)) y)
	normX = x[0]/(float)btd->drawWidth;
	normY = y[0]/(float)btd->drawHeight;
	scaleY = btd->maxBlockSize/(btd->height*SQRT2);
	// Without scaling:
	//normX = (float) (COS315*normX - COS315*normY + COS315/2.f);
	//normY = (float) (COS315*normX + COS315*normY - COS315/2.f);
	normX = (float) (normX + COS315*scaleY*normY - COS315*scaleY/2.f);
	normY = (float) (normX - COS315*scaleY*normY + COS315*scaleY/2.f);
	newX = (int)( btd->height * normX );
	newY = (int)( btd->height * normY );

	//Check for precision errors
	if(newX<0 || newY<0 || newX>=btd->height || newY>=btd->height) {
		//printf("  Precision error: (%d,%d) out of range\n",newX,newY);
		return -1;
	}

   newX *= btd->texSubsample;
   newY *= btd->texSubsample;
	x[0] = newX;
	y[0] = newY;
	return 0;
}

//------------------------------------------------------------------------
// Get permutation vector used to create the block tridiagonalized matrix
//------------------------------------------------------------------------
BTD* getBTDMatrixPV(char* filename, graph* g, float dataThresh,
					float** tlut, int texUnit) {
	int i;
	FILE* fp;
	BTD* btd = createBTDMatrix();

	//Read permutation vector file
	fp=fopen(filename,"r");
	if(fp==NULL) {
		printf("  %s does not exist\n",filename);
		return NULL;
	}
	fscanf(fp,"%d",&btd->width);
	btd->height=btd->width; //assumes square matrix

	btd->pv = (int*)malloc(btd->width*sizeof(int));
	if(btd->pv == NULL) {
		perror("  getBTDMatrixPV(): btd->pv"); exit(-1);
	}
	printf("  Reading %s...",filename);
	for(i=0;i<btd->width;i++) {
		fscanf(fp,"%d ",&(btd->pv[i]));
		btd->pv[i]--; //make base 0 (fortran base 1)
	}
	printf("done\n");

	reconstructBTDMatrix(btd, g,dataThresh);
	bindBTDTexture(btd, tlut,texUnit);

	//Texture may have been resized so have to change belt info
	//  Warning: integer division truncation, may result in subpixel difference
	btd->maxBlockSize = btd->maxBlockSize/btd->texSubsample;
	btd->beltOffset = (float)btd->maxBlockSize/(float)btd->height/2.f/SQRT2;

	//bindBTDTextureMap(btd, tlut,texUnit, g,dataThresh,0,1);
	//vcbImgWritePGM("TestPermut.pgm", btd->btm, 1, btd->width, btd->height);

	fclose(fp);
	return btd;
}

//------------------------------------------------------------------------
// Get block tridiagonalized matrix from a PGM file, bind to texture
//------------------------------------------------------------------------
BTD* getBTDMatrixPGM(char* filename, graph* g, float dataThresh,
					 float** tlut, int texUnit) {
	BTD* btd = createBTDMatrix();
	btd->btm=vcbImgReadPGM(filename, &btd->width, &btd->height);
	bindBTDTexture(btd, tlut,texUnit);
	//bindBTDTextureMap(btd, tlut,texUnit, g,dataThresh,0,1);
	return btd;
}

//------------------------------------------------------------------------
// Write the permutation vector and BTD matrix to the file
//------------------------------------------------------------------------
void writePVData(BTD* btd, FILE* fp) {
	int row,col,idx=0;

	fprintf(fp,"  Old PV: ");
	for(row=0;row<btd->width;row++)
		fprintf(fp,"%d\t",(int)btd->pv[row]);

	fprintf(fp,"\n");
	for(row=0;row<btd->width;row++) {
		for(col=0;col<btd->height;col++) {
			fprintf(fp,"%d\t",(int)btd->btm[idx++]);
		}
		fprintf(fp,"\n");
	}
}

//------------------------------------------------------------------------
// Mark all BTD belt selections in the given index
//------------------------------------------------------------------------
void markBTDMatrixBelt(BTD* btd, graph* g, int onFlag) {
	int i,j,k, d;
	int x[2],y[2], btdX[1],btdY[1];

	// Add back only vertices selected
	// Note: pvMap is base 1 from fortran code
	for(i=0;i<btd->numSArea;i++) {
		//Screen space (pixel) coordinates
		x[0]=btd->sArea[4*i];		y[0]=btd->sArea[4*i+1];
		x[1]=btd->sArea[4*i+2];		y[1]=btd->sArea[4*i+3];

      //Diamond selections
      if(y[1]==-1) {
         d=x[1];
         x[0]=x[0]-d; //left
         x[1]=x[0]+d;  y[1]=y[0]; //right

		   //Belt space (row/col vert) coordinates
		   screen2belt(btd, &(x[0]),&(y[0]));
         screen2belt(btd, &(x[1]),&(y[1]));
         if (belt2btd(btd,&(x[0]),&(y[0])) == -1 ||
             belt2btd(btd,&(x[1]),&(y[1])) == -1) {
            printf(" markBTDMatrixBelt(): Error in diamond selection mapping");
            return;
         }
         printf(" Diamond at (%d,%d) to (%d,%d)\n",x[0],y[0],x[1],y[1]);

		   //Show all verts selected
		   for(j=x[0];j<x[1];j++) {
				g->vertColorList[ btd->pvInvMap[j] ] = onFlag;
         }
         for(k=y[0];k<y[1];k++) {
				g->vertColorList[ btd->pvInvMap[k] ] = onFlag;
		   }
      }

      //Rectangular selections
      else {
		   //Belt space (row/col vert) coordinates
		   screen2belt(btd, &(x[0]),&(y[0]));
		   screen2belt(btd, &(x[1]),&(y[1]));
		   
		   //Show all verts selected
		   for(j=x[0];j<x[1];j++) {
			   for(k=y[0];k<y[1];k++) {
				   //Matrix data space (row/col vert) coordinates
				   btdX[0] = j;	btdY[0] = k;
				   if( belt2btd(btd,btdX,btdY) != -1) {
					   g->vertColorList[ btd->pvInvMap[btdX[0]]-1 ] = onFlag;
					   g->vertColorList[ btd->pvInvMap[btdY[0]]-1 ] = onFlag;
				   }
			   }
		   }
      }

	}
}

//------------------------------------------------------------------------
// Mark all BTD matrix selections in the given index
//------------------------------------------------------------------------
void markBTDMatrix(BTD* btd, graph* g, int onFlag) {
	int i,j,k, d,offFlag = -1;
	int x[2],y[2];

	for(i=0;i<g->numVerts;i++) g->vertColorList[i]=offFlag;

	// Add back only vertices selected
	// Note: pvMap is base 1 from fortran code
	for(i=0;i<btd->numSArea;i++) {
		//Screen space (pixel) coordinates
		x[0]=btd->sArea[4*i];		x[1]=btd->sArea[4*i+2];
		y[0]=btd->sArea[4*i+1];		y[1]=btd->sArea[4*i+3];
      //Diamond selections
      if(y[1]==-1) {
         d=x[1];
         x[0]=x[0]-d; //left
         x[1]=x[0]+d;  y[1]=y[0]; //right

		   //Belt space (row/col vert) coordinates
		   screen2belt(btd, &(x[0]),&(y[0]));
         screen2belt(btd, &(x[1]),&(y[1]));
         if (belt2btd(btd,&(x[0]),&(y[0])) == -1 ||
             belt2btd(btd,&(x[1]),&(y[1])) == -1) {
            printf(" markBTDMatrixBelt(): Error in diamond selection mapping");
            return;
         }
         //printf(" Diamond at (%d,%d) to (%d,%d)\n",x[0],y[0],x[1],y[1]);

		   //Show all verts selected
		   for(j=x[0];j<x[1];j++) {
				g->vertColorList[ btd->pvInvMap[j] ] = onFlag;
         }
         for(k=y[0];k<y[1];k++) {
				g->vertColorList[ btd->pvInvMap[k] ] = onFlag;
		   }
      }

      //Rectangular selections
      else {
		   screen2btd(btd, &(x[0]),&(y[0]));
		   screen2btd(btd, &(x[1]),&(y[1]));
		   //Show all verts selected
		   for(j=x[0];j<x[1];j++) {
			   for(k=y[0];k<y[1];k++) {
				   //Matrix data space (row/col vert) coordinates
				   g->vertColorList[ btd->pvInvMap[j] ] = onFlag;
				   g->vertColorList[ btd->pvInvMap[k] ] = onFlag;
			   }
		   }
      }
	}
}

//------------------------------------------------------------------------
// Mark all BTD belt selections as index based on order selected
//   In overlap, first selection takes precedence
//------------------------------------------------------------------------
void markBTDMatrixBeltUnique(BTD* btd, graph* g) {
	int i,j,k, d,offFlag = -1;
	int x[2],y[2], btdX[1],btdY[1];

	if(btd->numSArea > 255) {
		printf("  Warning: not valid color index\n");
	}
	for(i=0;i<g->numVerts;i++) g->vertColorList[i]=offFlag;

	// Add back only vertices selected
	// Note: pvMap is base 1 from fortran code
	//       reverse order to give preference to first selected block
	for(i=btd->numSArea-1;i>=0;i--) {
		//Screen space (pixel) coordinates
		x[0]=btd->sArea[4*i];		x[1]=btd->sArea[4*i+2];
		y[0]=btd->sArea[4*i+1];		y[1]=btd->sArea[4*i+3];

      //Diamond selections
      if(y[1]==-1) {
         d=x[1];
         x[0]=x[0]-d; //left
         x[1]=x[0]+d;  y[1]=y[0]; //right

		   //Belt space (row/col vert) coordinates
		   screen2belt(btd, &(x[0]),&(y[0]));
         screen2belt(btd, &(x[1]),&(y[1]));
         if (belt2btd(btd,&(x[0]),&(y[0])) == -1 ||
             belt2btd(btd,&(x[1]),&(y[1])) == -1) {
            printf(" markBTDMatrixBelt(): Error in diamond selection mapping");
            return;
         }
         //printf(" Diamond at (%d,%d) to (%d,%d)\n",x[0],y[0],x[1],y[1]);

		   //Show all verts selected
		   for(j=x[0];j<x[1];j++) {
				g->vertColorList[ btd->pvInvMap[j] ] = i;
         }
         for(k=y[0];k<y[1];k++) {
				g->vertColorList[ btd->pvInvMap[k] ] = i;
		   }
      }

      //Rectangular selections
      else {
         //Belt space (row/col vert) coordinates
		   screen2belt(btd, &(x[0]),&(y[0]));
		   screen2belt(btd, &(x[1]),&(y[1]));
		   
		   //Show all verts selected
		   for(j=x[0];j<x[1];j++) {
			   for(k=y[0];k<y[1];k++) {
				   //Matrix data space (row/col vert) coordinates
				   btdX[0] = j;	btdY[0] = k;
				   if( belt2btd(btd,btdX,btdY) != -1) {
					   g->vertColorList[ btd->pvInvMap[btdX[0]] ] = i;
					   g->vertColorList[ btd->pvInvMap[btdY[0]] ] = i;
				   }
			   }
		   }
      }
	}
}

//------------------------------------------------------------------------
// Mark all BTD matrix selections as index based on order selected
//   In overlap, first selection takes precedence
//------------------------------------------------------------------------
void markBTDMatrixUnique(BTD* btd, graph* g) {
	int i,j,k, d, offFlag=-1;
	int x[2],y[2];

	if(btd->numSArea > 255) {
		printf("  Warning: not valid color index\n");
	}
	for(i=0;i<g->numVerts;i++) g->vertColorList[i]=offFlag;


	// Add back only vertices selected
	// Note: pvMap is base 1 from fortran code
	for(i=btd->numSArea-1;i>=0;i--) {
		//Screen space (pixel) coordinates
		x[0]=btd->sArea[4*i];		x[1]=btd->sArea[4*i+2];
		y[0]=btd->sArea[4*i+1];		y[1]=btd->sArea[4*i+3];

      //Diamond selections
      if(y[1]==-1) {
         d=x[1];
         x[0]=x[0]-d; //left
         x[1]=x[0]+d;  y[1]=y[0]; //right

		   //Belt space (row/col vert) coordinates
		   screen2belt(btd, &(x[0]),&(y[0]));
         screen2belt(btd, &(x[1]),&(y[1]));
         if (belt2btd(btd,&(x[0]),&(y[0])) == -1 ||
             belt2btd(btd,&(x[1]),&(y[1])) == -1) {
            printf(" markBTDMatrixBelt(): Error in diamond selection mapping");
            return;
         }
         //printf(" Diamond at (%d,%d) to (%d,%d)\n",x[0],y[0],x[1],y[1]);

		   //Show all verts selected
		   for(j=x[0];j<x[1];j++) {
				g->vertColorList[ btd->pvInvMap[j] ] = i;
         }
         for(k=y[0];k<y[1];k++) {
				g->vertColorList[ btd->pvInvMap[k] ] = i;
		   }
      }

      //Rectangular selections
      else {
		   screen2btd(btd, &(x[0]),&(y[0]));
		   screen2btd(btd, &(x[1]),&(y[1]));
		   //Show all verts selected
		   for(j=x[0];j<x[1];j++) {
			   for(k=y[0];k<y[1];k++) {
				   //Matrix data space (row/col vert) coordinates
				   g->vertColorList[ btd->pvInvMap[j] ] = i;
				   g->vertColorList[ btd->pvInvMap[k] ] = i;
			   }
		   }
      }
	}
}

short int** getAverageWeight(BTD* btd, graph* g, graph* lod) {
	int i,k,el1,el2,v1,v2,count;
	short int** avgWeight;
   //int max,min,element,btdloc,*pos;

	// Calculate average weight between all areas
	//   note: don't free or you'll lose the original data
	if( (avgWeight=(short int**)malloc(btd->numSArea*sizeof(short int*))) == NULL) {
		perror("createLOD() avgWeight"); exit(-1);
	}
	avgWeight[0]=NULL;
	for(i=1;i<btd->numSArea;i++) {
		if( (avgWeight[i]=(short int*)malloc(i*sizeof(short int))) == NULL) {
			perror("createLOD() avgWeight[]"); exit(-1);
		}
	}
	for(i=1;i<btd->numSArea;i++) {
		for(k=0;k<i;k++) {
			avgWeight[i][k] = INAN;
		}
	}

	//Average the weights
	count = 0;
	for(i=1;i<btd->numSArea;i++) {
/*
//Tells which lod supernodes correspond to which selections
printf("\n\n%d CONTAINS:\n",i);
pos=(int*)malloc(g->numBin[i]*sizeof(int));
for(el1=0; el1<g->numBin[i]; el1++) {
   element=g->lodBin[i][el1];
   btdloc=btd->pvMap[element];
   pos[el1]=btdloc;
}
getMinMaxiv(g->numBin[i], pos, &min, &max);
printf("%d to %d",min,max);*/
		for(k=0;k<i;k++) {

   		for(el1=0;el1<g->numBin[i];el1++) {
				for(el2=0;el2<g->numBin[k];el2++) {
               v1 = g->lodBin[i][el1];
					v2 = g->lodBin[k][el2];
					if(v1<v2) SWAP(v1,v2);
					if(g->mat2d[v1][v2]!=INAN) {
						if(avgWeight[i][k] == INAN) {
							count++;
							avgWeight[i][k] = g->mat2d[v1][v2]/g->numBin[i];
//printf("avgWeight[%d][%d]=%d/%d=%d from (%d,%d)\n",i,k,g->mat2d[v1][v2],g->numBin[i],avgWeight[i][k],v1,v2);
                  } else {
							avgWeight[i][k] += g->mat2d[v1][v2]/g->numBin[i];
//printf("%d+%d/%d=%d\n",avgWeight[i][k]-lod->mat2d[v1][v2]/lod->numBin[i],lod->mat2d[v1][v2],lod->numBin[i],avgWeight[i][k]);
						}
					}
				}
			}

         if(avgWeight[i][k]!=INAN) avgWeight[i][k]=avgWeight[i][k]/g->numBin[k];
//printf("avgWeight[%d][%d]=%d/%d=%d\n",i,k,avgWeight[i][k]*g->numBin[k],g->numBin[k],avgWeight[i][k]);
		}
	}

	lod->numEdges = count;
	//lod->mat2d = avgWeight;
   return avgWeight;
}

int* getNumVertsInBin(BTD* btd, graph* lod) {
	int i, offFlag = -1;
	int *numVertsInBin;

	if( (numVertsInBin=(int*)malloc( btd->numSArea*sizeof(int))) == NULL) {
		perror("createLOD() numVertsInBin"); exit(-1);
	}
	for(i=0;i<btd->numSArea;i++) numVertsInBin[i]=0;
	for(i=0;i<lod->numVerts;i++) {
		if(lod->vertColorList[i] != offFlag) {
			numVertsInBin[lod->vertColorList[i]]++;
		}
	}

	//for(i=0;i<btd->numSArea;i++) printf(" %dv in bin %d\n",numVertsInBin[i],i);
	return numVertsInBin;
}

int** getVertIdx(BTD* btd, graph* lod) {
	int i,tidx, offFlag = -1;
	int* idx;
	int** vertIdx;

	if( (idx=(int*)malloc( btd->numSArea*sizeof(int) )) == NULL) {
		perror("createLOD() idx"); exit(-1);
	}
	for(i=0;i<btd->numSArea;i++) idx[i]=0;

	if( (vertIdx=(int**)malloc( btd->numSArea*sizeof(int*) )) == NULL) {
		perror("createLOD() vertIdx"); exit(-1);
	}
	for(i=0;i<btd->numSArea;i++) {
		if( (vertIdx[i]=(int*)malloc( lod->numBin[i]*sizeof(int) )) == NULL) {
			perror("createLOD() vertIdx[]"); exit(-1);
		}
	}

	// Arrange the vertex IDs into their appropriate bin
	for(i=0;i<lod->numVerts;i++) {
		if(lod->vertColorList[i] != offFlag) {
			//find vert and put in list
			tidx=lod->vertColorList[i];
			vertIdx[tidx][idx[tidx]] = i;
			idx[tidx]+=1;
			//printf(" %d in bin %d\n",i,tidx);
		}
	}
	for(i=0;i<btd->numSArea;i++) {
		if(idx[i]!=lod->numBin[i]) {
			printf("  Warning: createLOD() calculation error\n");
		}
	}
	free(idx);
	return vertIdx;
}

//------------------------------------------------------------------------
// Create a graph based on the newly selected area
//   Note: loops never tested for numSArea>1
//------------------------------------------------------------------------
graph* createLOD(BTD* btd, graph* g) {
	int i,j, tidx;
	short int *avgPosn;
	graph* lod;

   int k,count;
   short int value,maxPos,minPos,maxNeg,minNeg;

	if(btd==NULL) {
		printf("No BTD selections to create LOD from\n");
		return NULL;
	}

	// Level-of-detail graph
	lod = sCopy(g);
	if( (lod->name=(char*)malloc( (strlen("LOD layout")+1)*sizeof(char))) == NULL) {
		perror("createLOD() lod->name"); exit(-1);
	}
	strcpy(lod->name,"LOD layout");

	// Different display lists but same data
	lod->numDisplayVerts = 0;
	lod->vertList = NULL;
	lod->numDisplayEdges = 0;
	lod->edgeList = NULL;

	// Don't free or you'll lose the original data
	if((lod->vertColorList=(int*)malloc(lod->numVerts*sizeof(int))) == NULL) {
		perror("createLOD() vertColorList"); exit(-1);
	}

	// Label all vertices
	if(btd->drawingBelt == 1) {
		markBTDMatrixBeltUnique(btd, lod);
	} else {
		markBTDMatrixUnique(btd, lod);
	}

	// Get array of the number of vertices for each selection
	if(lod->numBin != NULL) free(lod->numBin);
	lod->numBin = getNumVertsInBin(btd, lod);

	// Get array of vertex indices for each cluster
	if(lod->lodBin != NULL) {
		for(i=0;i<lod->numVerts;i++) free(lod->lodBin[i]);
		free(lod->lodBin);
	}
	lod->lodBin = getVertIdx(btd, lod); // all verts not -1

	// Calculate average position for each area
	//  Assumes: Using displayThresh=0 layout (may not work with multilayout mode)
	if( (avgPosn=(short int*)malloc(3*btd->numSArea*sizeof(short int))) == NULL) {
		perror("createLOD() avgPosn"); exit(-1);
	}
	for(i=0;i<btd->numSArea;i++) {
		avgPosn[3*i  ] = 0;
		avgPosn[3*i+1] = 0;
		avgPosn[3*i+2] = 0;
	}
	for(i=0;i<btd->numSArea;i++) {
		for(j=0;j<lod->numBin[i];j++) {
			tidx = lod->lodBin[i][j];
			avgPosn[3*i  ] += lod->vertPosn[3*tidx  ];
			avgPosn[3*i+1] += lod->vertPosn[3*tidx+1];
			avgPosn[3*i+2] += lod->vertPosn[3*tidx+2];
		}
		avgPosn[3*i  ] = avgPosn[3*i  ]/lod->numBin[i];
		avgPosn[3*i+1] = avgPosn[3*i+1]/lod->numBin[i];
		avgPosn[3*i+2] = avgPosn[3*i+2]/lod->numBin[i];
//printf(" Average Position for bin %d = (%d,%d,%d)\n",i,avgPosn[3*i],avgPosn[3*i+1],avgPosn[3*i+2]);
	}

	// Reformat the entire graph to contain only the essential LOD data
	//   note: be careful of free since lod is shallow copy pointing to original data
	lod->vertPosn = avgPosn;
	
	//Hack: Not sure if this is best solution (sharing data)
	g->numBin=lod->numBin;
	g->lodBin=lod->lodBin;

	lod->mat2d = getAverageWeight(btd,g, lod); //lod->mat2d & lod->numEdges
	lod->numVerts = btd->numSArea;


   //Normalize to proper range
   maxPos=minNeg=SHRT_MIN;
   minPos=maxNeg=SHRT_MAX;
   for(i=1;i<btd->numSArea;i++) {
		for(k=0;k<i;k++) {
         value = lod->mat2d[i][k];
         if(value!=INAN) {
            if(value>0) {
               if(value>maxPos) maxPos=value;
               else if(value<minPos) minPos=value;
            }
            else {
               if(value<maxNeg) maxNeg=value;
               else if(value>minNeg) minNeg=value;
            }
         }
		}
	}

   count=0;
   for(i=1;i<btd->numSArea;i++) {
		for(k=0;k<i;k++) {
         value = lod->mat2d[i][k];
         if(value!=INAN) {
            if(value>0) {
               Normalize1sd( minPos,maxPos, 8000,10000, &(lod->mat2d[i][k]) );
            }
            else {
               Normalize1sd( maxNeg,minNeg, -10000,-8000, &(lod->mat2d[i][k]) );
            }
            count++;
         }
         printf("avgWeight[%d][%d]=%d\n",i,k,lod->mat2d[i][k]);
		}
	}
   lod->numVerts=count;

   /*//Shows there's error with getAverageWeight mapping
   //Perhaps not [8000,10000]
   count=0;
   for(i=1;i<btd->numSArea;i++) {
		for(k=0;k<i;k++) {
         if(value!=INAN) lod->mat2d[i][k]=-8000;
         count++;
		}
	}
   lod->numEdges=count;*/

	// Set up to be displayed correctly
	// Turn all verts on
	showAll(lod,255);

	lod->offset[2] = 77;
	for(i=0;i<lod->numVerts;i++)
		lod->vertColorList[i] = (i*58+58)%255;//HACK: same eqn from createSubgraph()

	// Display new lod graph info
	for(i=0;i<lod->numVerts;i++) {
		printf("  Vert %d = (%d,%d,%d)\n",i,lod->vertPosn[i*3],lod->vertPosn[i*3+1],lod->vertPosn[i*3+2]);
	}
	for(i=1;i<btd->numSArea;i++) {
		for(j=0;j<i;j++) {
			if(lod->mat2d[i][j]!=INAN) {
				printf("  Edge (%d,%d) = %d\n",i,j,lod->mat2d[i][j]);
			}
		}
	}

	return lod;
}

//------------------------------------------------------------------------
// Update a graph based on the newly selected area
//------------------------------------------------------------------------
void updateLastLOD(BTD* btd, graph* g, graph* lod) {
	int i,vid,tidx, size, numVerts;
	int *vertList, *bin;
	short int *vertPosn, avgPosn[3];

	// Different display lists but same data
	lod->numDisplayVerts = 0;
	lod->numDisplayEdges = 0;
	if(lod->edgeList != NULL) {
		free(lod->edgeList);
		lod->edgeList=NULL;
	}

	// Allocate space for one more vertex information
	size = (lod->numVerts+1)*sizeof(int);
	vertList = (int*) malloc( size );
	if(vertList == NULL) {
		perror("updateLastLOD(): vertList"); exit(-1);
	}
	memcpy(vertList,lod->vertList, size - sizeof(int));
	free(lod->vertList);
	lod->vertList = vertList;

	size = 3*(lod->numVerts+1)*sizeof(short int);
	vertPosn = (short int*) malloc( size );
	if(vertPosn == NULL) {
		perror("updateLastLOD(): vertPosn"); exit(-1);
	}
	memcpy(vertPosn,lod->vertPosn, size - 3*sizeof(short int));
	free(lod->vertPosn);
	lod->vertPosn = vertPosn;

	lod->numVerts++;

	// Label all vertices (original graph)
	if(btd->drawingBelt == 1) {
		markBTDMatrixBeltUnique(btd, g);
	} else {
		markBTDMatrixUnique(btd, g);
	}

	// Find avg vert posn for new vertex location
	numVerts = 0;
	vid = lod->numVerts - 1;
	for(i=0;i<g->numVerts;i++) {
		if(g->vertColorList[i] == vid) {
			numVerts++;
		}
	}
	//List of new vertices
	bin=(int*)malloc( numVerts*sizeof(int) );
	if(bin == NULL) {
		perror("updateLastLOD(): bin"); exit(-1);
	}
	tidx=0;
	for(i=0;i<g->numVerts;i++) {
		if(g->vertColorList[i] == vid) {
			bin[tidx++] = g->vertColorList[i];
		}
	}

	vid = lod->numVerts - 1;
	avgPosn[0]=0; avgPosn[1]=0; avgPosn[2]=0; 
	for(i=0;i<numVerts;i++) {
		tidx = bin[i];
		avgPosn[0] += g->vertPosn[3*tidx  ];
		avgPosn[1] += g->vertPosn[3*tidx+1];
		avgPosn[2] += g->vertPosn[3*tidx+2];
	}

	// This happens when a selection box has all vertices
	// contained by previous selection boxes
	if(numVerts==0) numVerts=1;

	avgPosn[0] = avgPosn[0]/numVerts;
	avgPosn[1] = avgPosn[1]/numVerts;
	avgPosn[2] = avgPosn[2]/numVerts;
	//Set vertex position to be average of all constituents
	lod->vertPosn[3*(lod->numVerts-1)  ] = avgPosn[0];
	lod->vertPosn[3*(lod->numVerts-1)+1] = avgPosn[1];
	lod->vertPosn[3*(lod->numVerts-1)+2] = avgPosn[2];

	lod->numDisplayEdges = 0;
	lod->edgeList = NULL;

	// Add space to vertPosn, recalculate averages for new selection, put in
	for(i=0;i<lod->numVerts-1;i++) {
		if(lod->mat2d[i] != NULL) free(lod->mat2d[i]);
	}
	free(lod->mat2d);

	// Get average weights same as before (didn't save indices for each LOD vertex)
	if(lod->numBin != NULL) free(lod->numBin);
	if(lod->lodBin != NULL) {
		for(i=0;i<lod->numVerts-1;i++) free(lod->lodBin[i]);
		free(lod->lodBin);
	}

	//Get array of the number of vertices for each selection
	g->numBin = getNumVertsInBin(btd, g);
	lod->numBin = g->numBin;
	//Get array of vertex indices for each cluster
	g->lodBin = getVertIdx(btd, g); // all verts not -1
	lod->lodBin = g->lodBin;
	lod->mat2d = getAverageWeight(btd,g,lod);

	// Set up to be displayed correctly
	// Turn all verts on
	showAll(lod,255);

	// Display all data
	/*for(i=0;i<lod->numDisplayVerts;i++) {
		vid = lod->vertList[i];
		printf("  Vert %d = (%d,%d,%d)\n",vid,lod->vertPosn[vid*3],lod->vertPosn[vid*3+1],lod->vertPosn[vid*3+2]);
	}
	for(i=0;i<lod->numDisplayEdges;i++) {
		printf("  Edge (%d,%d) = %d\n",lod->edgeList[2*i],lod->edgeList[2*i+1],g->mat2d[g->edgeList[2*i]][g->edgeList[2*i+1]]);
	}
	for(i=0;i<lod->numVerts;i++) {
		printf("  Vert %d = (%d,%d,%d)\n",i,lod->vertPosn[i*3],lod->vertPosn[i*3+1],lod->vertPosn[i*3+2]);
	}
	for(i=1;i<btd->numSArea;i++) {
		for(j=0;j<i;j++) {
			if(lod->mat2d[i][j]!=INAN) {
				printf("  Edge (%d,%d) = %d\n",i,j,lod->mat2d[i][j]);
			}
		}
	}*/

	// Free extra data
	free(bin);
}

int graphBTD(BTD* btd, graph* g, char* filename, int size, int* verts) {
   int i,j, gid1,gid2;
   float weight;
	FILE* fout;

	fout = fopen(filename,"w");
	if(fout==NULL) {
		printf("Error: can't open %s for writing.\n", filename);
		return -1;
	}
   for(i=0;i<size;i++) {
      if(verts[i]<0 || verts[i]>g->numVerts) {
         printf("  graphBTD(): %d invalid index\n",i);
         return -1;
      }
   }

   for(i=0;i<size;i++) {
      fprintf(fout," BTD_id %d ",verts[i]);
      fprintf(fout," Graph_id %d\t",btd->pvInvMap[verts[i]]);
   }
   fprintf(fout,"\n");
   for(j=0;j<g->numVerts;j++) {
      gid2=btd->pvInvMap[j];
      for(i=0;i<size;i++) {
         gid1=btd->pvInvMap[verts[i]];
         if(gid1==gid2) weight=1.f;
         else if(gid1>gid2) weight=g->mat2d[gid1][gid2]/DENOM_THRESH;
         else weight=g->mat2d[gid2][gid1]/DENOM_THRESH;
   
         if(weight==-3.f) weight=-0.1f;
         else if(weight<0) weight*=-1.f;
         fprintf(fout,"%0.4f\t",weight);
      }
      fprintf(fout,"\n");
   }
   fclose(fout);

   return 0;
}
