#include "imagesift.h"

bool featurepair_score_cmp( imagesift::featurepair a, 
                            imagesift::featurepair b );

bool featurepair_score_cmp( imagesift::featurepair a, 
                            imagesift::featurepair b )
{
  return a.score < b.score;
}

imagesift::imagesift()
{
}

imagesift::~imagesift()
{
  clear();
}

int imagesift::init(image2d &im )
{
  scalespace ss;
  clear();
  ss.analyze( im, .005 );
  std::cout << "copying scale\n";
  ss.getscales( scales );
  std::cout << "getting features\n";
  std::vector< scalespace::feature > f;
  ss.getfeatures( f );
  std::cout << "calculating scales\n";
  ss.clear();
  scalemap.clear();
  Magick::Image ms;
  for( unsigned int i = 0; i < scales.size(); i++ )
    {
      double s;
      s = scales[i]->k;
      scalemap[ s ] = scales[i];
      if( orimap.count(s) > 0 )
        {
          std::cout << "dup scale\n";
          exit(0);
        }
      if( magmap.count(s) > 0 )
        {
          std::cout << "dup mag\n";
          exit(0);
        }
      std::cout << "scale " << i << " " << s << "\n";
      orimap[s] = new image2d;
      magmap[s] = new image2d;
      scales[i]->im->getimage( ms );
      char buffer[128];
      sprintf( buffer, "scale%i.png", i );
      ms.write( buffer );
      std::cout << "calculating\n";
      calc_magori( *(scales[i]->im), magmap[ scales[i]->k ],
                   orimap[ scales[i]->k ] );
      std::cout << "finished calculating\n";
    }
    
  for(unsigned i = 0; i < f.size(); i++ )
    {
      featuresift *fs;
      fs = new featuresift;
      int ret;
      ret = fs->init( magmap[ f[i].scale ],
                      orimap[ f[i].scale ],
                      f[i].x, f[i].y, 41*f[i].scale );
      if( ret == 0 )
        myfeatures.push_back( fs );
      else
        delete fs;
    }
  std::cout << "inserted " << myfeatures.size() << " features\n";
  selfsearch();
  return 0;
}

int imagesift::match( std::vector< featuresift* > &a,
                      std::vector< featuresift* > &b,
                      std::map< unsigned long int, std::vector< featurepair > > &ab )
{
  ab.clear();
  std::vector< featurepair > myf;
  //  std::map< unsigned long int, double > avgs, vars;
  for( unsigned int i = 0; i < a.size(); i++ )
    {
      myf.clear();
      unsigned long int aid;
      aid = (unsigned long int) a[i];
      featurepair fp;
      fp.a = a[i];
      double myavg, myvar;
      myavg = myvar = 0;
      for( unsigned int j = 0; j < b.size(); j++ )
        {
          fp.b = b[j];
          fp.score = a[i]->match( *(b[j]) );
          myf.push_back( fp );
          myavg = myavg + fp.score;
          
        }
      if( myf.size() > 0 )
        {
          myavg = myavg / ( (double) myf.size() );

          for( unsigned int j = 0; j < myf.size(); j++ )
            myvar = myvar + pow( myavg - myf[j].score, 2 );
          myvar = pow( myvar / ( (double) myf.size() ), 0.5 );
          
          std::sort( myf.begin(), myf.end(), featurepair_score_cmp );
          int c;
          c = 1;
          
          while( myf[c].score < myf[0].score+myvar && c < (int) myf.size()-1 )
            c++;
          
          myf.resize( c );
          
          ab[ aid ] = myf;
        }
    }
  std::cout << "matched: " << ab.size() << " features\n";
  return 0;
}

