/* Fordham Robotics and Computer Vision lab 2010, 2011 dml */

/* 
 CHECK SALIENCE

 This code will check saliance on stereo camera data given to it.
 The result is an array of salient spots in space, with a score associated with each.
 The array can be reset if nothing interesting is found or when finished with this location.
 Otherwise the array remains and any additional salient spots are cross-checked with these for stability.
 Stable salient spots have their scores increased, as do salient spots beside one another.
 At any point, the best salient cluster can be identified and the physical location identified.

*/

#include "Aria.h"
#include "ArNetworking.h"

#include <stdlib.h>
#include <unistd.h>
#include <signal.h>

#include <cv.h>
#include <highgui.h>

#include <math.h>

//#include <iostream>
#include <algorithm>
#include <vector>

#define OPENCV
#include "stereoCamera.h"
#include "checkSalience.h"

using namespace std; // needed for the STL stuff

extern void transformBumbleBee2World(double from[3], double to[3], 
				     double pan, double tilt,
				     double rx, double ry, double rth);


void salienceChecker::doSaliency(double pan, double tilt, ArRobot *r) {
  getStereoData();
  targetBox = saliencyArchitecture(pan, tilt, r);
}

void salienceChecker::getStereoData() {

  printf("\nSalienceChecker getting stereodataset ...\n");
  cvZero(imgBGR); cvZero(imgSpace);
  myStereoCamera->doStereoFrame(imgBGR, disparity, imgSpace, (FILE *)0);
  printf("             ... done\n");
  cvCvtColor(imgBGR,imgLab,CV_BGR2Lab);
  return;
}


/*********------------BASIC SALIENCE---------------------*********/
void 
salienceChecker::saliencySquares(int rows, int cols, 
		IplImage *img, IplImage *spc,// input color and depth image
		IplImage *fColorA, IplImage *fColorS,// output color mean and SD
		IplImage *fSpaceA, IplImage *fSpaceS
		) {
  int h = img->height, w = img->width,
    i, j, k, l, hInc = h/rows, wInc=w/cols;
  CvRect thisSquare;
  CvScalar mC, sC, mS, sS;

  cvZero(fColorS); cvZero(fColorA); cvZero(fSpaceS); cvZero(fSpaceA);
  thisSquare.width = wInc; thisSquare.height=hInc;
  thisSquare.x = thisSquare.y = 0;
  for (i=0; i<rows-hInc; i+=1) {// height
    for (j=0; j<cols-wInc; j+=1) {//width
      cvSetImageROI(img,thisSquare);
      cvSetImageROI(spc,thisSquare);
      // calculate avg and std of feature set
      cvAvgSdv(img, &mC, &sC, 0);
      cvAvgSdv(spc, &mS, &sS, 0);
      for (k=0; k<SSCALE/2; k++) 
	    for (l=0; l<SSCALE/2; l++) {
		cvSet2D(fColorA,i*SSCALE/2+k,j*SSCALE/2+l,mC);
		cvSet2D(fColorS,i*SSCALE/2+k,j*SSCALE/2+l,sC); 
		cvSet2D(fSpaceA,i*SSCALE/2+k,j*SSCALE/2+l,mS);
		cvSet2D(fSpaceS,i*SSCALE/2+k,j*SSCALE/2+l,sS); 
		}
	 thisSquare.x += wInc; 
	 if (thisSquare.x+thisSquare.width >= w) 
	 thisSquare.width = w - thisSquare.x-1;// do border
	}
    thisSquare.x=0; thisSquare.width = wInc;
    thisSquare.y += hInc;
    if (thisSquare.y+thisSquare.height >= h) 
      thisSquare.height = h-thisSquare.y-1; // do border
  }
  cvResetImageROI(img); 
  cvResetImageROI(spc); 
  // global stats for space and color images
  cvAvgSdv(img, &avgC, &stdC, 0);
  cvAvgSdv(spc, &avgS, &stdS, 0);
  printf("Space-z: Avg=%lf, sd=%lf\n",avgS.val[2],stdS.val[2]);
  printf("Color-AB/2: Avg=%lf, sd=%lf\n",(avgC.val[1]+avgC.val[2])/2,
	 (stdC.val[1]+stdC.val[2])/2 );

  printf("Saliency Squares done.");
  return;
}



