#include "featuresift.h"

featuresift::featuresift()
{
  clear();
}

featuresift::~featuresift()
{
}

int featuresift::clear()
{
  for(int i = 0; i < 128; i++ )
    feature[i] = nfeature[i] = 0;
  return 0;
}

int featuresift::init( image2d *mag,
                       image2d *ori, int lx,
                       int ly, int mysz )
{
  clear();

#ifdef __DEBUG__
  std::cout << "init feature: " << lx << " " << ly << " " << sz << "\n";
#endif
  sz = mysz;
  int windowlx, windowly;
  int windowsz;
  windowsz = sz/4;
  cx = lx;
  cy = ly;
  lx = lx - sz/2;
  ly = ly - sz/2;
  for( int i = 0; i < 4; i++ )    {
      for( int j = 0; j < 4; j++ )
        {
          windowlx = lx + windowsz*i;
          windowly = ly + windowsz*i;
          int z;
          z = calc_gradhist( feature+( (i*4+j)*8 ),
                             windowlx, windowly, sz,
                             mag, ori);
          if( z != 0 )
            {
#ifdef __DEBUG_SIFT__
              std::cout << "skipping\n";
#endif
              return -1;
            }
        }
    }
#ifdef __DEBUG_SIFT__
  std::cout << "good\n";
#endif
  normalize();
  return 0;
}

double featuresift::match( featuresift &sift )
{
  double d;
  d = 0;
  for( int i = 0; i < 128; i++ )
    d = d + pow( nfeature[i] - sift.nfeature[i], 2);
#ifdef __DEBUG_SIFT__
  std::cout << std::hex << this << " " << &sift << " " << std::dec << d << "\n";
  std::cout << "\t";
  for( int i = 0; i < 128; i++ )
    std::cout << nfeature[i] << " ";
  std::cout << "\n\t";
  for( int i = 0 ;i < 128; i++ )
    std::cout << sift.nfeature[i] << " ";
  std::cout << "\n";
#endif
  return d;
}

int featuresift::normalize()
{
  double avg, v, sd;
  avg = v = 0;
  for( int i = 0; i < 128; i++ )
    avg = avg + feature[i];
  avg = avg / 128.0;
  sd = 0;
  for( int i = 0; i < 128; i++ )
    sd = sd + pow( feature[i] - avg, 2 );
  sd = sd / 128.0;
  for( int i = 0; i < 128; i++ )
    {
      if( feature[i] < avg-sd )
        feature[i] = 0;
      else
        v = v + feature[i];
    }
  if( v == 0 )
    {
      std::cout << "ERROR vector normal = 0\n";
      exit(0);
    }
#ifdef __DEBUG_SIFT__
  std::cout << std::hex << this << " feature: ";
#endif
  for( int i = 0; i < 128; i++ )
    {
      nfeature[i] = feature[i]/v;
#ifdef __DEBUG_SIFT__
      std::cout << nfeature[i] << " ";
#endif
    }
#ifdef __DEBUG_SIFT__
  std::cout << "\n";
#endif
  return 0;
}

int featuresift::calc_gradhist( double *v,
                                int lx, int ly,
                                int sz, 
                                image2d *mag,
                                image2d *ori )
{
  for( int i = 0; i < 8; i++ )
    v[i] = 0;
  int ux, uy;
  ux = lx + sz;
  uy = ly + sz;
  double totalmag;
  totalmag = 0;
  if( lx < 0 || ly < 0 || ux > mag->getheight() || uy > mag->getwidth() )
    return -1;
  for( int i = lx; i < ux ; i++ )
    {
      for( int j = ly; j < uy; j++ )
        {
          double m, o;
          m = mag->getpixel( i, j );
          o = ori->getpixel( i, j );
          o = (o + PI)/(2*PI)*8.0;
          double ip, fp;
          fp = modf( o, &ip );
          int iip;
          iip = ((int)ip)+1;
          if( ip > 7.5 )
            {
              std::cout << "ERROR i part > 7\n";
              std::cout << ip << "\n";
              std::cout << o << "\n";
              exit(0);
            }
          if( ip < 0 )
            {
              std::cout << "ERROR i part < 0\n";
              exit(0);
            }
          if( fp < 0 )
            {
              std::cout << "ERROR f part < 0\n";
              exit(0);
            }
          totalmag = totalmag + m;
          v[ (int)ip ] = v[ (int)ip ] + m*(1-fp);
          v[ iip%8 ] = v[ iip %8] + m*(fp);
        }
    }
#ifdef __DEBUG_SIFT__
  std::cout << "total mag: " << totalmag << " " << sz <<"\n";
#endif
  return 0;
}