int imagesift::match( imagesift &o, 
                      std::list< featurepair > &fp,
                      Magick::Image ima1, 
                      Magick::Image imb1)
{
  // this image's features mapping to other image
  std::map< unsigned long int, std::vector<featurepair> > abmap;
  // other image's features mapping to this image
  std::map< unsigned long int, std::vector<featurepair> > bamap;

  match( myfeatures, o.myfeatures, abmap );
  match( o.myfeatures, myfeatures, bamap );

  for( std::map< unsigned long int, std::vector<featurepair> >::iterator it = abmap.begin();
       it != abmap.end(); it ++ )
    {
      unsigned long int bp;
      featurepair myp;
#ifdef __DEBUG_SIFT__
      std::cout << "processing: " << it->first << "\n";
#endif
      myp.a = it->second[0].a;
      for( unsigned i = 0; i < it->second.size(); i++ )
        {
          bp = (unsigned long int) it->second[i].b;
          int flag;
          flag = 0;
          unsigned int js;
          std::vector<featurepair> *cfpl;
          cfpl = &( bamap[bp] );
          js = cfpl->size();
          for( unsigned j = 0; j < js && flag == 0; j++ )
            {
              if( (*(cfpl))[i].b == myp.a )
                flag = 1;
            }
          if( flag == 1 )
            {
              myp.b = it->second[i].b;
              myp.score = it->second[i].score;
              fp.push_back( myp );
            }
        }
    }

  cluster_filter( fp );
  std::map< std::pair< int, int >, int > myts;
  Magick::Image ima, imb;
  ima = ima1;
  imb = imb1;
  ima.type(Magick::TrueColorType);
  imb.type(Magick::TrueColorType);
  Magick::ColorRGB rgb(1,0,0);
  rgb.alpha(1);

  ima.fillColor(rgb);
  imb.fillColor(rgb);
  rgb.green(1);
  rgb.red(0);
  rgb.blue(0);
  rgb.alpha(0);
  ima.strokeColor(rgb);
  ima.strokeWidth(2);
  imb.strokeColor(rgb);
  imb.strokeWidth(2);
  for( unsigned int i = 0; i < myfeatures.size(); i++ )
    {
      int x, y;
      x = myfeatures[i]->posx();
      y = myfeatures[i]->posy();
      ima.draw( Magick::DrawableCircle(y, x, y, x + 2 ) );
    }
  for( unsigned int i = 0; i < o.myfeatures.size(); i++ )
    {
      int x, y;
      x = o.myfeatures[i]->posx();
      y = o.myfeatures[i]->posy();
      imb.draw( Magick::DrawableCircle(y, x, y, x + 2 ) );
    }

  rgb.red(1);
  rgb.green(0);
  rgb.blue(0);
  rgb.alpha(0);
  ima.strokeColor(rgb);
  ima.strokeWidth(2);
  imb.strokeColor(rgb);
  imb.strokeWidth(2);
#ifdef __DEBUG_SIFT__
  Magick::Geometry geo;
  geo = ima.size();
  std::cout << geo.height() << "x" << geo.width() << "\n";
  geo = imb.size();
  std::cout << geo.height() << "x" << geo.width() << "\n";
#endif
  for( std::list< featurepair >::iterator it = fp.begin(); it != fp.end(); it++ )
    {
      int x, y;
      x = it->a->posx();
      y = it->a->posy();
      ima.draw( Magick::DrawableCircle(y, x, y, x + it->a->size()/2 ) );
      x = it->b->posx();
      y = it->b->posy();
      imb.draw( Magick::DrawableCircle(y, x, y, x + it->b->size()/2 ) );
      std::pair<int, int> mypair;
      mypair.first = (it->a->posx() - it->b->posx())/4;
      mypair.second = (it->a->posy() - it->b->posy())/4;
      if( myts.count( mypair ) > 0 )
        myts[mypair] = myts[mypair]+1;
      else
        myts[mypair] = 1;
    }
  ima.write( "ima.png" );
  imb.write( "imb.png" );
  for( std::map< std::pair<int, int>, int >::iterator it = myts.begin();
       it != myts.end(); it++ )
    {
      if( it->second > 1 )
        {
          std::cout << it->first.first*4 << " " << it->first.second*4 << " : " << it->second << "\n";
        }
    }

  return 0;
}