bool myScoreCompare(MyScoreType a, MyScoreType b) {
	return (a.score > b.score);
}


/* looks at the regions of saliency and tries to rank them */

CvRect salienceChecker::saliencyComponents(IplImage *src, 
					   double pan, double tilt, ArRobot *robot){
 
 
  CvMemStorage* storage = cvCreateMemStorage(0);
  CvSeq* contour  = 0;
  CvSeq* pContour = 0;

  
  cvThreshold(src,srcBW,int(0.8*255),255,CV_THRESH_BINARY); // pick 80% salient
  //cvErode(srcBW,srcBW); cvErode(srcBW,srcBW); // clean up specs
  //cvShowImage("mapThresh",srcBW);

	
  cvFindContours( srcBW, storage, &contour, sizeof(CvContour), 
		  CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );

  cvZero( dst );

  int i, row=src->width/SSCALE, col=src->height/SSCALE, 
    imageArea = row*col, imageHeight = col;

  double sizeThreshold = 0.01 * double(imageArea), // ratio of image size as size threshold
    upperSizeThreshold = 0.8 * double(imageArea);

  double aspRatio[MAX_CANDIDATES], area[MAX_CANDIDATES], avgVal[MAX_CANDIDATES];
	
  struct MyScoreType myScore[MAX_CANDIDATES];

  CvSeq *pcCont[MAX_CANDIDATES]; CvRect cBox[MAX_CANDIDATES];
  int numCandidates;

  printf("CC: Area Size threshold is %lf\n",sizeThreshold);

  pContour = contour;

  for(numCandidates=0; pContour != 0 && numCandidates<MAX_CANDIDATES; 
      pContour=pContour->h_next ) {
    area[numCandidates] =  fabs(cvContourArea(pContour));
    cBox[numCandidates] = cvBoundingRect(pContour,1);
    
    if (area[numCandidates] > sizeThreshold && 
	area[numCandidates] < upperSizeThreshold //&&
	//cBox[numCandidates].x+cBox[numCandidates].width<col && 
	//cBox[numCandidates].y+cBox[numCandidates].height<row 
       )
      {
	pcCont[numCandidates]=pContour;
	aspRatio[numCandidates] = 
	  ((float)cBox[numCandidates].height)/
	  ((float)cBox[numCandidates].width);
	cvSetImageROI(src,cBox[numCandidates]);
	CvScalar s = cvAvg(src);
	avgVal[numCandidates] = s.val[0];///area[numCandidates];
	myScore[numCandidates].score = 
	  (0.4 * area[numCandidates]/imageArea + 
	   0.4 * avgVal[numCandidates]/255 + 
	   0.2 * aspRatio[numCandidates]/imageHeight );

	//(0.1 * area[numCandidates]/imageArea + 
	//0.2 * avgVal[numCandidates]/255 + 
	//0.7 * aspRatio[numCandidates]/imageHeight );
	printf("Raw candidate %d: score %lf\n",numCandidates,
	       myScore[numCandidates].score);
	myScore[numCandidates].index = numCandidates;
	cvResetImageROI(src);
	if (aspRatio[numCandidates]>0.6 ) 
	  numCandidates++;
      }
  }

  vector<MyScoreType> mySortedScore (myScore, myScore+numCandidates);

  sort (mySortedScore.begin(), mySortedScore.begin()+ numCandidates, 
	myScoreCompare );

  CvScalar colorL[4] = {CV_RGB(255,0,0),CV_RGB(255,255,0),
			CV_RGB(255,255,255),CV_RGB(150,150,150)};

  pContour = contour;

  for(i=0; pContour != 0 && i<numCandidates; pContour = pContour->h_next,i++ )    { 
    int j = mySortedScore[i].index;
    CvScalar color ;//= CV_RGB( rand()&255, rand()&255, rand()&255 );
    CvRect box = cBox[j];
    if (i<4) color =colorL[i]; else color =colorL[3];
    /* replace CV_FILLED with 1 to see the outlines */
    cvDrawContours( dst, pcCont[j], color, color, -1, CV_FILLED, 8 );
    cvRectangle( dst, cvPoint(box.x,box.y),cvPoint((box.x+box.width),
						   (box.y+box.height)),
		 CV_RGB(int(255),100,100), 2 );
    printf("CC: Region (%d)%d; S(a%lf, v%lf, ar%lf)=%lf\n",
	   i,j,area[j], avgVal[j], aspRatio[j],mySortedScore[i].score);	
  }
  printf("There are a total of %d components above min size & shape.\n",
	 numCandidates);
 
  CvRect tB;

  /* Put the top three candidates in the saliency list */
  printf("Salience Promotions.\n");
  int candidate; float r;
  double from[3],to[3];

  for (i=0 ;i<3 && i<numCandidates; i++) {
    candidate = mySortedScore[i].index;
    tB.x = SSCALE*cBox[candidate].x; // map image cordinate in imgSpace image
    tB.y = SSCALE*cBox[candidate].y;
    tB.width = SSCALE*(cBox[candidate].width);
    tB.height = SSCALE*(cBox[candidate].height);
    cvSetImageROI(imgSpace,tB);
    avgS=cvAvg(imgSpace); // calculate average x,y,z
    cvResetImageROI(imgSpace);
    //    printf("%d. tB[%d,%d; %d,%d] \n",
    //   i,tB.x,tB.y,tB.width,tB.height);
    if (INRANGE(320,240,tB.x,tB.y) && INRANGE(320,240,tB.x+tB.width,tB.y+tB.height)) {

      CvScalar top = cvGet2D(imgSpace,tB.y,tB.x);
      CvScalar bot = cvGet2D(imgSpace,tB.y+tB.height,tB.x+tB.width);
      r = 0.5*1000.0*(MDIST2( (top.val[0]-bot.val[0]), (top.val[1]-bot.val[1])));
			

      from[0]=avgS.val[0]; from[1]=avgS.val[1]; from[2]=avgS.val[2];
      transformBumbleBee2World(from,to,pan,tilt,
			       robot->getX(), robot->getY(), robot->getTh());

      addSalList(to[0], to[1], to[2], r, mySortedScore[i].score);
      printf("%d. tB[%d,%d; %d,%d] = (%5.1lf, %5.1lf, %5.1lf ; %5.1lf, %lf).\n", i,
	     tB.x,tB.y,tB.width,tB.height,to[0], to[1],to[2],r, mySortedScore[i].score);
    } else 
      printf("%d. tB[%d,%d; %d,%d] = (%5.1lf, %5.1lf, %5.1lf ; %5.1lf, %lf). tB out of range!.\n", i,
	     tB.x,tB.y,tB.width,tB.height,to[0], to[1],to[2],0.0, mySortedScore[i].score);

  }

  if (numCandidates==1)
    tB = cBox[mySortedScore[0].index];
  else if (numCandidates==2) 
    tB = cvMaxRect(&cBox[mySortedScore[0].index],&cBox[mySortedScore[1].index]);
  else if (numCandidates>2) {
    tB = cvMaxRect(&cBox[mySortedScore[0].index],&cBox[mySortedScore[1].index]); 
    tB = cvMaxRect(&tB,&cBox[mySortedScore[2].index]); 
  }

  cvRectangle( dst, cvPoint(tB.x,tB.y),cvPoint((tB.x+tB.width),
					       (tB.y+tB.height)),
	       CV_RGB(int(0),255,0), 2 );

  cvRectangle( imgBGR, cvPoint(SSCALE*tB.x,SSCALE*tB.y),
	       cvPoint(SSCALE*(tB.x+tB.width),SSCALE*(tB.y+tB.height)),
	       CV_RGB(int(0),255,0), 2 );

  //cvShowImage( "mapRegions", dst ); // save this image to see salient regions
  //cvWaitKey(10);

  return tB;
}

