#include "scalespace.h"

scalespace::scalespace()
{
}

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

int scalespace::clear()
{
  ssf.del();
  return 0;
}

int scalespace::dximage( image2d &in, image2d &out)
{
  int h, w;
  h = in.getheight()-1;
  w = in.getwidth();
  out.create(h,w);
  for( int i = 1; i < h; i++ )
    {
      for( int j = 0; j < w; j++ )
        {
          double dx;
          dx = in.getpixel( i-1, j )
            - in.getpixel( i+1, j );
          out.setpixel( i, j, dx );
        }
    }
  return 0;
}

int scalespace::dyimage( image2d &in, image2d &out )
{
  int h, w;
  h = in.getheight();
  w = in.getwidth()-1;
  out.create(h,w);
  for( int i = 0; i < h; i++ )
    {
      for( int j = 1; j < w; j++ )
        {
          double dx;
          dx = in.getpixel( i, j-1 )
            - in.getpixel( i, j+1 );
          out.setpixel( i, j, dx );
        }
    }
  return 0;
}

int scalespace::hessian( image2d &in,  image2d &dxx,
                         image2d &dyy, image2d &dxy,
                         image2d &dyx )
{
  image2d dx, dy;
  dximage( in, dx );
  dyimage( in, dy );
  dximage( dx, dxx );
  dximage( dy, dyx );
  dyimage( dy, dyy );
  dyimage( dx, dxy );
  return 0;
}

int scalespace::getscales( std::vector< scales* > &s )
{
  for( int i = 0; i < 9; i++ )
    {
      s.push_back( new scales );
      s[i]->im = new image2d;
      s[i]->im->load( &(myscales[i]) );
      s[i]->k = pow(1.7,i);
    }
  return 0;
}

int scalespace::getfeatures( std::vector< feature > &f )
{
  for( int i = 0; i < ssf.getheight(); i++ )
    {
      for( int j = 0; j < ssf.getwidth(); j++ )
        {
          double v;
          v = ssf.getpixel(i,j);
          if( v > 0 )
            {
              feature myf;
              myf.x = i;
              myf.y = j;
              myf.scale = pow(1.7, v );
              f.push_back(myf);
            }
        }
    }

  return 0;
}

int scalespace::analyze( image2d &imin, double sig )
{
  sigma = sig;
  std::cout << "calculating gaussians\n";
  Magick::Image mim;
  //  char buffer[256];
  image2d *ims[9];
  double sigmas[9];
  for( int i = 0; i < 9; i++ )
    {
      ims[i] = &(myscales[i]);
      sigmas[i] = sigma * pow(1.7, i);
    }
  imin.gaussian(sigmas, ims, 9);
  for( int i = 0; i < 9; i++ )
    {
      char buffer[128];
      myscales[i].getimage( mim );
      sprintf( buffer, "sspscale%i.png", i );
      mim.write( buffer );
    }
  std::cout << "calculating diffs\n";
  for( int i = 0; i < 8; i++ )
    {
      dog[i].im.load( myscales+i );
      dog[i].im.sub( myscales+(i+1) );
      hessian( dog[i].im, dog[i].dxx, dog[i].dyy,
               dog[i].dxy, dog[i].dyx );
    }
  std::cout << "finding extremas\n";
  ssf.create( dog[0].im.getheight(),
              dog[0].im.getwidth() );

  findextrema();

  return 0;
}

#ifndef _NOMAGICK_
int scalespace::print( Magick::Image &im )
{
  int h, w;
  h = ssf.getheight();
  w = ssf.getwidth();
  im.type(Magick::TrueColorType);
  Magick::ColorRGB rgb(1,0,0);
  rgb.alpha(1);
  im.fillColor(rgb);
  rgb.red(1);
  rgb.green(0);
  rgb.blue(0);
  rgb.alpha(0);
  im.strokeColor(rgb);
  im.strokeWidth(2);
  int d = 0;
  for( int i = 0; i < h; i++ )
    {
      for( int j = 0; j < w; j++ )
        {
          double v;
          v = ssf.getpixel(i, j);
          if( v > 0 )
            {
              double r;
              r = pow(1.7, v)*10;
              if( i+r < h )
                {
                  im.draw( Magick::DrawableCircle(j, i, j, i + r ) );
                  d++;
                }
            }
        }
    }
  std::cout << "drew " << d << " circles\n";
  return 0;
}
#endif