int imagesift::cluster_filter( std::list<featurepair> &fp )
{
  kmeans em;
  double *d;
  d = new double[ fp.size() ];
  int i = 0;
  for( std::list<featurepair>::iterator it = fp.begin();
       it != fp.end(); it++ )
    {
      d[i] = it->score;
      i++;
    }
  em.load( d, (int)(fp.size()) );
  std::cout << "clustering\n";
  em.cluster( 2 );
  double cut;
  cut = (em.center(0)[0] + em.center(0)[1]) / 2.0;
  std::vector< std::list<featurepair>::iterator > todel;
  for( std::list<featurepair>::iterator it = fp.begin();
       it != fp.end(); it++ )
    {
      if( it->score > cut )
        todel.push_back( it );
    }
  std::cout << "set points done\n";
  std::cout << "delete " << todel.size() << "\n";
  std::cout << fp.size() << " to ";
  for( unsigned i = 0; i < todel.size(); i++ )
    {
      fp.erase( todel[i] );
    }
  std::cout << fp.size() << "\n";

  return 0;
}

int imagesift::selfsearch()
{
  double avg;
  avg = 0;
  for( unsigned int i = 0; i < myfeatures.size(); i++)
    {
      for( unsigned int j = 0; j < myfeatures.size(); j++)
        {
          avg = avg + myfeatures[i]->match( *(myfeatures[j]) );
        }
    }
  std::cout << "avg score: "
            << avg/( (double)(myfeatures.size()*myfeatures.size()) ) << "\n";
  return 0;
}

int imagesift::calc_magori( image2d &in, image2d *mag, image2d *ori )
{
  mag->create( in.getheight(), in.getwidth() );
  ori->create( in.getheight(), in.getwidth() );

  for( int i = 0; i < in.getheight(); i++ )
    {
      for( int j = 0; j < in.getwidth(); j++ )
        {
          double dx, dy;
          int i0, i1, j0, j1;
          if( i > 0 )
            i0 = i-1;
          else
            i0 = 0;
          if( j > 0 )
            j0 = j-1;
          else
            j0 = 0;
          if( i == (in.getheight()-1) )
            i1 = i;
          else
            i1 = i+1;
          if( j == (in.getwidth()-1) )
            j1 = j;
          else
            j1 = j+1;
          dx = (in.getpixel( i0, j ) - in.getpixel(i1, j ))/(i1-i0);
          dy = (in.getpixel(i, j0) - in.getpixel(i, j1 ))/(j1-j0);
          if( isnan( dx ) || isnan( dy ) )
            {
              std::cout << "ERROR dx dy is nan\n";
              std::cout << dx << " " << dy << " " << i << " " << j << "\n";
              std::cout << i0 << " " << i1 << " : "  << j0 << " " << j1 << "\n";
            }
          mag->setpixel( i, j, sqrt( pow(dx,2)+pow(dy,2) ) );
          ori->setpixel( i, j, atan2( dx, dy ) );
        }
    }
  return 0;
}

int imagesift::clearimages()
{
  for( unsigned int i = 0; i < scales.size(); i++ )
    {
      delete scales[i]->im;
      delete scales[i];
    }
  scales.clear();
  scalemap.clear();
  for( std::map< double, image2d*>::iterator it = magmap.begin();
       it != magmap.end(); it++ )
    delete it->second;
  for( std::map< double, image2d*>::iterator it = orimap.begin();
       it != orimap.end(); it++ )
    delete it->second;
  magmap.clear();
  orimap.clear();
  return 0;
}

int imagesift::clear()
{
  for( unsigned int i = 0; i < scales.size(); i++ )
    {
      delete scales[i]->im;
      delete scales[i];
    }
  scales.clear();
  scalemap.clear();
  for( std::map< double, image2d*>::iterator it = magmap.begin();
       it != magmap.end(); it++ )
    delete it->second;
  for( std::map< double, image2d*>::iterator it = orimap.begin();
       it != orimap.end(); it++ )
    delete it->second;
  magmap.clear();
  orimap.clear();
  for( unsigned int i = 0; i < myfeatures.size(); i++)
    delete myfeatures[i];
  return 0;
}

