#include "gauss.h"
#include "linalg.h"
#include "pointerRecognition.hh"

using namespace boost;
using namespace mimas;
using namespace std;

#define GAUSSERROR (32.0f / 256.0f)

PointerRecognition::PointerRecognition
  ( CameraProjectorCalibrationPtr _cameraProjectorCalibration,
    const image< rgba< unsigned char > > &_refImg ):
    cameraProjectorCalibration( _cameraProjectorCalibration ),
    x(0), y(0), w(320), h(200), numPixels(0), threshold( 0.025 ),
    haveOldPos(false), minX(0), minY(0), maxX(0), maxY(0),
    trackingRange(0)
{
  setReferenceImage( _refImg );
  setSigma( 3.0 );
}

void PointerRecognition::setReferenceImage
   ( const image< rgba< unsigned char > > &_refImg )
{
  // Resize to zero (otherwise old content is preserved).
  histogram.resize( extents[ 0 ][ 0 ][ 0 ] );
  histogram.resize( extents[ 8 ][ 8 ][ 8 ] );
  numPixels = _refImg.getWidth() * _refImg.getHeight();
  for ( const rgba< unsigned char > *p = _refImg.rawData();
        p != _refImg.rawData() + numPixels; p++ )
    histogram[ (int)p->getRed()   / 32 ]
             [ (int)p->getGreen() / 32 ]
             [ (int)p->getBlue()  / 32 ]++;
}

bool PointerRecognition::findPointer
   ( const image< rgba< unsigned char > > &_frame,
     Vector &camPos, Vector &pos )
  throw (mimasexception)
{
  MMERROR( histogram.num_elements() > 0, mimasexception, ,
           "Reference image was not initialised." );

  int minLevel = (int)( numPixels * threshold );

  minX = 0;
  minY = 0;
  maxX = _frame.getWidth() - 1;
  maxY = _frame.getHeight() - 1;

  if ( haveOldPos ) {
    if ( minX < oldX + deltaX - trackingRange )
      minX = oldX + deltaX - trackingRange;
    if ( maxX > oldX + deltaX + trackingRange )
      maxX = oldX + deltaX + trackingRange;
    if ( minY < oldY + deltaY - trackingRange )
      minY = oldY + deltaY - trackingRange;
    if ( maxY > oldY + deltaY + trackingRange )
      maxY = oldY + deltaY + trackingRange;
  };

  if ( maxX - minX <= minSize ) {
    if ( minX == 0 )
      maxX = minSize;
    else
      minX = maxX - minSize;
  };
  if ( maxY - minY <= minSize ) {
    if ( minY == 0 )
      maxY = minSize;
    else
      minY = maxY - minSize;
  };

  image< float > binary; binary.init( maxX + 1 - minX, maxY + 1 - minY );
  {
    const rgba< unsigned char > *p = &_frame.pixel( minX, minY );
    float *q = binary.rawData();
    for ( int j=minY; j<=maxY; j++ ) {
      for ( int i=minX; i<=maxX; i++ ) {
        if ( histogram[ (int)p->getRed()   / 32 ]
                      [ (int)p->getGreen() / 32 ]
                      [ (int)p->getBlue()  / 32 ] >= minLevel )
          *q = 255.0;
        else
          *q = 0.0;
        p++;
        q++;
      };
      p += _frame.getWidth() - ( maxX + 1 - minX );
    };
  };

  segmentedImage = gaussBlur< float >( binary, (float)sigma, GAUSSERROR );
  bool retVal = false;
  {
    unsigned char *p = segmentedImage.rawData();
    for ( int j=0; j<segmentedImage.getHeight(); j++ ){
      for ( int i=0; i<segmentedImage.getWidth(); i++ ){
        if ( *p++ >= 64 ) {
          camPos[0] = minX + i;
          camPos[1] = minY + j;
          camPos[2] = 1.0;
          pos = prod( inv( cameraProjectorCalibration->getHomography() ),
                      camPos );
          pos[0] /= pos[2];
          pos[1] /= pos[2];
          pos[2] = 1.0;
          if ( pos[0] >= x && pos[1] >= y &&
               pos[0] < x + w && pos[1] < y + h ) {
            // cerr << camPos[0] << ", " << camPos[1] << " -> " << endl;
            // cerr << pos[0] << ", " << pos[1] << endl;
            if ( haveOldPos ) {
              deltaX = minX + i - oldX;
              deltaY = minY + i - oldY;
            } else {
              deltaX = 0;
              deltaY = 0;
            };
            retVal = true;
            oldX = minX + i;
            oldY = minY + j;
            i=segmentedImage.getWidth(); j=segmentedImage.getHeight();
          };
        }
      }
    }
  };

  haveOldPos = retVal;
  return haveOldPos;
}


void PointerRecognition::setSigma( double _sigma )
{
  sigma = _sigma;
  minSize = gaussBlurFilter< float >( _sigma, GAUSSERROR ).size();
}