void salienceChecker::saveSalienceImage(char* filename) {
  cvSaveImage(filename,dst);
}

/* allocates  the images used in the architecture */

void salienceChecker::initSaliencySquares(int row, int col) {

 if (g_SaliencyInit) return;
 g_SaliencyInit=true;
 // row = 120; col=160; // fix this as a step towards multiscale operation, dml

 printf("\ninitializing the 6 saliency mappings, Map size %d %d\n",row,col);

 fColorAvgMap =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,3);
 fColorSdvMap =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,3);

 if (fColorAvgMap!=0 && fColorSdvMap!=0) printf("Arch 1 images allocated.\n");
 else printf("image allocation failed.\n");

 cvZero(fColorAvgMap);
 cvZero(fColorSdvMap);

 fSpaceAvgMap =  cvCreateImage(cvSize(col,row),IPL_DEPTH_64F,3);
 fSpaceSdvMap =  cvCreateImage(cvSize(col,row),IPL_DEPTH_64F,3);

 if (fSpaceAvgMap!=0 && fSpaceSdvMap!=0) printf("Arch 2 images allocated.\n");
 else printf("image allocation failed.\n");

 cvZero(fSpaceAvgMap);
 cvZero(fSpaceSdvMap);

 fColorMap =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,1);
 fSpaceMap =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,1);

 if (fColorMap!=0 && fColorMap!=0) printf("Arch 3 images allocated.\n");
 else printf("image allocation failed.\n");

  cvZero(fColorMap);
  cvZero(fSpaceMap);

 fTempMap =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,3);
 fTempMapBW1 =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,1);
 fTempMapBW2 =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,1);

 if (fTempMap!=0 && fTempMapBW1!=0&&fTempMapBW2!=0) printf("Arch 4 images allocated.\n");
 else printf("image allocation failed.\n");

 cvZero(fTempMap);
 cvZero(fTempMapBW1);
 cvZero(fTempMapBW2);

 fConspMapBW = cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,1);

 fColorAvgMapBW =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,1);
 fColorSdvMapBW =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,1);

 if (fConspMapBW!=0 && fColorAvgMapBW!=0&&fColorSdvMapBW!=0) printf("Arch 5 images allocated.\n");
 else printf("image allocation failed.\n");

 cvZero(fConspMapBW);
 cvZero(fColorAvgMapBW);
 cvZero(fColorSdvMapBW);


 fSpaceAvgMapBW =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,1);
 fSpaceSdvMapBW =  cvCreateImage(cvSize(col,row),IPL_DEPTH_8U,1);

 if (fSpaceAvgMapBW!=0 && fSpaceSdvMapBW!=0) printf("Arch 6 images allocated.\n");
 else printf("image allocation failed.\n");

 cvZero(fSpaceAvgMapBW);
 cvZero(fSpaceSdvMapBW);

  return;
}




