#include "fftwext.h"

#include <cmath>

namespace seemicro
{

CvScalar toColorScalar(double v, int (*above_threshold)(double))
{
  if(fabs(v)>255)
  {
    // lightgreen
    if(above_threshold &&
       above_threshold(v)) return cvScalar(0, 255, 0);
    // red
    return cvScalar(0, 0, 255);
  }

  // blue
  if(v<0) return cvScalar(v, 50, 50);

  // gray
  return cvScalar(v, v, v);
}

void IplImage2fftw_real(IplImage *img, double* ff, int ffwidth, int ffheight)
{
  int w = img->width;
  int h = img->height;
  if(ffheight<=0) ffheight = h;
  if(ffwidth <=0) ffwidth  = w;

  if(ffheight<h) h = ffheight;
  if(ffwidth <w) w = ffwidth;

  for(int x=0; x<w; x++)
    for(int y=0; y<h; y++)
      ff[y + x*ffheight] = cvGetReal2D(img, y, x)/double(255);
}

void doubles2IplImage(double scale, IplImage *img, double* ff,
  int ffwidth, int ffheight, int (*above_threshold)(double))
{
  int w = img->width;
  int h = img->height;
  if(ffheight<=0) ffheight = h;
  if(ffwidth <=0) ffwidth  = w;

  if(ffheight<h) h = ffheight;
  if(ffwidth <w) w = ffwidth;

  for(int x=0; x<w; x++)
    for(int y=0; y<h; y++)
      cvSet2D(img, y, x, toColorScalar(ff[y + x*ffheight]*scale*255/w/h,
        above_threshold));
}

void fftw_complex2IplImages(fftw_complex *ff, IplImage *outR, IplImage *outI)
{
  int w = outR->width;
  int h = outR->height;
  for(int x=0; x<w; x++)
    for(int y=0; y<h; y++)
    {
      cvSet2D(outR, y, x, toColorScalar(c_re(ff[y + x*h])));
      cvSet2D(outI, y, x, toColorScalar(c_im(ff[y + x*h])));
    }
}

void fftw_complex2IplImageMagnitude(fftw_complex *ff, IplImage *magnitude)
{
  int w = magnitude->width;
  int h = magnitude->height;
  for(int x=0; x<w; x++)
    for(int y=0; y<h; y++)
    {
      double r = c_re(ff[y + x*h]);
      double i = c_im(ff[y + x*h]);
      cvSet2D(magnitude, y, x, toColorScalar(sqrt(r*r+i*i)));
    }
}

void fftw_complex_multiply(fftw_complex *a, fftw_complex *b, int w, int h)
{
  for(int x=0; x<w; x++)
    for(int y=0; y<h; y++)
    {
      double r1 = c_re(a[y + x*h]);
      double i1 = c_im(a[y + x*h]);
      double r2 = c_re(b[y + x*h]);
      double i2 = c_im(b[y + x*h]);
      c_re(a[y + x*h]) = r1*r2 - i1*i2;
      c_im(a[y + x*h]) = r1*i2 + r2*i1;
    }
}


double try_mark_region(double *ff, int width, int height,
    int (*above_threshold)(double),
    int x, int y, int* x_max, int* y_max, double scale)
{
  int i = x*height + y;
  double v = ff[i]*scale*255/width/height;
  if(!above_threshold(v)) return 0;

  // mark as checked
  ff[i] = 0;

  *x_max = x;
  *y_max = y;

  // search recursively
  int x_max_new, y_max_new;
  double v_new;

  // left (better for caching to check now than after right?)
  if(x>=1)
  {
    v_new = try_mark_region(ff, width, height, above_threshold,
	x-1, y, &x_max_new, &y_max_new, scale);
    if(v_new>v)
    { v=v_new; *x_max = x_max_new; *y_max = y_max_new; }
  }
  // right
  if(x<width-1)
  {
    v_new = try_mark_region(ff, width, height, above_threshold,
	x+1, y, &x_max_new, &y_max_new, scale);
    if(v_new>v)
    { v=v_new; *x_max = x_max_new; *y_max = y_max_new; }
//    if(*x_max > width) std::cerr << "x: " << *x_max << endl;
  }
  // down
  if(y<height-1)
  {
    v_new = try_mark_region(ff, width, height, above_threshold,
	x, y+1, &x_max_new, &y_max_new, scale);
    if(v_new>v)
    { v=v_new; *x_max = x_max_new; *y_max = y_max_new; }
  }
  return v;
}

int find_maxima(struct maximum *maxima, int width, int height, double *ff,
    int (*above_threshold)(double), int max_maxima
    )
{
  int count = 0;

  for(int x=0; x<width; x++)
    for(int y=0; y<height; y++)
    {
      int x_max, y_max;
      double v = try_mark_region(ff, width, height, above_threshold,
	  x, y, &x_max, &y_max, 1/double(width/3));
      if(v>0) // we are above_threshold()
      {
        // in sortierte liste der maxima aufnehmen
        int i=-1;
        if(count>0) for(i=count;i>=0;i--)
          if(maxima[i].v>v) break;// will have to insert at i+1
        if(i<count-1 || count==0) // otherwise throw away
        {
          // shift elements to higher index to make space for insert
          for(int j=count-1;j>i;j--)
            memcpy(&maxima[j+1], &maxima[j], sizeof(maximum));
          // insert
          maxima[i+1].v = v;
          maxima[i+1].x = x_max;
          maxima[i+1].y = y_max;
          //cerr << "add/insert v:" << v << endl;
          if(count<max_maxima) count++;
        }
      }
    }

  return count;
}

void findMaxPoint(int width, int height, double *ff, double min_threshold,
  maximum *max)
{
  max->x = -1;
  max->y = -1;
  double v = min_threshold;

  for(int x=0; x<width; x++)
    for(int y=0; y<height; y++)
      if(ff[y + x*height] > v)
      {
        max->x = x;
	max->y = y;
	v = ff[y + x*height];
      }
  max->v = v;
}

} // namespace seemicro