int scalespace::filter()
{
  int h, w;
  h = dog[0].im.getheight();
  w = dog[0].im.getwidth();
  int count;
  double avg;
  avg = 0;
  count = 0;
  double gc;
  gc = 0;
  double max, min;
  max = 0;
  min = 1000;
  for( int i = 0; i < h; i++ )
    {
      for( int j = 0; j < w; j++ )
        {
          if( ssf.getpixel(i, j) > 0 )
            {
              if( fabs( dog[(int)ssf.getpixel(i,j)].dxy.getpixel(i, j) ) < 0.000001 )
                {
                  ssf.setpixel(i, j, 0 );
                  count++;
                }
              else
                {
                  double v;
                  v = fabs( dog[(int)ssf.getpixel(i,j)].dxy.getpixel(i, j) );
                  avg = avg + v;
                  if( max < v )
                    max = v;
                  if( min > v )
                    min = v;
                  gc = gc + 1;
                }
            }
        }
    }
  avg = avg/gc;
  double sd;
  sd = gc = 0;
  for( int i = 0; i < h; i++ )
    {
      for( int j = 0; j < w; j++ )
        {
          if( ssf.getpixel(i, j) > 0 )
            {
              double v;
              v = fabs( dog[(int)ssf.getpixel(i,j)].dxy.getpixel(i, j) );
              sd = sd + pow( avg - v, 2);
              gc = gc + 1;
            }
        }
    }
  sd = sd/gc;
  std::cout << "average " << avg << " sd " << sd << "\n";
  avg = avg - sqrt( sd );
  //  avg = 0.003;
  std::cout << "removed: " << count << " edge points\n";
  std::cout << "removing below " << avg << "\n";
  std::cout << "min, max " << min << " " << max << "\n";
  count = 0;
  for( int i = 0; i < h; i++ )
    {
      for( int j = 0; j < w; j++ )
        {
          if( ssf.getpixel(i, j) > 0 )
            {
              if( fabs(dog[(int)ssf.getpixel(i,j)].dxy.getpixel(i, j) ) < avg )
                {
                  ssf.setpixel(i,j, 0);
                  count++;
                }
              
              else if( checkedge(i, j ) == 1 )
                {
                  ssf.setpixel(i, j, 0 );
                  count++;
                }
              
            }
        }
    }
  std::cout << "removed: " << count 
            << " points\n";
  return 0;
}

int scalespace::checkedge( int x, int y )
{
  double a, b, c, d;
  int scale;
  scale = ssf.getpixel(x, y);

  a = dog[scale].dxx.getpixel( x, y );
  b = dog[scale].dxy.getpixel( x, y );
  c = dog[scale].dyx.getpixel( x, y );
  d = dog[scale].dyy.getpixel( x, y );

  double tr, det;
  tr = a+d;
  det = a*d-b*b;
  double thres;
  thres = (11.0*11.0)/(10.0);

  if( (tr*tr/det) > thres )
    return 1;

  return 0;
}

int scalespace::findextrema()
{
  int w, h;
  h = dog[0].im.getheight();
  w = dog[0].im.getwidth();
  for( int s = 1; s < 7; s++ )
    {
      for( int i = 1; i < h-1; i++ )
        {
          for( int j = 1; j < w-1; j++ )
            {
              int minflag, maxflag;
              minflag = maxflag = 1;
              double v;
              v = dog[s].im.getpixel(i,j);
              for( int ss = s-1; ss < s+2; ss++ )
                {
                  for( int si = i-1; si < i+2; si++ )
                    {
                      for( int sj = j-1; sj < j+2; sj++ )
                        {
                          double cv;
                          cv = dog[ss].im.getpixel(si, sj );
                          if( v < cv )
                            maxflag = 0;
                          if( v > cv )
                            minflag = 0;
                        }
                    }
                }
              if( minflag == 1 || maxflag == 1 )
                {
                  ssf.setpixel( i, j, s );
                }
            }
        }
    }
  filter();
  return 0;
}
