#ifndef POLARCORRELATION_H
#define POLARCORRELATION_H

#include "../fftwext.h"
#include "recognition.h"
#include "imagesource.h"
#include "../micro.h"
#include <iostream>

// TMP
extern "C"
{
    #include <opencv/highgui.h>
}

using namespace std;

namespace seemicro
{

struct PolarCorrelationModule : public RecognitionModule
{
  /**
    * Ergebnisse
    */
  Micro& micro;

  /**
    * Betrag der Korrelationsbildes an der Maximum-Stelle.
    * Wird von correlate() gesetzt.
    */
  float correlationStrength;

  /**
    * Referenz zum Kantensegmentbild
    */
  ImageSource& src;

  IplImage *kernel_img;

  /**
    * für fftw / Zwischenergebnisse
    */
  double *ffin, *kernelin, *ffback;
  fftw_plan plan, plan_kernel, planB;
  fftw_complex *ffout, *kernelout;

  PolarCorrelationModule(Micro& m, ImageSource& As) :
   micro(m), src(As)
  {
    int w = src.img->width;
    int h = src.img->height;

    ffin     = (double*) fftw_malloc(w*h*sizeof(double));
    ffback   = (double*) fftw_malloc(w*h*sizeof(double));
    kernelin = (double*) fftw_malloc(w*h*sizeof(double));
    ffout = (fftw_complex*) fftw_malloc(w*(h/2+1)*sizeof(fftw_complex));
    kernelout = (fftw_complex*) fftw_malloc(w*(h/2+1)*sizeof(fftw_complex));

    kernel_img = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 1);
    char wisdomfile[] = "wisdom.fftw3";
    FILE *f = fopen(wisdomfile, "r");
    if(f && fftw_import_wisdom_from_file(f)==1)
    {
      if(DEBUGLEVEL>=2) cerr << "Loaded wisdom from 'wisdom.fftw3'.\n";
      fclose(f);
    }
    else if(DEBUGLEVEL>=1) cerr << "Could not load wisdom from 'wisdom.fftw3'.\n";

    plan  = fftw_plan_dft_r2c_2d(w, h, (double*)ffin, (fftw_complex*)ffout, 0);
    planB = fftw_plan_dft_c2r_2d(w, h, (fftw_complex*)ffout, (double*)ffback, 0);
    f = fopen(wisdomfile, "w");
    if(f)
    {
      fftw_export_wisdom_to_file(f);
      fclose(f);
    }

    paramChanged = true;
  }

  void makeKernel(void)
  {
    if(!paramChanged) return;
    ENTER(3, endl)
    int w = src.img->width;
    int h = src.img->height;

    //draw
    mypoint m(w/2,h/2);
    MicroAdam& adam= *dynamic_cast<MicroAdam*>(&micro);
    int hab = 7;

    // senkrechte linien
    cvLine(kernel_img, m+mypoint( hab,0), m+mypoint( hab+5,-50), 255, 2);
    cvLine(kernel_img, m+mypoint(-hab,0), m+mypoint(-hab-5,-50), 255, 2);

    // waagerechte linien
    cvLine(kernel_img, m+mypoint( hab,0), m+mypoint( hab+40,0), 255, 2);
    cvLine(kernel_img, m+mypoint(-hab,0), m+mypoint(-hab-40,0), 255, 2);

    // TMP
    cvSaveImage("vorlage-polar.png", kernel_img);

    circular_shift(kernel_img, kernel_img, w/2, h/2);

    IplImage2fftw_real(kernel_img, (double*)kernelin, w, h);
    plan_kernel = fftw_plan_dft_r2c_2d(w, h,
	(double*)kernelin, (fftw_complex*)kernelout, 0);
    fftw_execute(plan_kernel);
    fftw_destroy_plan(plan_kernel);
    paramChanged = false;
  }

  static int above_threshold(double v)
  {
    return v > 10*255;
  }

  void processKeyFrame(void)
  {
#ifdef DEBUG
    if(DEBUGLEVEL>=3) cerr << "->correlate2()\n";
#endif
    if(!src.outputChanged && !paramChanged) return;

    makeKernel();// really?

    int w = src.img->width;
    int h = src.img->height;

    IplImage2fftw_real(src.img, (double*)ffin);

    fftw_execute(plan);

    // do the actual convolution
    fftw_complex_multiply_conjugate((fftw_complex*)ffout, (fftw_complex*)kernelout,
	w, h/2+1);

    fftw_execute(planB);

    IplImage *backI = cvCreateImage(cvSize(src.img->width, src.img->height), IPL_DEPTH_8U, 3);
    doubles2IplImage(1/150.0, backI, (double*)ffback,
	           backI->width, backI->height, above_threshold);
    cvSaveImage("polar-correlation.png", backI);

    maximum max;
    findMaxPoint(w, h, ffback, 1.0, &max);
    if(max.x>=0 && max.y>=0)
    {
      micro.alpha = max.x;
    }
    else
    {
      if(DEBUGLEVEL>=2) cerr << "Correlation: no global max?\n";
    }

    outputChanged = true;
#ifdef DEBUG
    if(DEBUGLEVEL>=3) cerr << "<-correlate2()\n";
#endif
  }

  void processFrame(myrect *ROI)
  {
    processKeyFrame();
  }

  ~PolarCorrelationModule()
  {
    fftw_destroy_plan(plan);
    fftw_destroy_plan(planB);
    cvReleaseImage(&kernel_img);
    fftw_free(ffin); ffin = NULL;
    fftw_free(ffback); ffback = NULL;
    fftw_free(kernelin); kernelin = NULL;
    fftw_free(ffout); ffout = NULL;
    fftw_free(kernelout); kernelout = NULL;
  }

}; // PolarCorrelationModule

} // seemicro

#endif

