//=============================================================================
#include "featureFinder.h"

#include "bwImage.h"
#include "rgbImage.h"
#include "floatImage.h"
#include "sortedList.h"
#include <math.h>

#define HARRIS_CORNER_WINDOW_SIZE 10

featureFinder::featureFinder( rgbImage *input )
{
  cornerResp_ = input->harrisCornerDetect( HARRIS_CORNER_WINDOW_SIZE );
  // Raise the corner response to the power of 3 to emphasize corners
  for ( int y=cornerResp_->getHeight()-1; y>=0; y-- )
    for ( int x=cornerResp_->getWidth()-1; x>=0; x-- )
      cornerResp_->pixel(x,y) =
	cornerResp_->pixel(x,y)*cornerResp_->pixel(x,y)*cornerResp_->pixel(x,y);
}

featureFinder::featureFinder( bwImage *input )
{
  cornerResp_ = input->harrisCornerDetect( HARRIS_CORNER_WINDOW_SIZE );
  // Raise the corner response to the power of 3 to emphasize corners
  for ( int y=cornerResp_->getHeight()-1; y>=0; y-- )
    for ( int x=cornerResp_->getWidth()-1; x>=0; x-- )
      cornerResp_->pixel(x,y) =
	cornerResp_->pixel(x,y)*cornerResp_->pixel(x,y)*cornerResp_->pixel(x,y);
}


featureFinder::~featureFinder()
{
  delete cornerResp_;
}

inline void 
featureFinder::findCentroid( floatImage *intensMap,
			     int x1, int y1, int x2, int y2,
			     int &centX, int &centY )
{
  intensMap->clampInImage(x1,y1);
  intensMap->clampInImage(x2,y2);
  
  float weightSumX = 0.0f;
  float weightSumY = 0.0f;
  float sum = 0.0f;
  
  for( int y=y1; y<=y2; y++ )
  {
    for ( int x=x1; x<=x2; x++ )
    {
      weightSumX += intensMap->pixel(x,y) * x;
      weightSumY += intensMap->pixel(x,y) * y;
      sum += intensMap->pixel(x,y);
    }
  }
  
  centX = rint(weightSumX / sum);
  centY = rint(weightSumY / sum);
}

inline void 
featureFinder::findStandardDeviation( floatImage *intensMap,
				      int x1, int y1, int x2, int y2,
				      int centX, int centY,
				      float &devX, float &devY )
{
  intensMap->clampInImage(x1,y1);
  intensMap->clampInImage(x2,y2);
  
  float weightSumX = 0.0f;
  float weightSumY = 0.0f;
  float sum = 0.0f;
  
  for( int y=y1; y<=y2; y++ )
  {
    for ( int x=x1; x<=x2; x++ )
    {
      weightSumX += intensMap->pixel(x,y) * (x-centX)*(x-centX);
      weightSumY += intensMap->pixel(x,y) * (y-centY)*(y-centY);
      sum += intensMap->pixel(x,y);
    }
  }

  devX = sqrtf( weightSumX/sum );
  devY = sqrtf( weightSumY/sum );
}

void 
featureFinder::findAFeature( floatImage *weightMap, int &locX, int &locY,
			     int &sizeX, int &sizeY, float &featLevel )
{
  // The window size to standard deviation ratio (ie 4.0 means we're
  // looking for a window which includes 95% of the intensity)
#define SD_WINDOW_RATIO 4.4f
  // Starting window size is the same as the Harris Corner detector resolution
  sizeX=HARRIS_CORNER_WINDOW_SIZE/2;
  sizeY=HARRIS_CORNER_WINDOW_SIZE/2;
  int currX, currY, currSizeX, currSizeY, oldCurrX, oldCurrY;
  weightMap->findMaxComponent( locX, locY );
  featLevel = weightMap->pixel( locX, locY );
  currX = locX;
  currY = locY;
  float devX, devY;
  int maxIters=100;

  do {
    oldCurrX = currX;
    oldCurrY = currY;
    currX = locX;
    currY = locY;
    currSizeX = sizeX;
    currSizeY = sizeY;
    findCentroid( weightMap,
		  currX-currSizeX/2, currY-currSizeY/2,
		  currX+currSizeX/2, currY+currSizeY/2,
		  locX, locY );
    findStandardDeviation( weightMap,
			   currX-currSizeX/2, currY-currSizeY/2,
			   currX+currSizeX/2, currY+currSizeY/2,
			   locX, locY, devX, devY );
    
    if ( ( abs(locX-oldCurrX) > currSizeX/6 ) ||
	 ( abs(locY-oldCurrY) > currSizeY/6 ) )
    { // This means that we've grown the window too much and started to
      // include a seperate "hotspot"
      locX = currX;
      locY = currY;
      //printf("frame %i: Detected close hotspot\n",frameNum);
      break;
    }

    if ( sizeX<devX*SD_WINDOW_RATIO )
    {
      sizeX += (devX*SD_WINDOW_RATIO-(float)sizeX)/2.0f;
      if ( sizeX==currSizeX ) sizeX++;
    }
    if ( sizeY<devY*SD_WINDOW_RATIO )
    {
      sizeY += (devY*SD_WINDOW_RATIO-(float)sizeY)/2.0f;
      if ( sizeY==currSizeY ) sizeY++;
    }
  } while ( ( (currX!=locX) || (currY!=locY) ||
	      (currSizeX!=sizeX) || (currSizeY!=sizeY) ) &&
	    ( maxIters-- >= 0 ) );
}

sortedSubWindowList
featureFinder::findFeatures( int numFeats )
{
  sortedSubWindowList featList;
  
  int featX, featY, featSizeX, featSizeY;
  float brightestFeatLevel, featLevel;
  
  findAFeature( cornerResp_, featX, featY, featSizeX, featSizeY,
		brightestFeatLevel );
  featLevel = brightestFeatLevel;
  brightestFeatLevel = cbrt(brightestFeatLevel);

  while (numFeats-->0) {
    featLevel = cbrt(featLevel);

    featList.addSubWindow( featLevel,
			   subWindow(featX,featY,featSizeX,featSizeY) );
    
    cornerResp_->drawSolidSquare(featX-featSizeX/2, featY-featSizeY/2,
				 featX+featSizeX/2, featY+featSizeY/2,
				 0,true);
    findAFeature( cornerResp_, featX, featY, featSizeX, featSizeY,
		  featLevel );
  }

  featList.scaleAllKeys( brightestFeatLevel, cbrt(featLevel) );
  
  return featList;
}