CvRect salienceChecker::saliencyArchitecture(double pan, double tilt, ArRobot *r){
  int iSize = SAL_ROWS*SAL_COLS;

  if (!g_SaliencyInit) initSaliencySquares(SAL_ROWS, SAL_COLS);

  saliencySquares(SAL_ROWS,SAL_COLS,  imgLab, imgSpace, 
		  fColorAvgMap, fColorSdvMap,fSpaceAvgMap, fSpaceSdvMap);

  /*------- COLOR ---------*/


  //cvAndS(fColorSdvMap,cvScalar(0,255,255),fColorSdvMap); //only a and b not L
  cvCvtScale(fColorSdvMap,fColorSdvMap,70,0); // scale up small sdv
  cvCvtColor(fColorSdvMap,fColorSdvMapBW,CV_RGB2GRAY);
  cvSubRS(fColorSdvMapBW,cvScalar(255),fColorSdvMapBW); // invert, favor low sdv
  cvShowImage( "mapCS", fColorSdvMapBW );


  //cvAndS(fColorAvgMap,cvScalar(0,255,255),fColorAvgMap); // only a and b not L
  cvCvtColor(fColorAvgMap,fColorAvgMapBW,CV_RGB2GRAY); 

  cvAndS(fColorAvgMap,cvScalar(0,255,0),fColorAvgMap); //add more of channel
  cvScale(fColorAvgMap,fColorAvgMap,0.5);
  cvCvtColor(fColorAvgMap,fTempMapBW1,CV_RGB2GRAY);
  cvAdd(fTempMapBW1,fColorAvgMapBW, fColorAvgMapBW);

  //cvShowImage( "mapCA", fColorAvgMapBW );



  //  //cvAndS(fColorSdvMap,cvScalar(0,255,255),fColorSdvMap); //only a and b not L
  //cvCvtScale(fColorSdvMap,fColorSdvMap,70,0); // scale up small sdv
  //cvCvtColor(fColorSdvMap,fColorSdvMapBW,CV_RGB2GRAY);
  //cvSubRS(fColorSdvMapBW,cvScalar(255),fColorSdvMapBW); // invert, favor low sdv
  //cvShowImage( "mapCS", fColorSdvMapBW );


  //cvAndS(fColorAvgMap,cvScalar(0,255,255),fColorAvgMap); // only a and b not L
  //cvCvtColor(fColorAvgMap,fColorAvgMapBW,CV_RGB2GRAY); 
  //cvShowImage( "mapCA", fColorAvgMapBW );
  
#if 0
  int cThresh = int(0.925*(avgC.val[1]+avgC.val[2])/2.0); // av color saliency
  cvThreshold(fColorAvgMapBW,fColorAvgMapBW,cThresh,0,CV_THRESH_TOZERO); 
   // low mean color set to 0
#else
  avgC.val[0]
        =0.825*(avgC.val[0]+avgC.val[1]+avgC.val[2])/3.0; // av color saliency 0.825
  //=0.825*(avgC.val[1]+avgC.val[2])/2.0; // av color saliency
  cvSubRS(fColorAvgMapBW,avgC,fTempMapBW1); // mean-I
  cvCvtScale(fTempMapBW1,fTempMapBW1,5,0);
  cvShowImage( "mapCA1", fTempMapBW1 );

  avgC.val[0]
    //=1.1*(avgC.val[0]+avgC.val[1]+avgC.val[2])/3.0; // av color saliency
      =0.9*(avgC.val[1]+avgC.val[2])/2.0; // av color saliency
  cvSubS(fColorAvgMapBW,avgC,fColorAvgMapBW); // I-mean
  cvShowImage( "mapCA", fColorAvgMapBW );

  cvAdd(fColorAvgMapBW, fTempMapBW1, fColorAvgMapBW); //|mean-I|

  int cThresh = int(cvAvg(fColorAvgMapBW).val[0]);
  cvThreshold(fColorAvgMapBW,fColorAvgMapBW,cThresh,0,CV_THRESH_TOZERO); 
   // low mean color set to 0
  //cvShowImage( "mapCA", fColorAvgMapBW );
#endif
  
  cvMul(fColorSdvMapBW,fColorAvgMapBW,fColorMap,1.0); // element wise multiply
  cvShowImage( "mapMulC", fColorMap ); 

  /*----- SPACE ------*/

  cvCvtScale(fSpaceSdvMap,fTempMap,500,0);
  cvAndS(fTempMap,cvScalar(0,0,255),fTempMap); // only Z
  cvCvtColor(fTempMap,fSpaceSdvMapBW,CV_BGR2GRAY);

  cvThreshold(fSpaceSdvMapBW,fTempMapBW1,1,255,CV_THRESH_BINARY); 
  // make a >0 mask
  cvSubRS(fTempMapBW1,cvScalar(255),fTempMapBW1); // invert distances
  //cvXorS(fTempMapBW1, cvScalar(255), fTempMapBW1); // make a <=0 mask
  cvOrS(fSpaceSdvMapBW,cvScalar(255),fSpaceSdvMapBW, fTempMapBW1); 
  // zap all the low values, set to 255
  cvThreshold(fSpaceSdvMapBW,fSpaceSdvMapBW,254,0,CV_THRESH_TOZERO_INV); 
  // kill 255 (=0 depth)

  cvThreshold(fSpaceSdvMapBW,fTempMapBW1,25,100,CV_THRESH_BINARY_INV); 
  // for display
  cvThreshold(fSpaceSdvMapBW,fSpaceSdvMapBW,25,1.0,CV_THRESH_BINARY_INV); 
   // low SD only set to 1
  cvShowImage( "mapSS", fTempMapBW1 );


  cvCvtScale(fSpaceAvgMap,fTempMap,50,0); // 50*5m = 250, 50*0.25=12.5
  cvAndS(fTempMap,cvScalar(0,0,255),fTempMap); // only Z
  cvCvtColor(fTempMap,fSpaceAvgMapBW,CV_BGR2GRAY);// gray level depth

  cvThreshold(fTempMap,fTempMap,13,255,CV_THRESH_BINARY_INV); // 0-mask 
  cvAndS(fTempMap,cvScalar(0,0,255),fTempMap); // only Z
  cvCvtColor(fTempMap,fTempMapBW1,CV_BGR2GRAY); // 1-channel 0-mask
  cvThreshold(fTempMapBW1,fTempMapBW1,0,1,CV_THRESH_BINARY); // binary 0-mask
  CvScalar  zSum = cvSum(fTempMapBW1);
  //printf("\nNumber of zeros = %lf\n",zSum.val[0]);
  cvSet(fSpaceAvgMapBW,cvScalar(255), fTempMapBW1); // set 0 to 255

  cvSubRS(fSpaceAvgMapBW,cvScalar(255),fSpaceAvgMapBW); // invert distances
  avgS = cvAvg(fSpaceAvgMapBW);
  // printf("Uncrrected Average Distance saliency threshold %lf\n",avgS.val[0]);
  int threshS = int(avgS.val[0]*iSize / float(iSize - zSum.val[0])); 
  printf("Average Distance saliency threshold %d\n",threshS);
  cvThreshold(fSpaceAvgMapBW,fSpaceAvgMapBW,threshS,
        0, CV_THRESH_TOZERO); // threshold at average scene depth
  cvShowImage( "mapSA", fSpaceAvgMapBW ); 

  cvMul(fSpaceSdvMapBW,fSpaceAvgMapBW,fSpaceMap,1.0); // element wise multiply 

  cvShowImage( "mapMulS", fSpaceMap );  
    
	
  /*-----------*/


  cvAddWeighted(fSpaceMap,0.7, fColorMap,
		0.7,0.0,fConspMapBW);

  cvShowImage( "mapConsp", fConspMapBW );

  return saliencyComponents(fConspMapBW,pan,tilt, r);
}

