//------------------------------------------------------------------------------
#include "movieIO.h"
#include "bwImage.h"
#include "rgbImage.h"
#include "hlsImage.h"
#include "floatImage.h"
#include "sortedList.h"
#include "featureFinder.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

void forEachFrame( movieIO *movie, int frameNum, rgbImage *rgbBuf, bwImage *bwBuf );

int main( int argc, char *argv[] )
{
  setvbuf(stdout,0,_IONBF,0);
  movieIO movie;
  if ( argc<2 )
  {
    printf( "Usage: %s <moviefile>\n", argv[0] );
    exit(0);
  }
  
  int i;
  rgbImage *rgbBuf;
  bwImage  *bwBuf;
  
  printf( "Preparing \"%s\"...\n", argv[1] );
  if ( !movie.open( argv[1] ) )
  {
    printf( "Error in preparing %s\n", argv[1] );
    exit(0);
  }
  printf("0%%%*c100%%\n",movie.getLength()-6,' ');
  for ( i=0; i<movie.getLength(); i++ )
  {
    printf("|");
    bwBuf  = movie.getBWFrame(i);
    rgbBuf = movie.getRGBFrame(i);

    forEachFrame(&movie,i,rgbBuf,bwBuf);
    
    movie.releaseBWFrame(bwBuf);
    movie.releaseRGBFrame(rgbBuf);
  }
  printf("\nSaving output to \"%s\"\n", "track.qt" );
  movie.setRate(6);
  movie.save( "track.qt" );
  movie.play();
  movie.close();
}

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

void 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 findAFeatureOld( floatImage *weightMap, int &locX, int &locY,
		   int &sizeX, int &sizeY,
		   int maxFeatX, int maxFeatY, int maxFeatArea, int frameNum )
{
  sizeX=maxFeatX/3;
  sizeY=maxFeatY/3;
  weightMap->findMaxComponent( locX, locY );
  float devX, devY;
  int maxSDIters=30;
  int maxIters=100;
  float winSizeToSD=9.0f;
  int currX, currY, currSizeX, currSizeY;
  
  do {
    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 );
    sizeX = rint(devX*winSizeToSD);
    sizeY = rint(devY*winSizeToSD);
    if ( (sizeX>maxFeatX) ||
	 (sizeY>maxFeatY) ||
	 (sizeX*sizeY>maxFeatArea) )
    {
      if ( maxIters--<0 ) break;
      sizeX=maxFeatX/3;
      sizeY=maxFeatY/3;
      weightMap->findMaxComponent( locX, locY );
      maxSDIters=20;
      winSizeToSD = sqrtf( winSizeToSD*winSizeToSD*0.98f );
      printf("frame %i: reducing SD ratio to %.2f\n",frameNum,winSizeToSD);
      continue;
    }
    if ( maxSDIters--<0 ) break;
  } while ( (currX!=locX) || (currY!=locY) ||
	    (currSizeX!=sizeX) || (currSizeY!=sizeY) );

  if (maxIters<0)
    printf("frame %i: not enough iters\n",frameNum);
}

void findAFeature( floatImage *weightMap, int &locX, int &locY,
		   int &sizeX, int &sizeY, float &featLevel, int frameNum )
{
  // 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
  sizeX=5;
  sizeY=5;
  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 ) );
}

void forEachFrame( movieIO *movie, int frameNum,
		   rgbImage *rgbBuf, bwImage *bwBuf )
{
  featureFinder finder( rgbBuf );
  sortedList *featList = finder.findFeatures(50);
  int *currFeatInfo;
  int featX, featY, halfFeatSizeX, halfFeatSizeY;
  for ( int i=0; i<30; i++ )
  {
    currFeatInfo = (int*)featList->getData(i);
    featX = currFeatInfo[0];
    featY = currFeatInfo[1];
    halfFeatSizeX = currFeatInfo[2]/2;
    halfFeatSizeY = currFeatInfo[3]/2;
    free( currFeatInfo );
    rgbBuf->addSquare( featX-halfFeatSizeX, featY-halfFeatSizeY,
		       featX+halfFeatSizeX, featY+halfFeatSizeY,
		       255,0,0,featList->getKey(i),true);
  }
  movie->setRGBFrame(frameNum,rgbBuf);
}

void forEachFrameOld( movieIO *movie, int frameNum,
		   rgbImage *rgbBuf, bwImage *bwBuf )
{
  floatImage *cornResp = rgbBuf->harrisCornerDetect( 7 );

  for ( int y=cornResp->getHeight()-1; y>=0; y-- )
    for ( int x=cornResp->getWidth()-1; x>=0; x-- )
      cornResp->pixel(x,y) =
	cornResp->pixel(x,y)*cornResp->pixel(x,y)*cornResp->pixel(x,y);

  int featX, featY, featSizeX, featSizeY;
  int numFeats=30;
  float brightestFeatLevel, featLevel;
  findAFeature( cornResp, featX, featY, featSizeX, featSizeY,
		brightestFeatLevel, frameNum );
  featLevel = brightestFeatLevel;
  brightestFeatLevel = cbrt(brightestFeatLevel);
  while (numFeats-->0) {
    featLevel = cbrt(featLevel);
    rgbBuf->addSquare(featX-featSizeX/2, featY-featSizeY/2,
		      featX+featSizeX/2, featY+featSizeY/2,
		      255,0,0,featLevel/brightestFeatLevel,true);
    cornResp->drawSolidSquare(featX-featSizeX/2, featY-featSizeY/2,
			      featX+featSizeX/2, featY+featSizeY/2,
			      0,true);
  findAFeature( cornResp, featX, featY, featSizeX, featSizeY,
		featLevel, frameNum );
  }
  movie->setRGBFrame(frameNum,rgbBuf);
  delete cornResp;
}