/*********---------------------------------*********/

void salienceChecker::addSalList(float x1, float y1, float z1, float r1, double s1) {

  if ( salListIndex==MAX_SALIENCES ) {
    printf("Salience Candidate List full\n");
    return;
  }
  SalCandidate *p = &salList[salListIndex++];
  p->x = x1; p->y = y1; p->z = z1; p->r = r1; p->score = s1;
  
  return;
}

void salienceChecker::printSalList() {

  printf("Salience List.\n");
  for (int i=0; i<salListIndex; i++) {
      SalCandidate *p = &salList[i];
      printf("%d.\t%4.1f %4.1f %4.1f %4.1f\t%lf\n",i,p->x, p->y, p->z, p->r, p->score);
  }
  printf("-----------------\n");
}

#define SALCCTHRESHOLD 0.5

/* calculate score weighted centroid of merged best targets*/
void salienceChecker::mergedBest(float *x, float *y, float *z, float *r) {
  int bI = bestSalList(x,y,z); // single best entry
  SalCandidate *q = &salList[bI];
  float sX=0, sY=0, sZ=0, sR=0, sS=0;

  for (int i=0; i<salListIndex-1; i++) {
       SalCandidate *p = &salList[i];
       if (MDIST3( (p->x-q->x), (p->y-q->y), (p->z-q->z) ) < SALCCTHRESHOLD*(p->r+q->r)) {
	 sS+=p->score; sR+=p->score*p->r; sX+=p->score*p->x; sY+=p->score*p->y; sZ+=p->score*p->z;
       }
  }
  *x = sX/sS; *y = sY/sS; *z = sZ/sS; *r = sR/sS;
}


void salienceChecker::crossCorrelateSalList() {
  for (int i=0; i<salListIndex-1; i++)
    for (int j=i+1; j<salListIndex; j++) {
       SalCandidate *p = &salList[i];
       SalCandidate *q = &salList[j];
       if (MDIST3( (p->x-q->x), (p->y-q->y), (p->z-q->z) ) < 2*(p->r+q->r)) {
	 p->score *=2; q->score *=2;
       }
    }
}

int salienceChecker::bestSalList(float *x, float *y, float *z)  {
  double bestScore=-1; int bestIndex=-1;
  for (int i=0; i<salListIndex; i++) 
    if (salList[i].score>bestScore) {
      bestScore=salList[i].score; bestIndex=i;
    }
  *x = salList[bestIndex].x; *y = salList[bestIndex].y; *z = salList[bestIndex].z;
  if (bestScore<0.39) bestIndex=-1;
  return bestIndex;
}

void salienceChecker::resetSalList() {
  printf("resetting salience list.\n");
  salListIndex=0;
}